include "inc/cmdsys.plh"
include "inc/int32.plh"
include "inc/sane.plh"
include "inc/longjmp.plh"

import sexpr
  const TYPE_MASK  = $70
  const NIL        = $00
  const BOOL_FALSE = $00
  const BOOL_TRUE  = $01
  const CONS_TYPE  = $10
  const SYM_TYPE   = $20
  const SYM_LEN    = $0F
  const NUM_TYPE   = $30
  const NUM_INT    = $31
  const NUM_FLOAT  = $32
  const ARRAY_TYPE = $40
  const MARK_BIT   = $80
  const MARK_MASK  = $7F
  const NULL_HACK  = 1  // Hack so we can set APVALs to NULL

  struc t_elem
    word link
    byte type
  end
  struc t_cons
    res[t_elem]
    word car
    word cdr
  end
  struc t_sym
    res[t_elem]
    word natv
    word lambda
    word array
    word apval
    char name[0]
  end
  struc t_numint
    res[t_elem]
    word intval[2]
  end
  struc t_numfloat
    res[t_elem]
    res floatval[10]
  end

  const ERR_INTERNAL     = -1
  const ERR_OUT_OF_MEM   = -2
  const ERR_SWEEP_OVRFLW = -3
  const ERR_SWEEP_UNDFLW = -4
  const ERR_BAD_DOT      = -5
  const ERR_NOT_SYM      = -6
  const ERR_NOT_ASSOC    = -7
  const ERR_NOT_FUN      = -8
  const ERR_NOT_NUM      = -9
  const ERR_NOT_INT      = -10
  const ERR_NOT_FLOAT    = -11
  const ERR_NOT_STR      = -12
  const ERR_BAD_INDEX    = -13
  const ERR_BAD_GO       = -14

  var exception
  var err_expr

  predef new_sym(symstr)#1
  predef new_int(intlo, inthi)#1
  predef new_float(extptr)#1
  predef print_expr(expr)#0
  predef eval_expr(expr)#1
  predef bool_pred(bool)#1
end

res[t_numint] nan = 0, 0, NUM_INT, 0, 0, 0, 128 // NaN

//
// Useful constants
//

res[t_extended] ext_pi = $35,$C2,$68,$21,$A2,$DA,$0F,$C9,$00,$40
res[t_extended] ext_e  = $9B,$4A,$BB,$A2,$5B,$54,$F8,$AD,$00,$40

res[t_extended] tempext

def int32_ext(intptr)
  word[4] int64

  int64[0] = intptr=>[0]
  int64[1] = intptr=>[1]
  if int64[1] < 0
    int64[2] = -1
    int64[3] = -1
  else
    int64[2] = 0
    int64[3] = 0
  fin
  sane:saveZP()
  sane:restoreZP(sane:op2FP(FFCOMP|FOZ2X, @tempext, @int64))
  return @tempext
end

def num_ext(numptr)

  if numptr->type == NUM_FLOAT
    return numptr + floatval
  fin
  return int32_ext(numptr + intval)
end

def eval_num(expr)
  var result

  result = eval_expr(expr=>car)
  if !result or result->type & TYPE_MASK <> NUM_TYPE
    err_expr = expr
    throw(exception, ERR_NOT_NUM)
  fin
  return result
end

def eval_ext(expr)
  var result

  result = eval_num(expr)
  if result->type == NUM_INT
    return int32_ext(result + intval)
  fin
  return result + floatval
end

export def eval_int(expr)#1 // Always return an int
  var result
  word[4] int64

  result = eval_num(expr)
  if result->type == NUM_FLOAT
    sane:saveZP()
    sane:restoreZP(sane:op2FP(FFCOMP|FOX2Z, @int64, result + floatval))
    return new_int(int64[0], int64[1])
  fin
  return result
end

export def eval_int16(expr)#1 // Always return an int
  var result
  word[4] int64

  result = eval_num(expr)
  if result->type == NUM_FLOAT
    sane:saveZP()
    sane:restoreZP(sane:op2FP(FFCOMP|FOX2Z, @int64, result + floatval))
    return int64
  fin
  return result=>intval
end

def natv_integer(symptr, expr)
  return eval_int(expr)
end

