#include "forth_internal.h"

static enum fh_error w_fetch(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t addr = 0;
  TRY(ds_pop(fh, &addr));
  uint32_t val = 0;
  TRY(fh_fetch(fh, addr, &val));
  TRY(ds_push(fh, val));
  return FH_OK;
}

static enum fh_error w_store(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t addr = 0;
  TRY(ds_pop(fh, &addr));
  uint32_t val = 0;
  TRY(ds_pop(fh, &val));

  TRY(fh_store(fh, addr, val));
  return FH_OK;
}

static enum fh_error w_plus_store(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t addr = 0;
  TRY(ds_pop(fh, &addr));
  uint32_t val = 0;
  TRY(fh_fetch(fh, addr, &val));
  uint32_t val2 = 0;
  TRY(ds_pop(fh, &val2));

  TRY(fh_store(fh, addr, val2 + val));
  return FH_OK;
}

static enum fh_error w_two_store(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t addr = 0;
  TRY(ds_pop(fh, &addr));
  uint32_t a = 0, b = 0;
  TRY(ds_pop(fh, &a));
  TRY(ds_pop(fh, &b));

  TRY(fh_store(fh, addr, a));
  TRY(fh_store(fh, addr + CELL, b));
  return FH_OK;
}

static enum fh_error w_two_fetch(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t addr = 0;
  TRY(ds_pop(fh, &addr));
  uint32_t a = 0, b = 0;
  TRY(fh_fetch(fh, addr, &a));
  TRY(fh_fetch(fh, addr + CELL, &b));
  TRY(ds_push(fh, b));
  TRY(ds_push(fh, a));
  return FH_OK;
}

static enum fh_error w_aligned(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t addr = 0;
  TRY(ds_pop(fh, &addr));
  TRY(ds_push(fh, WORDALIGNED(addr)));
  return FH_OK;
}

static enum fh_error w_allot(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t count = 0;
  TRY(ds_pop(fh, &count));
  int32_t ci = (int32_t) count;

  if (ci > 0) {
    TRY(fh_heap_reserve(fh, ci, NULL));
  } else {
    LOG("Deallot %d", count);
    fh->here = WORDALIGNED((uint32_t) (int32_t) fh->here + count);
  }
  return FH_OK;
}

static enum fh_error w_move(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t count = 0, dst = 0, src = 0;
  TRY(ds_pop(fh, &count));
  TRY(ds_pop(fh, &dst));
  TRY(ds_pop(fh, &src));
  
  if (src+count>=HEAP_SIZE) {
    LOGE("MOVE src out of bounds");
    return FH_ERR_ILLEGAL_FETCH;
  }
  
  if (dst+count>=HEAP_SIZE) {
    LOGE("MOVE dst out of bounds");
    return FH_ERR_ILLEGAL_STORE;
  }
  
  fh_heap_copy(fh, dst, src, count);
  
  return FH_OK;
}

static enum fh_error w_comma(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;

  if (fh->here & 3) {
    LOGE("HERE not aligned before 'comma'");
    return FH_ERR_ILLEGAL_STORE;
  }

  uint32_t value = 0;
  TRY(ds_pop(fh, &value));
  TRY(fh_heap_put(fh, &value, CELL));
  return FH_OK;
}

static enum fh_error w_c_comma(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;

  uint32_t value = 0;
  TRY(ds_pop(fh, &value));
  TRY(fh_heap_put(fh, &value, 1));
  return FH_OK;
}

static enum fh_error w_c_store(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;

  uint32_t addr;
  TRY(ds_pop(fh, &addr));

  uint32_t value = 0;
  TRY(ds_pop(fh, &value));
  TRY(fh_store_char(fh, addr, (char) value));
  return FH_OK;
}

static enum fh_error w_c_fetch(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;

  uint32_t addr;
  TRY(ds_pop(fh, &addr));

  char value = 0;
  TRY(fh_fetch_char(fh, addr, &value));

  TRY(ds_push(fh, (uint32_t) value));
  return FH_OK;
}

static enum fh_error w_align(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  fh->here = WORDALIGNED(fh->here);
  return FH_OK;
}

static enum fh_error w_pad(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t addr = fh->here + PAD_OFFSET;
  if (addr + MIN_PAD_SIZE >= HEAP_END) {
    LOGE("Heap overflow, PAD is too small!");
    return FH_ERR_HEAP_FULL;
  }

  TRY(ds_push(fh, addr));
  return FH_OK;
}

static enum fh_error w_here(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  TRY(ds_push(fh, fh->here));
  return FH_OK;
}

const struct name_and_handler fh_builtins_mem[] = {
    {"chars",   wp_mul,       0, 1},
    {"char+",   wp_add,       0, 1},
    {"cells",   wp_mul,       0, CELL},
    {"cell+",   wp_add,       0, CELL},
    {"@",       w_fetch,      0, 0},
    {"!",       w_store,      0, 0},
    {"+!",      w_plus_store, 0, 0},
    {"2!",      w_two_store,  0, 0},
    {"2@",      w_two_fetch,  0, 0},
    {"aligned", w_aligned,    0, 0},
    {"allot",   w_allot,      0, 0},
    {"align",   w_align,      0, 0},
    {",",       w_comma,      0, 0},
    {"c,",      w_c_comma,    0, 0},
    {"c@",      w_c_fetch,    0, 0},
    {"c!",      w_c_store,    0, 0},
    {"here",    w_here,       0, 0},
    {"state",   wp_const,     0, MAGICADDR_STATE},
    {"pad",     w_pad,        0, 0},
    {"move",    w_move,       0, 0},

    { /* end marker */ }
};