#include <string.h>
#include <stdbool.h>
#include "forth.h" // for fh_init
#include "fh_runtime.h"
#include "fh_config.h"
#include "fh_error.h"
#include "fh_print.h"
#include "fh_builtins.h"
#include "fh_stack.h"
#include "fh_mem.h"

#define TOBOOL(a) (a == 0 ? 0 : 0xFFFFFFFF)

#define ENSURE_STATE(__state) do {      \
  if (fh->state != (__state)) {         \
    return FH_ERR_INVALID_STATE;        \
  }                                     \
} while (0)


/**
 * Encode a code point using UTF-8
 *
 * Copied from ESPTERM source
 *
 * @param out - output buffer (min 4 characters), will be 0-terminated if shorten than 4
 * @param utf - code point 0-0x10FFFF
 * @return number of bytes on success, 0 on failure (also produces U+FFFD, which uses 3 bytes)
 */
static int utf8_encode(char *out, uint32_t utf)
{
  if (utf <= 0x7F) {
    // Plain ASCII
    out[0] = (char) utf;
    out[1] = 0;
    return 1;
  } else if (utf <= 0x07FF) {
    // 2-byte unicode
    out[0] = (char) (((utf >> 6) & 0x1F) | 0xC0);
    out[1] = (char) (((utf >> 0) & 0x3F) | 0x80);
    out[2] = 0;
    return 2;
  } else if (utf <= 0xFFFF) {
    // 3-byte unicode
    out[0] = (char) (((utf >> 12) & 0x0F) | 0xE0);
    out[1] = (char) (((utf >> 6) & 0x3F) | 0x80);
    out[2] = (char) (((utf >> 0) & 0x3F) | 0x80);
    out[3] = 0;
    return 3;
  } else if (utf <= 0x10FFFF) {
    // 4-byte unicode
    out[0] = (char) (((utf >> 18) & 0x07) | 0xF0);
    out[1] = (char) (((utf >> 12) & 0x3F) | 0x80);
    out[2] = (char) (((utf >> 6) & 0x3F) | 0x80);
    out[3] = (char) (((utf >> 0) & 0x3F) | 0x80);
//		out[4] = 0;
    return 4;
  } else {
    // error - use replacement character
    out[0] = (char) 0xEF;
    out[1] = (char) 0xBF;
    out[2] = (char) 0xBD;
    out[3] = 0;
    return 0;
  }
}


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

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

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

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

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

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

static enum fh_error w_zero_less(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a = 0;
  TRY(ds_pop(fh, &a));
  TRY(ds_push(fh, TOBOOL(a < 0)));
  return FH_OK;
}

static enum fh_error w_zero_greater(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a = 0;
  TRY(ds_pop(fh, &a));
  TRY(ds_push(fh, TOBOOL(a > 0)));
  return FH_OK;
}

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

static enum fh_error w_zero_not_equals(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a = 0;
  TRY(ds_pop(fh, &a));
  TRY(ds_push(fh, TOBOOL(a != 0)));
  return FH_OK;
}


static enum fh_error w_less(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a = 0, b = 0;
  TRY(ds_pop(fh, &b));
  TRY(ds_pop(fh, &a));
  TRY(ds_push(fh, TOBOOL(a < b)));
  return FH_OK;
}

static enum fh_error w_greater(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a = 0, b = 0;
  TRY(ds_pop(fh, &b));
  TRY(ds_pop(fh, &a));
  TRY(ds_push(fh, TOBOOL(a > b)));
  return FH_OK;
}

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

static enum fh_error w_not_equals(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a = 0, b = 0;
  TRY(ds_pop(fh, &b));
  TRY(ds_pop(fh, &a));
  TRY(ds_push(fh, TOBOOL(a != b)));
  return FH_OK;
}

static enum fh_error wp_add(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  enum fh_error rv;
  uint32_t a = 0;
  TRY(ds_pop(fh, &a));
  TRY(ds_push(fh, a + w->param));
  return FH_OK;
}

static enum fh_error wp_mul(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  enum fh_error rv;
  uint32_t a = 0;
  TRY(ds_pop(fh, &a));
  TRY(ds_push(fh, a * w->param));
  return FH_OK;
}