def natv_sum(symptr, expr)
  var num, extptr
  word[2]         intsum
  res[t_extended] extsum

  intsum[0] = 0
  intsum[1] = 0
  num       = eval_num(expr)
  expr      = expr=>cdr
  if num->type == NUM_INT
    //
    // Sum as integers unless a float is encountered
    //
    intsum[0] = num=>intval[0]
    intsum[1] = num=>intval[1]
    while expr
      num  = eval_num(expr)
      expr = expr=>cdr
      if num->type == NUM_FLOAT
        break
      fin
      load32(@intsum)
      add32(num + intval)
      store32(@intsum)
    loop
  fin
  if num->type == NUM_FLOAT
    //
    // Sum as floating point numbers
    //
    int32_ext(@intsum)
    memcpy(@extsum, num + floatval, t_extended)
    sane:saveZP()
    sane:restoreZP(sane:op2FP(FOADD, @extsum, @tempext))
    while expr
      extptr = eval_ext(expr)
      sane:saveZP()
      sane:restoreZP(sane:op2FP(FOADD, @extsum, extptr))
      expr = expr=>cdr
    loop
    return new_float(@extsum)
  fin
  return new_int(intsum[0], intsum[1])
end

def natv_sub(symptr, expr)
  res[t_numfloat] num1, num2
  word[2] dif
  res[t_extended] ext1, ext2

  memcpy(@num1, eval_num(expr),      t_numfloat)
  memcpy(@num2, eval_num(expr=>cdr), t_numfloat)
  if num1.type == NUM_INT and num2.type == NUM_INT
    load32(@num1 + intval)
    sub32(@num2 + intval)
    store32(@dif)
    return new_int(dif[0], dif[1])
  fin
  memcpy(@ext1, num_ext(@num1), t_extended)
  memcpy(@ext2, num_ext(@num2), t_extended)
  sane:saveZP()
  sane:restoreZP(sane:op2FP(FOSUB, @ext1, @ext2))
  return new_float(@ext1)
end

def natv_mul(symptr, expr)
  var num, extptr
  word[2]         intmul
  res[t_extended] extmul

  intmul[0] = 1
  intmul[1] = 0
  num       = eval_num(expr)
  expr      = expr=>cdr
  if num->type == NUM_INT
    //
    // Multiply as integers unless a float is encountered
    //
    intmul[0] = num=>intval[0]
    intmul[1] = num=>intval[1]
    while expr
      num  = eval_num(expr)
      expr = expr=>cdr
      if num->type == NUM_FLOAT
        break
      fin
      load32(@intmul)
      mul32(num + intval)
      store32(@intmul)
    loop
  fin
  if num->type == NUM_FLOAT
    //
    // Multiply as floating point numbers
    //
    int32_ext(@intmul)
    memcpy(@extmul, num + floatval, t_extended)
    sane:saveZP()
    sane:restoreZP(sane:op2FP(FOMUL, @extmul, @tempext))
    while expr
      extptr = eval_ext(expr)
      sane:saveZP()
      sane:restoreZP(sane:op2FP(FOMUL, @extmul, extptr))
      expr = expr=>cdr
    loop
    return new_float(@extmul)
  fin
  return new_int(intmul[0], intmul[1])
end

def natv_div(symptr, expr)
  res[t_numfloat] num1, num2
  word[2] div
  res[t_extended] ext1, ext2

  memcpy(@num1, eval_num(expr),      t_numfloat)
  memcpy(@num2, eval_num(expr=>cdr), t_numfloat)
  if num1.type == NUM_INT and num2.type == NUM_INT
    load32(@num1 + intval)
    div32(@num2 + intval)
    store32(@div)
    return new_int(div[0], div[1])
  fin
  memcpy(@ext1, num_ext(@num1), t_extended)
  memcpy(@ext2, num_ext(@num2), t_extended)
  sane:saveZP()
  sane:restoreZP(sane:op2FP(FODIV, @ext1, @ext2))
  return new_float(@ext1)
end

