#include "forth_internal.h"

// TODO stacks should grow down, not up!

enum fh_error ds_roll(struct fh_thread_s *fh, int n)
{
  if (fh->data_stack_top <= n) {
    LOGE("DS roll UNDERFLOW");
    return FH_ERR_DS_UNDERFLOW;
  }

  uint32_t yn = fh->data_stack_top - 1 - n;
  uint32_t yoinked = fh->data_stack[yn];
  for (uint32_t i = yn; i < fh->data_stack_top; i++) {
    fh->data_stack[i] = fh->data_stack[i + 1];
  }
  fh->data_stack[fh->data_stack_top - 1] = yoinked;
  return FH_OK;
}

/** Peek top of data stack */
enum fh_error ds_peek_n(struct fh_thread_s *fh, uint32_t *out, int n)
{
  if (fh->data_stack_top <= n) {
    LOGE("DS peek_n UNDERFLOW");
    return FH_ERR_DS_UNDERFLOW;
  }
  *out = fh->data_stack[fh->data_stack_top - 1 - n];
  return FH_OK;
}

/** Peek top of return stack */
enum fh_error rs_peek_n(struct fh_thread_s *fh, uint32_t *out, int n)
{
  if (fh->return_stack_top <= n) {
    LOGE("RS peek_n UNDERFLOW");
    return FH_ERR_RS_UNDERFLOW;
  }
  *out = fh->return_stack[fh->return_stack_top - 1 - n];
  return FH_OK;
}

/** Replace value on return stack */
enum fh_error rs_poke_n(struct fh_thread_s *fh, uint32_t value, int n)
{
  if (fh->return_stack_top <= n) {
    LOGE("RS peek_n UNDERFLOW");
    return FH_ERR_RS_UNDERFLOW;
  }
  fh->return_stack[fh->return_stack_top - 1 - n] = value;
  return FH_OK;
}

/** Pop from data stack */
enum fh_error ds_pop(struct fh_thread_s *fh, uint32_t *out)
{
  if (fh->data_stack_top == 0) {
    LOGE("DS pop UNDERFLOW");
    return FH_ERR_DS_UNDERFLOW;
  }
  *out = fh->data_stack[--fh->data_stack_top];
  LOG("DS pop %d (0x%08x)", *out, *out);
  return FH_OK;
}

/** Pop from return stack */
enum fh_error rs_pop(struct fh_thread_s *fh, uint32_t *out)
{
  if (fh->return_stack_top == 0) {
    LOGE("RS pop UNDERFLOW");
    return FH_ERR_RS_UNDERFLOW;
  }
  *out = fh->return_stack[--fh->return_stack_top];
  LOG("RS pop %d (0x%08x)", *out, *out);
  return FH_OK;
}

#define UPDATE_HWM(hwm, top) \
  do {                       \
    if((hwm) < (top)) {      \
      (hwm) = (top);         \
    }                        \
  } while(0)

enum fh_error ds_push_dw(struct fh_thread_s *fh, uint64_t in)
{
  enum fh_error rv;
  TRY(ds_push(fh, in & 0xFFFFFFFFULL));
  TRY(ds_push(fh, (in & 0xFFFFFFFF00000000ULL) >> 32));
  return FH_OK;
}

enum fh_error ds_pop_dw(struct fh_thread_s *fh, uint64_t *out)
{
  enum fh_error rv;
  uint32_t a, b;
  TRY(ds_pop(fh, &a));
  TRY(ds_pop(fh, &b));
  *out = ((uint64_t) a << 32) | ((uint64_t) b);
  return FH_OK;
}

/** Push to data stack */
enum fh_error ds_push(struct fh_thread_s *fh, uint32_t in)
{
  LOG("DS push %d (0x%08x)", in, in);
  if (fh->data_stack_top == DATA_STACK_DEPTH) {
    return FH_ERR_DS_OVERFLOW;
  }
  fh->data_stack[fh->data_stack_top++] = in;
  UPDATE_HWM(fh->data_stack_hwm, fh->data_stack_top);
  return FH_OK;
}

/** Push to return stack */
enum fh_error rs_push(struct fh_thread_s *fh, uint32_t in)
{
  LOG("RS push %d (0x%08x)", in, in);
  if (fh->return_stack_top == RETURN_STACK_DEPTH) {
    return FH_ERR_RS_OVERFLOW;
  }
  fh->return_stack[fh->return_stack_top++] = in;
  UPDATE_HWM(fh->return_stack_hwm, fh->return_stack_top);
  return FH_OK;
}