static enum fh_error wp_div(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  enum fh_error rv;
  uint32_t a = 0;
  TRY(ds_pop(fh, &a));
  TRY(ds_push(fh, a * w->param));
  return FH_OK;
}

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

  if (c == 0) {
    return FH_ERR_DIV_BY_ZERO;
  }

  uint64_t v = ((uint64_t) a * (uint64_t) b) / (uint64_t) c;

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

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

  if (c == 0) {
    return FH_ERR_DIV_BY_ZERO;
  }

  uint64_t product = ((uint64_t) a * (uint64_t) b);
  uint64_t v = product / (uint64_t) c;
  uint64_t m = product % (uint64_t) c;

  TRY(ds_push(fh, (uint32_t) m));
  TRY(ds_push(fh, (uint32_t) v));
  return FH_OK;
}

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

  if (b == 0) {
    return FH_ERR_DIV_BY_ZERO;
  }

  TRY(ds_push(fh, a / b));
  return FH_OK;
}

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

  int32_t sa = (int32_t) a; // TODO is this right?

  if (sa < 0) { sa = -sa; }

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

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

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

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

  if (b == 0) {
    return FH_ERR_DIV_BY_ZERO;
  }

  uint32_t rem = a % b;
  uint32_t div = a / b;

  TRY(ds_push(fh, rem));
  TRY(ds_push(fh, div));
  return FH_OK;
}

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

  fh_setstate(fh, FH_STATE_COMPILE, FH_SUBSTATE_COLON_NAME);

  uint32_t ptr;
  TRY(fh_heap_reserve(fh, DICTWORD_SIZE, &ptr));

  struct fh_word_s *new_word = fh_word_at(fh, ptr);
  new_word->previous = fh->dict_last;
  new_word->param = fh->here;
  new_word->handler = w_user_word;
  fh->dict_last = ptr;

  return FH_OK;
}

static enum fh_error w_postpone(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  ENSURE_STATE(FH_STATE_COMPILE);

  fh_setsubstate(fh, FH_SUBSTATE_POSTPONE_NAME);
  return FH_OK;
}

static enum fh_error w_leftbracket(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  ENSURE_STATE(FH_STATE_COMPILE);
  fh_setstate(fh, FH_STATE_INTERPRET, 0);
  return FH_OK;
}

static enum fh_error w_rightbracket(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  ENSURE_STATE(FH_STATE_INTERPRET);
  fh_setstate(fh, FH_STATE_COMPILE, 0);
  return FH_OK;
}

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

  struct fh_instruction_s instr;
  uint32_t val;
  TRY(ds_pop(fh, &val));
  instr_init(&instr, FH_INSTR_NUMBER, val);
  TRY(fh_heap_put(fh, &instr, INSTR_SIZE));

  return FH_OK;
}

static enum fh_error w_semicolon(struct fh_thread_s *fh, const struct fh_word_s *w0)
{
  (void) w0;
  enum fh_error rv;
  struct fh_instruction_s instr;

  ENSURE_STATE(FH_STATE_COMPILE);

  instr_init(&instr, FH_INSTR_ENDWORD, 0);
  TRY(fh_heap_put(fh, &instr, INSTR_SIZE));

  /* Return to interpret state */
  fh_setstate(fh, FH_STATE_INTERPRET, 0);
  return FH_OK;
}

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

  if (fh->dict_last == 0) {
    LOGE("Dict is empty, cannot modify previous word!");
    return FH_ERR_INVALID_STATE;
  }

  fh_word_at(fh, fh->dict_last)->flags |= WORDFLAG_IMMEDIATE;

  return FH_OK;
}

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

  ENSURE_STATE(FH_STATE_COMPILE);

  instr_init(&instr, FH_INSTR_WORD, fh->dict_last);
  TRY(fh_heap_put(fh, &instr, INSTR_SIZE));

  return FH_OK;
}

static enum fh_error w_dupe(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a = 0;
  TRY(ds_peek(fh, &a));
  TRY(ds_push(fh, a));
  return FH_OK;
}

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