def natv_rem(symptr, expr)
  res[t_numfloat] num1, num2
  word[2] rem
  res[t_extended] ext1, ext2

  memcpy(@num1, eval_num(expr),      t_numfloat)
  memcpy(@num2, eval_num(expr=>cdr), t_numfloat)
  if num1.type == NUM_INT and num2.type == NUM_INT
    load32(@num1 + intval)
    rem[1], rem[0] = div32(@num2 + intval)
    return new_int(rem[0], rem[1])
  fin
  memcpy(@ext1, num_ext(@num1), t_extended)
  memcpy(@ext2, num_ext(@num2), t_extended)
  sane:saveZP()
  sane:restoreZP(sane:op2FP(FOREM, @ext1, @ext2))
  return new_float(@ext1)
end

def natv_neg(symptr, expr)
  var num
  word[2] neg

  num = eval_num(expr)
  if num->type == NUM_INT
    load32(num + intval)
    neg32
    store32(@neg)
    return new_int(neg[0], neg[1])
  fin
  num->floatval[9] = num->floatval[9] ^ $80 // Fun with float bits
  return new_float(num + floatval)
end

def natv_abs(symptr, expr)
  var num
  word[2] abs

  num = eval_num(expr)
  if num->type == NUM_INT
    if num=>intval[1] < 0
      load32(num + intval)
      neg32
      store32(@abs)
    else
      abs[0] = num=>intval[0]
      abs[1] = num=>intval[1]
    fin
    return new_int(abs[0], abs[1])
  fin
  num->floatval[9] = num->floatval[9] & $7F // Fun with float bits
  return new_float(num + floatval)
end

def natv_gt(symptr, expr)
  res[t_numfloat] num1, num2
  res[t_extended] ext1, ext2

  memcpy(@num1, eval_num(expr),      t_numfloat)
  memcpy(@num2, eval_num(expr=>cdr), t_numfloat)
  if num1.type == NUM_INT and num2.type == NUM_INT
    load32(@num1 + intval)
    return bool_pred(isgt32(@num2 + intval))
  fin
  memcpy(@ext1, num_ext(@num1), t_extended)
  memcpy(@ext2, num_ext(@num2), t_extended)
  sane:saveZP()
  sane:restoreZP(sane:op2FP(FOSUB, @ext2, @ext1))
  return bool_pred(ext2[9] & $80) // Check sign bit
end

def natv_lt(symptr, expr)
  res[t_numfloat] num1, num2
  res[t_extended] ext1, ext2

  memcpy(@num1, eval_num(expr),      t_numfloat)
  memcpy(@num2, eval_num(expr=>cdr), t_numfloat)
  if num1.type == NUM_INT and num2.type == NUM_INT
    load32(@num1 + intval)
    return bool_pred(islt32(@num2 + intval))
  fin
  memcpy(@ext1, num_ext(@num1), t_extended)
  memcpy(@ext2, num_ext(@num2), t_extended)
  sane:saveZP()
  sane:restoreZP(sane:op2FP(FOSUB, @ext1, @ext2))
  return bool_pred(ext1[9] & $80) // Check sign bit
end

def natv_min(symptr, expr)
  var num
  word[2] intmin
  res[t_extended] extmin, ext1, ext2

  num  = eval_num(expr)
  expr = expr=>cdr
  if num->type == NUM_INT
    //
    // Find min as integers unless a float is encountered
    //
    intmin[0] = num=>intval[0]
    intmin[1] = num=>intval[1]
    while expr
      num = eval_num(expr)
      if num->type == NUM_FLOAT
        memcpy(@extmin, int32_ext(@intmin), t_extended)
        break
      fin
      load32(@intmin)
      if isgt32(num + intval)
        intmin[0] = num=>intval[0]
        intmin[1] = num=>intval[1]
      fin
      expr = expr=>cdr
    loop
    if !expr; return new_int(intmin[0], intmin[1]); fin
  else
    memcpy(@extmin, num + floatval, t_extended)
    if expr; num = eval_num(expr); fin
  fin
  while expr
    memcpy(@ext1, num_ext(num), t_extended)
    memcpy(@ext2, @ext1,        t_extended)
    sane:saveZP()
    sane:restoreZP(sane:op2FP(FOSUB, @ext2, @extmin))
    if ext2[9] & $80 // Check sign bit
      memcpy(@extmin, @ext1, t_extended)
    fin
    expr = expr=>cdr
    if expr; num = eval_num(expr); fin
  loop
  return new_float(@extmin)
end

