#include "forth_internal.h"

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;
}

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_source_id(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  TRY(ds_push(fh, fh->input->source_id));
  return FH_OK;
}

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_lshift(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_rshift(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_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((int32_t) 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((int32_t) 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_min(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, (int32_t) a < (int32_t) b ? a : b));
  return FH_OK;
}

static enum fh_error w_max(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, (int32_t) a < (int32_t) b ? b : a));
  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((int32_t) a < (int32_t) 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((int32_t) a > (int32_t) b)));
  return FH_OK;
}

static enum fh_error w_within(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t test, low, high;
  TRY(ds_pop(fh, &high));
  TRY(ds_pop(fh, &low));
  TRY(ds_pop(fh, &test));

  // This should work, according to the spec:
  //  : WITHIN ( test low high -- flag ) OVER - >R - R> U< ;

  // t l h | OVER
  // t l h l | -
  // t l (h-l) | >r
  // t l | R:(h-l) | -
  // (t-l) | R:(h-l) | R>
  // (t-l) (h-l) | U<
  // =within

  const int32_t ti = (int32_t) test;
  const int32_t li = (int32_t) low;
  const int32_t hi = (int32_t) high;

  const bool within = (uint32_t) (ti - li) < (uint32_t) (hi - li);

  TRY(ds_push(fh, TOBOOL(within)));
  return FH_OK;
}

static enum fh_error w_u_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_u_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;
}

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;
}

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 w_2div(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 & 0x80000000) | (a >> 1)));
  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_ARITH;
  }

  int64_t v = ((int64_t) (int32_t) a * (int64_t) (int32_t) b) / (int64_t) (int32_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_ARITH;
  }

  const int64_t product = ((int64_t) (int32_t) a * (int64_t) (int32_t) b);
  const int64_t v = product / (int64_t) (int32_t) c;
  const int64_t m = product % (int64_t) (int32_t) c;

  TRY(ds_push(fh, (uint32_t) (int32_t) m));
  TRY(ds_push(fh, (uint32_t) (int32_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_ARITH;
  }

  TRY(ds_push(fh, (int32_t) a / (int32_t) 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;

  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_ARITH;
  }

  const int32_t rem = (int32_t) a % (int32_t) b;
  const int32_t div = (int32_t) a / (int32_t) b;

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

static enum fh_error w_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_ARITH;
  }

  const int32_t rem = (int32_t) a % (int32_t) b;

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

static enum fh_error w_s_to_d(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));

  const int32_t as = (int32_t) a; // because of sign extend
  const int64_t a64 = as;

  TRY(ds_push_dw(fh, (uint64_t) a64));
  return FH_OK;
}

static enum fh_error w_m_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));

  // make signed and then sign extend
  const int64_t res = (int64_t) (int32_t) a * (int64_t) (int32_t) b;

  TRY(ds_push_dw(fh, (uint64_t) res));
  return FH_OK;
}

static enum fh_error w_um_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));

  // make signed and then sign extend
  const uint64_t res = (uint64_t) a * (uint64_t) b;

  TRY(ds_push_dw(fh, res));
  return FH_OK;
}

// Copied from https://stackoverflow.com/a/51457071/2180189
void floor_div64(int64_t *q, int64_t *r, int64_t a, int64_t b)
{
  const int64_t q0 = a / b;
  const int64_t r0 = a % b;
  if (b > 0) {
    *q = r0 >= 0 ? q0 : q0 - 1;
    *r = r0 >= 0 ? r0 : r0 + b;
  } else {
    *q = r0 <= 0 ? q0 : q0 - 1;
    *r = r0 <= 0 ? r0 : r0 + b;
  }
}

static enum fh_error w_fm_mod(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  int32_t div;
  int64_t num;
  TRY(ds_pop(fh, (uint32_t *) &div));
  TRY(ds_pop_dw(fh, (uint64_t *) &num));

  int64_t res, rem;

  floor_div64(&res, &rem, num, div);

  if ((int64_t) (int32_t) rem != rem) {
    LOGE("Remainder too large");
    return FH_ERR_ARITH;
  }
  if ((int64_t) (int32_t) res != res) {
    LOGE("Division result too large");
    return FH_ERR_ARITH;
  }

  TRY(ds_push(fh, (int32_t) rem));
  TRY(ds_push(fh, (int32_t) res));
  return FH_OK;
}

static enum fh_error w_um_mod(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  uint32_t div;
  uint64_t num;
  TRY(ds_pop(fh, &div));
  TRY(ds_pop_dw(fh, &num));

  const uint64_t res = num / (uint64_t) div;
  const uint64_t rem = num % (uint64_t) div;

  if ((uint64_t) (uint32_t) rem != rem) {
    LOGE("Remainder too large");
    return FH_ERR_ARITH;
  }
  if ((uint64_t) (uint32_t) res != res) {
    LOGE("Division result too large");
    return FH_ERR_ARITH;
  }

  TRY(ds_push(fh, (uint32_t) rem));
  TRY(ds_push(fh, (uint32_t) res));
  return FH_OK;
}

static enum fh_error w_sm_rem(struct fh_thread_s *fh, const struct fh_word_s *w)
{
  (void) w;
  enum fh_error rv;
  int32_t div;
  int64_t num;
  TRY(ds_pop(fh, (uint32_t *) &div));
  TRY(ds_pop_dw(fh, (uint64_t *) &num));

  const int64_t res = num / (int64_t) div;
  const int64_t rem = num % (int64_t) div;

  if ((int64_t) (int32_t) rem != rem) {
    LOGE("Remainder too large");
    return FH_ERR_ARITH;
  }
  if ((int64_t) (int32_t) res != res) {
    LOGE("Division result too large");
    return FH_ERR_ARITH;
  }

  TRY(ds_push(fh, (int32_t) rem));
  TRY(ds_push(fh, (int32_t) res));
  return FH_OK;
}

const struct name_and_handler fh_builtins_arith[] = {
    /* Arithmetics */
    {"base",    wp_const,          0, MAGICADDR_BASE},
    {"source-id", w_source_id,          0, 0},
    {"decimal", wp_setbase,        0, 10},
    {"hex",     wp_setbase,        0, 16},
    {"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},
    {"mod",     w_mod,             0, 0},
    {"invert",  w_invert,          0, 0},
    {"negate",  w_negate,          0, 0},
    {"lshift",  w_lshift,          0, 0},
    {"rshift",  w_rshift,          0, 0},
    {"min",     w_min,             0, 0},
    {"max",     w_max,             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},
    {"u<",      w_u_less,          0, 0},
    {"=",       w_equals,          0, 0},
    {"<>",      w_not_equals,      0, 0},
    {">",       w_greater,         0, 0},
    {"u>",      w_u_greater,       0, 0},
    {"1+",      wp_add,            0, 1},
    {"1-",      wp_add,            0, -1},
    {"2+",      wp_add,            0, 2},
    {"2-",      wp_add,            0, -2},
    {"2*",      wp_mul,            0, 2},
    {"2/",      w_2div,            0, 0},
    {"s>d",     w_s_to_d,          0, 0},
    {"m*",      w_m_star,          0, 0},
    {"um*",     w_um_star,         0, 0},
    {"fm/mod",  w_fm_mod,          0, 0},
    {"sm/rem",  w_sm_rem,          0, 0},
    {"um/mod",  w_um_mod,          0, 0},
    {"within",  w_within,          0, 0},
    { /* end marker */ }
};