static enum fh_error w_question_dupe(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a = 0;
  TRY(ds_peek(fh, &a));
  if (a) {
    TRY(ds_push(fh, a));
  }
  return FH_OK;
}

static enum fh_error w_two_dup(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a = 0;
  uint32_t b = 0;
  TRY(ds_peek_n(fh, &a, 0));
  TRY(ds_peek_n(fh, &b, 1));
  TRY(ds_push(fh, b));
  TRY(ds_push(fh, a));
  return FH_OK;
}

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

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

static enum fh_error w_swap(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  TRY(ds_roll(fh, 1));
  return FH_OK;
}

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

  uint32_t a, b, c, d;
  TRY(ds_pop(fh, &a));
  TRY(ds_pop(fh, &b));
  TRY(ds_pop(fh, &c));
  TRY(ds_pop(fh, &d));

  TRY(ds_push(fh, b));
  TRY(ds_push(fh, a));
  TRY(ds_push(fh, d));
  TRY(ds_push(fh, c));
  return FH_OK;
}

static enum fh_error w_rot(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  TRY(ds_roll(fh, 2));
  return FH_OK;
}

static enum fh_error w_over(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a = 0;
  TRY(ds_peek_n(fh, &a, 1));
  TRY(ds_push(fh, a));
  return FH_OK;
}

static enum fh_error w_two_over(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a = 0;
  uint32_t b = 0;
  TRY(ds_peek_n(fh, &a, 2));
  TRY(ds_peek_n(fh, &b, 3));
  TRY(ds_push(fh, b));
  TRY(ds_push(fh, a));
  return FH_OK;
}

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

static enum fh_error w_pick(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t nth = 0;
  uint32_t a = 0;
  TRY(ds_pop(fh, &nth));
  TRY(ds_peek_n(fh, &a, nth));
  TRY(ds_push(fh, a));
  return FH_OK;
}

static enum fh_error w_roll(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t n = 0;
  TRY(ds_pop(fh, &n));
  TRY(ds_roll(fh, n));
  return FH_OK;
}

static enum fh_error w_to_r(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a;
  TRY(ds_pop(fh, &a));
  TRY(rs_push(fh, a));
  return FH_OK;
}

static enum fh_error w_two_to_r(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a;
  uint32_t b;
  TRY(ds_pop(fh, &a));
  TRY(ds_pop(fh, &b));
  TRY(rs_push(fh, b));
  TRY(rs_push(fh, a));
  return FH_OK;
}

static enum fh_error w_two_r_from(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a;
  uint32_t b;
  TRY(rs_pop(fh, &a));
  TRY(rs_pop(fh, &b));
  TRY(ds_push(fh, b));
  TRY(ds_push(fh, a));
  return FH_OK;
}

static enum fh_error w_two_r_fetch(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a;
  uint32_t b;
  TRY(rs_peek_n(fh, &a, 0));
  TRY(rs_peek_n(fh, &b, 1));
  TRY(ds_push(fh, b));
  TRY(ds_push(fh, a));
  return FH_OK;
}

static enum fh_error w_r_from(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a;
  TRY(rs_pop(fh, &a));
  TRY(ds_push(fh, a));
  return FH_OK;
}

static enum fh_error w_r_fetch(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a;
  TRY(rs_peek(fh, &a));
  TRY(ds_push(fh, a));
  return FH_OK;
}

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

  FHPRINT("%d ", (int32_t) a);
  return FH_OK;
}

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

  FHPRINT("%.*s", count, fh_str_at(fh, addr));
  return FH_OK;
}

static enum fh_error wp_putc(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) fh;
  FHPRINT("%c", w->param);
  return FH_OK;
}

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

  FHPRINT("DS ");
  for (int i = 0; i < fh->data_stack_top; i++) {
    FHPRINT("%d ", fh->data_stack[i]);
  }

  FHPRINT("\nRS ");
  for (int i = 0; i < fh->return_stack_top; i++) {
    FHPRINT("%d ", fh->return_stack[i]);
  }

  FHPRINT("\n");
  return FH_OK;
}

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

  fh->data_stack_top = 0;
  fh->return_stack_top = 0;
  fh_setstate(fh, FH_STATE_QUIT, 0);

  return FH_OK;
}

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

  fh->return_stack_top = 0;
  fh_setstate(fh, FH_STATE_QUIT, 0);

  return FH_OK;
}

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

  fh_setsubstate(fh, FH_SUBSTATE_EXIT);

  return FH_OK;
}