def natv_max(symptr, expr)
  var num
  word[2] intmax
  res[t_extended] extmax, ext1, ext2

  num  = eval_num(expr)
  expr = expr=>cdr
  if num->type == NUM_INT
    //
    // Find max as integers unless a float is encountered
    //
    intmax[0] = num=>intval[0]
    intmax[1] = num=>intval[1]
    while expr
      num = eval_num(expr)
      if num->type == NUM_FLOAT
        memcpy(@extmax, int32_ext(@intmax), t_extended)
        break
      fin
      load32(@intmax)
      if islt32(num + intval)
        intmax[0] = num=>intval[0]
        intmax[1] = num=>intval[1]
      fin
      expr = expr=>cdr
    loop
    if !expr; return new_int(intmax[0], intmax[1]); fin
  else
    memcpy(@extmax, num + floatval, t_extended)
    if expr; num = eval_num(expr); fin
  fin
  while expr
    memcpy(@ext1, @extmax,      t_extended)
    memcpy(@ext2, num_ext(num), t_extended)
    sane:saveZP()
    sane:restoreZP(sane:op2FP(FOSUB, @ext1, @ext2))
    if ext1[9] & $80 // Check sign bit
      memcpy(@extmax, @ext2, t_extended)
    fin
    expr = expr=>cdr
    if expr; num = eval_num(expr); fin
  loop
  return new_float(@extmax)
end

def sane_op1(op1, expr)
  res[t_extended] ext

  memcpy(@ext, eval_ext(expr), t_extended)
  sane:saveZP()
  sane:restoreZP(sane:op1FP(op1, @ext))
  return new_float(@ext)
end

def natv_logb(symptr, expr)
  return sane_op1(FOLOGB, expr)
end

def natv_scalebI(symptr, expr)
  var int
  res[t_extended] ext

  memcpy(@ext, eval_ext(expr), t_extended)
  int = eval_int16(expr=>cdr)
  sane:saveZP()
  sane:restoreZP(sane:op2FP(FOSCALB, @ext, int))
  return new_float(@ext)
end

def natv_trunc(symptr, expr)
  return sane_op1(FOTTI, expr)
end

def natv_round(symptr, expr)
  return sane_op1(FORTI, expr)
end

def natv_sqrt(symptr, expr)
  return sane_op1(FOSQRT, expr)
end

//
// ELEMS functions
//

def elem_op1(op1, expr)
  res[t_extended] ext

  memcpy(@ext, eval_ext(expr), t_extended)
  sane:saveZP()
  sane:restoreZP(sane:op1ELEM(op1, @ext))
  return new_float(@ext)
end

def elem_op2(op2, expr)
  res[t_extended] ext1, ext2

  memcpy(@ext1, eval_ext(expr),      t_extended)
  memcpy(@ext2, eval_ext(expr=>cdr), t_extended)
  sane:saveZP()
  sane:restoreZP(sane:op2ELEM(op2, @ext1, @ext2))
  return new_float(@ext1)
end

def natv_cos(symptr, expr)
  return elem_op1(FOCOSX, expr)
end

def natv_sin(symptr, expr)
  return elem_op1(FOSINX, expr)
end

def natv_tan(symptr, expr)
  return elem_op1(FOTANX, expr)
end

def natv_atan(symptr, expr)
  return elem_op1(FOATANX, expr)
end

def natv_log2(symptr, expr)
  return elem_op1(FOLOG2X, expr)
end

def natv_log21(symptr, expr)
  return elem_op1(FOLOG21X, expr)
end

def natv_ln(symptr, expr)
  return elem_op1(FOLNX, expr)
end

def natv_ln1(symptr, expr)
  return elem_op1(FOLN1X, expr)
end

def natv_pow2(symptr, expr)
  return elem_op1(FOEXP2X, expr)
end

def natv_pow21(symptr, expr)
  return elem_op1(FOEXP21X, expr)
end

def natv_powE(symptr, expr)
  return elem_op1(FOEXPX, expr)
end

def natv_powE1(symptr, expr)
  return elem_op1(FOEXP1X, expr)
end

def natv_powI(symptr, expr)
  var int
  res[t_extended] ext

  memcpy(@ext, eval_ext(expr), t_extended)
  int = eval_int16(expr=>cdr)
  sane:saveZP()
  sane:restoreZP(sane:op2ELEM(FOXPWRI, @ext, int))
  return new_float(@ext)
