#include <string.h>

#include "fh_print.h"
#include "fh_error.h"
#include "fh_runtime.h"
#include "fh_mem.h"

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("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;
      break;

    case MAGICADDR_HERE:
      *dst = fh->here;
      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];
  } 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;

    default:
      if (addr & 3) {
        LOGE("Address 0x%08x is not aligned!", addr);
        return FH_ERR_ILLEGAL_STORE;
      }
      if (addr <= HEAP_SIZE - 4) {
        *((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) {
    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_SIZE) {
    return FH_ERR_HEAP_FULL;
  }

  if (addr) {
    *addr = p;
  }

  // Erase the region. This is out of abundance of caution, not really needed if it was erased initially. Maybe.
  memset(&fh->heap[p], 0, 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)
{
  memcpy(&fh->heap[addr], src, len);
}

/** 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));
  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);
}


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

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

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