static enum fh_error w_s_quote(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  fh_setsubstate(fh, FH_SUBSTATE_S_QUOTE);
  return FH_OK;
}

//static enum fh_error w_char(struct fh_thread_s *fh, const struct fh_word_s *w)
//{
//  (void) w;
//  fh_setsubstate(fh, FH_SUBSTATE_CHAR);
//  return FH_OK;
//}

static enum fh_error w_error_word0(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  LOGE("Invocation of word #0 (illegal)");
  fh_setstate(fh, FH_STATE_QUIT, 0);
  return FH_OK;
}

static enum fh_error w_dot_quote(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  fh_setsubstate(fh, FH_SUBSTATE_DOT_QUOTE);
  return FH_OK;
}

static enum fh_error w_backslash(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  fh_setsubstate(fh, FH_SUBSTATE_LINE_COMMENT);
  return FH_OK;
}

static enum fh_error w_paren(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  fh_setsubstate(fh, FH_SUBSTATE_PAREN_COMMENT);
  return FH_OK;
}

static enum fh_error w_bye(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  fh_setstate(fh, FH_STATE_SHUTDOWN, 0);
  return FH_OK;
}

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

  ENSURE_STATE(FH_STATE_COMPILE);

  TRY(cs_push(fh, fh->here));
  instr_init(&instr, FH_INSTR_JUMPZERO, MAGICADDR_UNRESOLVED);
  TRY(fh_heap_put(fh, &instr, INSTR_SIZE));
  return FH_OK;
}

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

  ENSURE_STATE(FH_STATE_COMPILE);

  uint32_t ifaddr = 0;
  TRY(cs_pop(fh, &ifaddr));
  struct fh_instruction_s *if_instr = fh_instr_at(fh, ifaddr);
  if (if_instr->data != MAGICADDR_UNRESOLVED) {
    LOGE("IF-ELSE control stack corruption");
    return FH_ERR_INTERNAL;
  }

  if_instr->data = fh->here + INSTR_SIZE;

  TRY(cs_push(fh, fh->here));
  instr_init(&instr, FH_INSTR_JUMP, MAGICADDR_UNRESOLVED);
  TRY(fh_heap_put(fh, &instr, INSTR_SIZE));
  return FH_OK;
}

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

  ENSURE_STATE(FH_STATE_COMPILE);

  uint32_t ifaddr = 0;
  TRY(cs_pop(fh, &ifaddr));
  struct fh_instruction_s *if_instr = fh_instr_at(fh, ifaddr);
  if (if_instr->data != MAGICADDR_UNRESOLVED) {
    LOGE("IF-ELSE control stack corruption");
    return FH_ERR_INTERNAL;
  }

  if_instr->data = fh->here;
  return FH_OK;
}

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

  ENSURE_STATE(FH_STATE_COMPILE);

  uint32_t destaddr = 0;
  TRY(cs_pop(fh, &destaddr));

  instr_init(&instr, FH_INSTR_JUMPZERO, destaddr);
  TRY(fh_heap_put(fh, &instr, INSTR_SIZE));
  return FH_OK;
}