end

def natv_pow(symptr, expr)
  return elem_op2(FOXPWRI, expr)
end

def natv_comp(symptr, expr)
  return elem_op2(FOCOMPND, expr)
end

def natv_annuity(symptr, expr)
  return elem_op2(FOANNUIT, expr)
end

//
// Bit-wise operations
//

def natv_bitnot(symptr, expr)
  symptr = eval_int(expr)
  return new_int(~symptr=>intval[0], ~symptr=>intval[1])
end

def natv_bitand(symptr, expr)
  var[2] bitval

  symptr    = eval_int(expr)
  bitval[0] = symptr=>intval[0]
  bitval[1] = symptr=>intval[1]
  symptr    = eval_int(expr=>cdr)
  return new_int(bitval[0] & symptr=>intval[0], bitval[1] & symptr=>intval[1])
end

def natv_bitor(symptr, expr)
  var[2] bitval

  symptr    = eval_int(expr)
  bitval[0] = symptr=>intval[0]
  bitval[1] = symptr=>intval[1]
  symptr    = eval_int(expr=>cdr)
  return new_int(bitval[0] | symptr=>intval[0], bitval[1] | symptr=>intval[1])
end

def natv_bitxor(symptr, expr)
  var[2] bitval

  symptr    = eval_int(expr)
  bitval[0] = symptr=>intval[0]
  bitval[1] = symptr=>intval[1]
  symptr    = eval_int(expr=>cdr)
  return new_int(bitval[0] ^ symptr=>intval[0], bitval[1] ^ symptr=>intval[1])
end

def natv_arithshift(symptr, expr)
  var[2] bitval
  var shift

  symptr    = eval_int(expr)
  bitval[0] = symptr=>intval[0]
  bitval[1] = symptr=>intval[1]
  symptr    = eval_int(expr=>cdr)
  shift     = symptr=>intval[0]
  if shift < 0
    //
    // Shift right
    //
    if shift < -31
      bitval[1] = bitval[1] < 0 ?? $FFFF :: 0
      bitval[0] = bitval[1]
    else
      while shift < 0
        bitval[0] = bitval[0] >> 1
        if bitval[1] & 1
          bitval[0] = bitval[0] | $8000
        else
          bitval[0] = bitval[0] & $7FFF
        fin
        bitval[1] = bitval[1] >> 1
        shift++
      loop
    fin
  else
    //
    // Shift left
    //
    if shift > 31
      bitval[0] = 0
      bitval[1] = 0
    else
      while shift > 0
        bitval[1] = bitval[1] << 1
        if bitval[0] & $8000
          bitval[1] = bitval[1] | 1
        fin
        bitval[0] = bitval[0] << 1
        shift--
      loop
    fin
  fin
  return new_int(bitval[0], bitval[1])
end

def natv_logicshift(symptr, expr)
  var[2] bitval
  var shift

  symptr    = eval_int(expr)
  bitval[0] = symptr=>intval[0]
  bitval[1] = symptr=>intval[1]
  symptr    = eval_int(expr=>cdr)
  shift     = symptr=>intval[0]
  if shift < 0
    //
    // Shift right
    //
    if shift < -31
      bitval[1] = 0
      bitval[0] = 0
    else
      while shift < 0
        bitval[0] = bitval[0] >> 1
        if bitval[1] & 1
          bitval[0] = bitval[0] | $8000
        else
          bitval[0] = bitval[0] & $7FFF
        fin
        bitval[1] = (bitval[1] >> 1) & $7FFF
        shift++
      loop
    fin
  else
    //
    // Shift left
    //
    if shift > 31
      bitval[0] = 0
      bitval[1] = 0
    else
      while shift > 0
        bitval[1] = bitval[1] << 1
        if bitval[0] & $8000
          bitval[1] = bitval[1] | 1
        fin
        bitval[0] = bitval[0] << 1
        shift--
      loop
    fin
  fin
  return new_int(bitval[0], bitval[1])
end

