#include "forth_internal.h"

// Important distinction: HEAP_END is the end of the normally addressable region. HEAP_SIZE is the full memory area.
//  Buffers are placed at the end of the heap!

void fh_align(struct fh_thread_s *fh)
{
  fh->here = WORDALIGNED(fh->here);
}

void fh_setbase(struct fh_thread_s *fh, uint32_t base)
{
  LOG("set BASE = %d", base);
  fh->base = base;
}

enum fh_error fh_fetch(struct fh_thread_s *fh, uint32_t addr, uint32_t *dst)
{
  switch (addr) {
    case MAGICADDR_BASE:
      *dst = fh->base;
      LOG("Fetch base %d", *dst);
      break;

    case MAGICADDR_HERE:
      *dst = fh->here;
      LOG("Fetch here %d", *dst);
      break;

    case MAGICADDR_SOURCEID:
      *dst = fh->input->source_id;
      LOG("Fetch SOURCE-ID %d", *dst);
      break;

    case MAGICADDR_STATE:
      *dst = TOBOOL(fh->state == FH_STATE_COMPILE);
      LOG("Fetch state %d", *dst);
      break;

    case MAGICADDR_INPTR:
      *dst = fh->inputptr;
      LOG("Fetch >IN %d", *dst);
      break;

    default:
      if (addr & 3) {
        LOGE("Address 0x%08x is not aligned!", addr);
        return FH_ERR_ILLEGAL_FETCH;
      }
      if (addr < HEAP_SIZE - 4) {
        *dst = *((uint32_t *) &fh->heap[addr]);
      } else {
        LOGE("Address 0x%08x too high!", addr);
        return FH_ERR_ILLEGAL_FETCH;
      }
  }

  return FH_OK;
}

enum fh_error fh_fetch_char(struct fh_thread_s *fh, uint32_t addr, char *dst)
{
  if (addr < HEAP_SIZE - 4) {
    *dst = (char) fh->heap[addr];
    LOG("Fetch 0x%08x char %d", addr, *dst);
  } else {
    LOGE("Address 0x%08x too high!", addr);
    return FH_ERR_ILLEGAL_FETCH;
  }

  return FH_OK;
}

enum fh_error fh_store(struct fh_thread_s *fh, uint32_t addr, uint32_t val)
{
  switch (addr) {
    case MAGICADDR_BASE:
      fh_setbase(fh, val);
      break;

    case MAGICADDR_HERE:
      LOGE("HERE is read-only!");
      return FH_ERR_ILLEGAL_STORE;

    case MAGICADDR_STATE:
      LOGE("STATE is read-only!");
      return FH_ERR_ILLEGAL_STORE;

    case MAGICADDR_SOURCEID:
      LOGE("SOURCE-ID is read-only!");
      return FH_ERR_ILLEGAL_STORE;

    case MAGICADDR_INPTR:
      LOG("set >IN %d", val);
      fh->inputptr = val;
      break;

    default:
      if (addr & 3) {
        LOGE("Address 0x%08x is not aligned!", addr);
        return FH_ERR_ILLEGAL_STORE;
      }
      if (addr <= HEAP_SIZE - 4) {
        LOG("Store 0x%08x int %d", addr, (int) val);
        *((uint32_t *) &fh->heap[addr]) = val;
      } else {
        LOGE("Address 0x%08x too high!", addr);
        return FH_ERR_ILLEGAL_STORE;
      }
  }

  return FH_OK;
}

enum fh_error fh_store_char(struct fh_thread_s *fh, uint32_t addr, char val)
{
  if (addr < HEAP_SIZE) {
    LOG("Store 0x%08x char %d", addr, val);
    fh->heap[addr] = val;
  } else {
    LOGE("Address 0x%08x too high!", addr);
    return FH_ERR_ILLEGAL_FETCH;
  }

  return FH_OK;
}

/** Allocate a heap region, e.g. for a string. The address is stored to `addr` */
enum fh_error fh_heap_reserve(
    struct fh_thread_s *fh,
    size_t len,
    uint32_t *addr
)
{
  uint32_t p = fh->here;

  if (p + len > HEAP_END) {
    return FH_ERR_HEAP_FULL;
  }

  if (addr) {
    *addr = p;
  }

  fh->here = p + len;
  //fh->here = WORDALIGNED(p + len);

  return FH_OK;
}

/** Write bytes to heap at a given location. The region must have been previously allocated! */
void fh_heap_write(struct fh_thread_s *fh, uint32_t addr, const void *src, uint32_t len)
{
  if (addr > HEAP_SIZE) {
    LOGE("Attempted write past end of heap");
    return;
  }

  memcpy(&fh->heap[addr], src, len);
}

enum fh_error fh_put_instr(struct fh_thread_s *fh, enum fh_instruction_kind kind, uint32_t data)
{
  fh_align(fh);

  struct fh_instruction_s instr = {
      .kind = kind,
      .data = data,
  };
  LOG("\x1b[90mAppend instr %s, data 0x%08x at 0x%08x\x1b[m", instr_name(kind), data, (uint32_t) fh->here);
  return fh_heap_put(fh, &instr, INSTR_SIZE);
}

/** Allocate heap region and write bytes to it */
enum fh_error fh_heap_put(struct fh_thread_s *fh, const void *src, uint32_t len)
{
  enum fh_error rv;
  uint32_t addr = 0;
  TRY(fh_heap_reserve(fh, len, &addr));
  LOG("Put %d bytes at 0x%08x", len, addr);
  fh_heap_write(fh, addr, src, len);
  return FH_OK;
}

/** Copy bytes from compile area to heap. The region must have been previously allocated! */
void fh_heap_copy(struct fh_thread_s *fh, uint32_t addr, uint32_t srcaddr, uint32_t len)
{
  memcpy(&fh->heap[addr], &fh->heap[srcaddr], len);
}

/** Copy bytes from anywhere to heap. The region must have been previously allocated! */
void fh_heap_copyptr(struct fh_thread_s *fh, uint32_t addr, char *source, uint32_t len)
{
  memcpy(&fh->heap[addr], source, len);
}


char *fh_str_at(struct fh_thread_s *fh, uint32_t addr)
{
  if (addr >= HEAP_SIZE) {
    LOGE("fh_str_at out of bounds! 0x%08x", addr);
    return NULL;
  }
  return (char *) &fh->heap[addr];
}

struct fh_instruction_s *fh_instr_at(struct fh_thread_s *fh, uint32_t addr)
{
  if (addr >= HEAP_END) {
    LOGE("fh_instr_at out of bounds! 0x%08x", addr);
    return NULL;
  }
  return (void *) &fh->heap[addr];
}

struct fh_word_s *fh_word_at(struct fh_thread_s *fh, uint32_t addr)
{
  if (addr >= HEAP_END) {
    LOGE("fh_word_at out of bounds! 0x%08x", addr);
    return NULL;
  }
  return (struct fh_word_s *) &fh->heap[addr];
}