static enum fh_error w_begin(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  ENSURE_STATE(FH_STATE_COMPILE);
  TRY(cs_push(fh, fh->here)); /* dest */
  return FH_OK;
}

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

  ENSURE_STATE(FH_STATE_COMPILE);

  uint32_t destaddr = 0;
  TRY(cs_pop(fh, &destaddr));

  TRY(cs_push(fh, fh->here)); // orig
  TRY(cs_push(fh, destaddr)); // dest

  instr_init(&instr, FH_INSTR_JUMPZERO, MAGICADDR_UNRESOLVED);
  TRY(fh_heap_put(fh, &instr, INSTR_SIZE));
  return FH_OK;
}

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

  ENSURE_STATE(FH_STATE_COMPILE);

  uint32_t origaddr = 0;
  uint32_t destaddr = 0;
  TRY(cs_pop(fh, &destaddr));
  TRY(cs_pop(fh, &origaddr));

  struct fh_instruction_s *branch_instr = fh_instr_at(fh, origaddr);
  if (branch_instr->data != MAGICADDR_UNRESOLVED) {
    LOGE("REPEAT control stack corruption");
    return FH_ERR_INTERNAL;
  }
  branch_instr->data = fh->here + INSTR_SIZE;

  instr_init(&instr, FH_INSTR_JUMP, destaddr);
  TRY(fh_heap_put(fh, &instr, INSTR_SIZE));

  return FH_OK;
}

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

  ENSURE_STATE(FH_STATE_COMPILE);

  uint32_t destaddr = 0;
  TRY(cs_pop(fh, &destaddr));

  instr_init(&instr, FH_INSTR_JUMP, destaddr);
  TRY(fh_heap_put(fh, &instr, INSTR_SIZE));

  return FH_OK;
}

static enum fh_error wp_setbase(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  fh_setbase(fh, w->param);
  return FH_OK;
}

static enum fh_error w_emit(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t a;
  TRY(ds_pop(fh, &a));

  char buf[5];
  int num = utf8_encode(buf, a);
  FHPRINT("%.*s", num, buf);
  return FH_OK;
}

static enum fh_error w_see(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  enum fh_error rv;
  fh_setsubstate(fh, FH_SUBSTATE_SEE_NAME);
  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 + 84 >= 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 wp_const(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  enum fh_error rv;
  TRY(ds_push(fh, w->param));
  return FH_OK;
}

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

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

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

  ENSURE_STATE(FH_STATE_INTERPRET);

  fh_init(fh);
  return FH_OK;
}

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_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));
  TRY(fh_heap_reserve(fh, count, NULL));
  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_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;
}

/** Add pointers to built-in word handlers to a runtime struct */
enum fh_error register_builtin_words(struct fh_thread_s *fh)
{
  struct name_and_handler {
    const char *name;
    word_exec_t handler;
    bool immediate;
    uint32_t param;
  };