def natv_rotate(symptr, expr)
  var[2] bitval
  var rotate, wrap

  symptr    = eval_int(expr)
  bitval[0] = symptr=>intval[0]
  bitval[1] = symptr=>intval[1]
  symptr    = eval_int(expr=>cdr)
  rotate    = symptr=>intval[0]
  if rotate < 0
    rotate = rotate | $FFFFFFE0
    while rotate < 0
      wrap      = bitval[0] & 1 ?? $8000 :: 0
      bitval[0] = bitval[0] >> 1
      if bitval[1] & 1
        bitval[0] = bitval[0] | $8000
      else
        bitval[0] = bitval[0] & $7FFF
      fin
      bitval[1] = wrap | (bitval[1] >> 1)
      rotate++
    loop
  else
    rotate = rotate & $0000001F
    while rotate > 0
      wrap      = bitval[1] & $8000 ?? 1 :: 0
      bitval[1] = bitval[1] << 1
      if bitval[0] & $8000
        bitval[1] = bitval[1] | 1
      fin
      bitval[0] = wrap | (bitval[0] << 1)
      rotate--
    loop
  fin
  return new_int(bitval[0], bitval[1])
end

export def load_elem#0
  new_sym("COS")=>natv     = @natv_cos
  new_sym("SIN")=>natv     = @natv_sin
  new_sym("TAN")=>natv     = @natv_tan
  new_sym("ATAN")=>natv    = @natv_atan
  new_sym("LOG2")=>natv    = @natv_log2
  new_sym("LOG2_1")=>natv  = @natv_log21
  new_sym("LN")=>natv      = @natv_ln
  new_sym("LN_1")=>natv    = @natv_ln1
  new_sym("POW2")=>natv    = @natv_pow2
  new_sym("POW2_1")=>natv  = @natv_pow21
  new_sym("POWE")=>natv    = @natv_powE
  new_sym("POWE_1")=>natv  = @natv_powE1
  new_sym("POW_I")=>natv   = @natv_powI
  new_sym("POWY")=>natv    = @natv_pow
  new_sym("COMP")=>natv    = @natv_comp
  new_sym("ANNUITY")=>natv = @natv_annuity
  sane:saveZP() // Force load of ELEM library
  sane:restoreZP(sane:op1ELEM(FOSINX, @ext_pi))
end

//
// Install numerical constants and functions
//

sane:initFP()
new_sym("*PI*")=>apval      = new_float(@ext_pi) ^ NULL_HACK
new_sym("*E*")=>apval       = new_float(@ext_e)  ^ NULL_HACK
new_sym("INTEGER")=>natv    = @natv_integer
new_sym("SUM")=>natv        = @natv_sum
new_sym("+")=>natv          = @natv_sum
new_sym("-")=>natv          = @natv_sub
new_sym("*")=>natv          = @natv_mul
new_sym("/")=>natv          = @natv_div
new_sym("REM")=>natv        = @natv_rem
new_sym("NEG")=>natv        = @natv_neg
new_sym("ABS")=>natv        = @natv_abs
new_sym(">")=>natv          = @natv_gt
new_sym("<")=>natv          = @natv_lt
new_sym("MIN")=>natv        = @natv_min
new_sym("MAX")=>natv        = @natv_max
new_sym("LOGB")=>natv       = @natv_logb
new_sym("SCALEB")=>natv     = @natv_scalebI
new_sym("TRUNCATE")=>natv   = @natv_trunc
new_sym("ROUND")=>natv      = @natv_round
new_sym("SQRT")=>natv       = @natv_sqrt
new_sym("BITNOT")=>natv     = @natv_bitnot
new_sym("BITAND")=>natv     = @natv_bitand
new_sym("BITOR")=>natv      = @natv_bitor
new_sym("BITXOR")=>natv     = @natv_bitxor
new_sym("ARITHSHIFT")=>natv = @natv_arithshift
new_sym("LOGICSHIFT")=>natv = @natv_logicshift
new_sym("ROTATE")=>natv     = @natv_rotate
new_sym("~")=>natv          = @natv_bitnot
new_sym("&")=>natv          = @natv_bitand
new_sym("|")=>natv          = @natv_bitor
new_sym("^")=>natv          = @natv_bitxor
new_sym("<<-")=>natv        = @natv_arithshift
new_sym("<<")=>natv         = @natv_logicshift
new_sym("<<<")=>natv        = @natv_rotate
return modkeep | modinitkeep
done