  const struct name_and_handler builtins[] = {
      {"",          w_error_word0,     1, 0},
      /* Weird meta stuff */
      {"immediate", w_immediate,       1, 0},
      {"postpone",  w_postpone,        1, 0},
      {"[",         w_leftbracket,     1, 0},
      {"]",         w_rightbracket,    1, 0},
      {"literal",   w_literal,         1, 0},
      /* Runtime stats */
      {"depth",     w_depth,           0, 0},
      {"unused",    w_unused,          0, 0},
      /* Debug tools & system */
      {"reset",     w_reset,           1, 0},
      {"see",       w_see,             0, 0},
      {"bye",       w_bye,             0, 0},
      /* Strings & Chars */
      {"s\"",       w_s_quote,         1, 0},
      {".\"",       w_dot_quote,       1, 0},
//      {"char",      w_char,            1, 0},
      /* Pointers */
      {"@",         w_fetch,           0, 0},
      {"!",         w_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},
      // TODO +!
      // TODO pictured numbers (#)
      // TODO tick
      // TODO comma
      // TODO >BODY, >IN, >NUMBER
      /* Arithmetics */
      {"decimal",   wp_setbase,        0, 10},
      {"hex",       wp_setbase,        0, 16},
      {"base",      wp_const,          0, MAGICADDR_BASE},
      {"here",      wp_const,          0, MAGICADDR_HERE},
      {"pad",       w_pad,             0, 0},
      {"false",     wp_const,          0, 0},
      {"true",      wp_const,          0, 0xFFFFFFFF},
      {"+",         w_plus,            0, 0},
      {"-",         w_minus,           0, 0},
      {"*",         w_star,            0, 0},
      {"*/",        w_star_slash,      0, 0},
      {"*/mod",     w_star_slash_mod,  0, 0},
      {"or",        w_or,              0, 0},
      {"and",       w_and,             0, 0},
      {"xor",       w_xor,             0, 0},
      {"/",         w_slash,           0, 0},
      {"abs",       w_abs,             0, 0},
      {"/mod",      w_slash_mod,       0, 0},
      {"invert",    w_invert,          0, 0},
      {"negate",    w_negate,          0, 0},
      {"0<",        w_zero_less,       0, 0},
      {"0=",        w_zero_equals,     0, 0},
      {"0<>",       w_zero_not_equals, 0, 0},
      {"0>",        w_zero_greater,    0, 0},
      {"<",         w_less,            0, 0},
      {"=",         w_equals,          0, 0},
      {"<>",        w_not_equals,      0, 0},
      {">",         w_greater,         0, 0},
      {"1+",        wp_add,            0, 1},
      {"char+",     wp_add,            0, 1},
      {"1-",        wp_add,            0, -1},
      {"2+",        wp_add,            0, 2},
      {"2-",        wp_add,            0, -2},
      {"2*",        wp_mul,            0, 2},
      {"chars",     wp_mul,            0, 1},
      {"2/",        wp_div,            0, 2},
      {"cells",     wp_mul,            0, CELL},
      {"cell+",     wp_add,            0, CELL},
      /* Stack manip */
      {"drop",      w_drop,            0, 0},
      {"dup",       w_dupe,            0, 0},
      {"?dup",      w_question_dupe,   0, 0},
      {"nip",       w_nip,             0, 0},
      {"over",      w_over,            0, 0},
      {"swap",      w_swap,            0, 0},
      {"rot",       w_rot,             0, 0},
      {"tuck",      w_tuck,            0, 0},
      {"pick",      w_pick,            0, 0},
      {"roll",      w_roll,            0, 0},
      /* Double wide stack manip */
      {"2drop",     w_two_drop,        0, 0},
      {"2dup",      w_two_dup,         0, 0},
      {"2over",     w_two_over,        0, 0},
      {"2swap",     w_two_swap,        0, 0},
      /* Return stack manip */
      {">r",        w_to_r,            0, 0},
      {"r>",        w_r_from,          0, 0},
      {"r@",        w_r_fetch,         0, 0},
      /* Double wide return stack manip */
      {"2>r",       w_two_to_r,        0, 0},
      {"2r>",       w_two_r_from,      0, 0},
      {"2r@",       w_two_r_fetch,     0, 0},
      /* Printing */
      {".",         w_dot,             0, 0},
      {"type",      w_type,            0, 0},
      {"cr",        wp_putc,           0, '\n'},
      {"space",     wp_putc,           0, ' '},
      {"bl",        wp_const,          0, ' '},
      {"??",        w_debug_dump,      0, 0},
      {"emit",      w_emit,            0, 0},
      /* Control flow */
      {"abort",     w_abort,           0, 0},
      {"quit",      w_quit,            0, 0},
      {"exit",      w_exit,            0, 0},
      {"if",        w_if,              1, 0},
      {"else",      w_else,            1, 0},
      {"then",      w_then,            1, 0},
      {"recurse",   w_recurse,         1, 0},
      {"begin",     w_begin,           1, 0},
      {"while",     w_while,           1, 0},
      {"repeat",    w_repeat,          1, 0},
      {"again",     w_again,           1, 0},
      {"until",     w_until,           1, 0},
      /* Syntax */
      {":",         w_colon,           0, 0},
      {";",         w_semicolon,       1, 0},
      {"\\",        w_backslash,       1, 0}, // line comment
      {"(",         w_paren,           1, 0}, // enclosed comment

      { /* end marker */ }
  };

  LOG("Adding builtin words");

  // foreach
  struct fh_word_s w;
  const struct name_and_handler *p = builtins;
  enum fh_error rv;
  while (p->handler) {
    strcpy(w.name, p->name);
    w.handler = p->handler;
    w.flags = WORDFLAG_BUILTIN | (p->immediate ? WORDFLAG_IMMEDIATE : 0);
    w.param = p->param;
    rv = fh_add_word(&w, fh);
    if (rv != FH_OK) {
      return rv;
    }
    p++;
  }
  return FH_OK;
}