#include "vm.h"
#define FRAME_HEADER_SIZE 12

u32 pc;       /* program counter */
u32 cp;       /* code pointer */
u32 mp;       /* memory pointer */
u32 fp;       /* frame pointer */
u8  lc;       /* child local count */
u8  status;   /* status flag */
u8 interrupt; /* device interrupt */
u32 *code;    /* code */
u8 *mem;      /* memory */
Frame *frames;/* stack frame */

#define MAX_LEN_INT32 11
#define MAX_INT32 2147483647
#define MIN_INT32 -2147483648
const char radix_set[11] = "0123456789";

u32 str_alloc(const char *str, u32 length) {
  u32 str_addr = mp;
  u32 i = 0;
  mp += 4;
  while (i < length) {
    mem[mp++] = str[i++];
  }
  mem[mp++] = '\0';

  WRITE_U32(str_addr, length);
  return str_addr;
}

bool step_vm() {

  u32 instruction = code[pc++];
  u8 opcode = DECODE_OP(instruction);

  Frame *frame = &frames[fp];
  u32 *globals = (u32*)(mem);

  switch (opcode) {
  case OP_HALT: {
    /* no need to decode, all are zeros */
    return false;
  }
  case OP_CALL: {
    DECODE_A(instruction)
    Frame *child_frame = &frames[fp + 1];
    /* function to jump to */
    u32 fn_ptr = frame->local[dest]; 
    /* reset child locals counter */
    lc = 0;
    /* push return address to child frame */
    child_frame->return_address = pc;
    /* push local address to return the value to */
    child_frame->parent_return_local = src1;
    /* now set the frame pointer, where the locals start */
    child_frame->start_mp = mp;
    /* increment frame pointer */
    fp++;
    /* jump to dest_ptr */
    pc = fn_ptr;
    USED(src2);
    return true;
  }
  case OP_RETURN: {
    DECODE_B(instruction)
    u32 i, size = 0;
    u32 return_value = frame->local[dest];
    Frame *child = frame;
    Frame *parent = &frames[fp - 1];
    u32 parent_local_return_address = frame->parent_return_local;
    USED(imm); 

    if (child->parent_return_local != 0xFF) {

    }

    if (false) {
    /* copy value to end of mp if it is a pointer */
      parent->local[parent_local_return_address] = mp;
      size = READ_U32(return_value);
      WRITE_U32(mp, size);
      mp += 4;
      for (i = 0; i < size; i++) {
        u8 value = READ_U8(return_value + i);
        WRITE_U8(mp, value);
        mp++;
      }
    } else {
    /* otherwise just write the return value to its location */
      parent->local[parent_local_return_address] = return_value;
    }

    /* jump to parent frame */
    pc = frame->return_address;
    /* reset memory to parents end of memory */
    mp = frame->start_mp;
    /* reset the frame pointer */
    fp--;
    return true;
  }
  case OP_SYSCALL: {
    DECODE_A(instruction)
    u32 id = dest;   /* syscall id */
    u32 size = src1; /* size of heap at that pointer */
    u32 rd = frame->local[src2]; /* the pointer */
    status = syscall(id, size, rd);
    return true;
  }
  case OP_PUSH: {
    DECODE_B(instruction)
    Frame *child_frame = &frames[fp + 1];
    child_frame->local[lc] = frame->local[dest];
    lc++;
    USED(imm); 
    return true;
  }
  case OP_POP: {
    DECODE_C(instruction)
    USED(imm);
    lc--;
    return true;
  }
  case OP_LOAD_IMM: {
    DECODE_B(instruction)
    frame->local[dest] = imm;
    return true;
  }
  case OP_LOAD_UPPER_IMM: {
    DECODE_B(instruction)
    u32 value = frame->local[dest];
    frame->local[dest] = (value | (((u32)(imm)) << 16));
    return true;
  }
  case OP_LOAD_IND_8: {
  }
  case OP_LOAD_IND_16: {
  }
  case OP_LOAD_IND_32: {
  }
  case OP_LOAD_ABS_8: {
  }
  case OP_LOAD_ABS_16: {
  }
  case OP_LOAD_ABS_32: {
  }
  case OP_LOAD_OFF_8: {
  }
  case OP_LOAD_OFF_16: {
  }
  case OP_LOAD_OFF_32: {
  }
  case OP_STORE_ABS_8: {
  }
  case OP_STORE_ABS_16: {
  }
  case OP_STORE_ABS_32: {
  }
  case OP_STORE_IND_8: {
  }
  case OP_STORE_IND_16: {
  }
  case OP_STORE_IND_32: {
  }
  case OP_STORE_OFF_8: {
  }
  case OP_STORE_OFF_16: {
  }
  case OP_STORE_OFF_32: {
  }
  case OP_MEM_ALLOC: {
    DECODE_A(instruction)
    u32 size, ldest;
    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);
    USED(src2);
    ldest = READ_U32(rd);
    WRITE_U32(ldest, mp);
    size = READ_U32(r1);
    WRITE_U32(mp, size);
    mp += (4 * (size + 4));
    return true;
  }
  case OP_MEM_CPY_8: {
    DECODE_A(instruction)
    u32 i = 0;
    u32 mdest = frame->local[dest];
    u32 msrc = frame->local[src1];
    u32 count = frame->local[src2];

    if (mdest + count >= mp) {
      status = 1;
      return true;
    }

    for (i = 0; i < count; i++) {
      mem[msrc + i] = mem[mdest + i];
    }

    status = 0;
    return true;
  }
  case OP_MEM_CPY_16: {
    DECODE_A(instruction)
    u32 i = 0;
    u32 mdest = frame->local[dest];
    u32 msrc = frame->local[src1];
    u32 count = frame->local[src2] * 2;

    if (mdest + count >= mp) {
      status = 1;
      return true;
    }

    for (i = 0; i < count; i++) {
      u16 value = READ_U16(mdest + i);
      WRITE_U16(msrc + i, value);
    }

    status = 0;
    return true;
  }
  case OP_MEM_CPY_32: {
    DECODE_A(instruction)
    u32 i = 0;
    u32 mdest = frame->local[dest];
    u32 msrc = frame->local[src1];
    u32 count = frame->local[src2];

    if (mdest + count >= mp) {
      status = 1;
      return true;
    }

    for (i = 0; i < count; i++) {
      globals[msrc + i] = globals[mdest + i];
    }

    status = 0;
    return true;
  }
  case OP_MEM_SET_8: {
    DECODE_A(instruction)
    u32 i, start, end;

    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);
    u32 r2 = fp + (src2 * 4);

    u8 value = (u8)READ_U32(r1);
    u32 count = READ_U32(r2);

    if (r2 == 0) {
      status = 1;
      return true;
    }

    start = READ_U32(rd);
    end = start + count;

    if (start >= mp || r2 > mp || end > mp) {
      status = 1;
      return true;
    }

    for (i = start; i < end; i++) {
      mem[i] = value;
    }

    status = 0;
    return true;
  }
  case OP_MEM_SET_16: {
    DECODE_A(instruction)
    u32 i, start, end;

    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);
    u32 r2 = fp + (src2 * 4);

    u16 value = (u16)READ_U32(r1);
    u32 count = READ_U32(r2);

    if (r2 == 0) {
      status = 1;
      return true;
    }

    start = READ_U32(rd);
    end = start + count;

    if (start >= mp || r2 > mp || end > mp) {
      status = 1;
      return true;
    }

    for (i = start; i < end; i += 2) {
      WRITE_U16(i, value);
    }

    status = 0;
    return true;
  }
  case OP_MEM_SET_32: {
    DECODE_A(instruction)
    u32 i, start, end;

    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);
    u32 r2 = fp + (src2 * 4);

    u32 value = READ_U32(r1);
    u32 count = READ_U32(r2);

    if (r2 == 0) {
      status = 1;
      return true;
    }

    start = READ_U32(rd);
    end = start + count;

    if (start >= mp || r2 > mp || end > mp) {
      status = 1;
      return true;
    }

    for (i = start; i < end; i += 4) {
      WRITE_U32(i, value);
    }

    status = 0;
    return true;
  }
  case OP_MOV: {
    DECODE_A(instruction)
    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);
    u32 value = READ_U32(r1);
    USED(src2);
    WRITE_U32(rd, value);
    return true;
  }
  case OP_ADD_INT: {
    MATH_OP(i32, +);
  }
  case OP_SUB_INT: {
    MATH_OP(i32, -);
  }
  case OP_MUL_INT: {
    MATH_OP(i32, *);
  }
  case OP_DIV_INT: {
    MATH_OP(i32, /);
  }
  case OP_ADD_NAT: {
    MATH_OP(u32, +);
  }
  case OP_SUB_NAT: {
    MATH_OP(u32, -);
  }
  case OP_MUL_NAT: {
    MATH_OP(u32, *);
  }
  case OP_DIV_NAT: {
    MATH_OP(u32, /);
  }
  case OP_ADD_REAL: {
    MATH_OP(i32, +);
  }
  case OP_SUB_REAL: {
    MATH_OP(i32, -);
  }
  case OP_MUL_REAL: {
    DECODE_A(instruction)

    i32 src1_whole = (i32)frame->local[src1] >> 16;
    i32 src2_whole = (i32)frame->local[src2] >> 16;

    i32 src1_decimal = (i32)frame->local[src1] & 16;
    i32 src2_decimal = (i32)frame->local[src2] & 16;

    i32 result = 0;
    result += (src1_whole * src2_whole) << 16;
    result += (src1_whole * src2_decimal);
    result += (src1_decimal * src2_whole);
    result += ((src1_decimal * src2_decimal) >> 16) & 16;
    frame->local[dest] = result;
    return true;
  }
  case OP_DIV_REAL: {
    DECODE_A(instruction)
    i32 result;
    i32 src1_val = (i32)frame->local[src1];
    i32 src2_val = (i32)frame->local[src2];

    u32 src2_reciprocal = 1;
    src2_reciprocal <<= 31;
    src2_reciprocal = (u32)(src2_reciprocal / src2_val);

    result = src1_val * src2_reciprocal;
    result <<= 1;

    frame->local[dest] = result;
    return true;
  }
  case OP_INT_TO_REAL: {
    DECODE_A(instruction)
    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);
    i32 result = (i32)READ_U32(r1) << 16;
    USED(src2);
    WRITE_U32(rd, result);
    return true;
  }
  case OP_INT_TO_NAT: {
    DECODE_A(instruction)
    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);
    u32 result = (u32)READ_U32(r1);
    USED(src2);
    WRITE_U32(rd, result);
    return true;
  }
  case OP_NAT_TO_REAL: {
    DECODE_A(instruction)
    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);
    i32 result = ((i32)READ_U32(r1) << 16);
    USED(src2);
    WRITE_U32(rd, result);
    return true;
  }
  case OP_NAT_TO_INT: {
    DECODE_A(instruction)
    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);
    i32 result = ((i32)READ_U32(r1));
    USED(src2);
    WRITE_U32(rd, result);
    return true;
  }
  case OP_REAL_TO_INT: {
    DECODE_A(instruction)
    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);
    i32 result = ((i32)READ_U32(r1) >> 16);
    USED(src2);
    WRITE_U32(rd, result);
    return true;
  }
  case OP_REAL_TO_NAT: {
    DECODE_A(instruction)
    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);
    u32 result = ((u32)READ_U32(r1) >> 16);
    USED(src2);
    WRITE_U32(rd, result);
    return true;
  }
  case OP_BIT_SHIFT_LEFT: {
    MATH_OP_NO_CAST(<<);
  }
  case OP_BIT_SHIFT_RIGHT: {
    MATH_OP_NO_CAST(>>);
  }
  case OP_BIT_SHIFT_R_EXT: {
    MATH_OP(i32, >>);
  }
  case OP_BIT_AND: {
    MATH_OP_NO_CAST(&);
  }
  case OP_BIT_OR: {
    MATH_OP_NO_CAST(|);
  }
  case OP_BIT_XOR: {
    MATH_OP_NO_CAST(^);
  }
  case OP_JMP_IMM: {
    DECODE_C(instruction)
    pc = imm;
    return true;
  }
  case OP_JMP_ABS: {
    DECODE_A(instruction)
    u32 rd = fp + (dest * 4);
    u32 jmp_dest = READ_U32(rd);
    if (jmp_dest > cp) {
      status = 1;
      return true;
    }
    USED(src1);
    USED(src2);

    pc = jmp_dest;
    return true;
  }
  case OP_JMP_OFF: {
    DECODE_A(instruction)
    u32 rd = fp + (dest * 4);
    u32 r1 = fp + (src1 * 4);

    u32 jmp_dest = READ_U32(rd) + READ_U32(r1);
    if (jmp_dest > cp) {
      status = 1;
      return true;
    }
    USED(src2);

    pc = jmp_dest;
    return true;
  }
  case OP_JMP_FLAG: {
    DECODE_A(instruction)
    u32 mask;
    u32 jmp_dest = frame->local[dest];
    if (jmp_dest > cp) {
      status = 1;
      return true;
    }
    USED(src1);
    USED(src2);

    mask = -(u32)(status == 0);
    pc = (jmp_dest & mask) | (pc & ~mask);
    return true;
  }
  case OP_JEQ_INT: {
    COMPARE_AND_JUMP(i32, ==);
  }
  case OP_JNE_INT: {
    COMPARE_AND_JUMP(i32, !=);
  }
  case OP_JGT_INT: {
    COMPARE_AND_JUMP(i32, >);
  }
  case OP_JLT_INT: {
    COMPARE_AND_JUMP(i32, <);
  }
  case OP_JLE_INT: {
    COMPARE_AND_JUMP(i32, <=);
  }
  case OP_JGE_INT: {
    COMPARE_AND_JUMP(i32, >=);
  }
  case OP_JEQ_NAT: {
    COMPARE_AND_JUMP(u32, ==);
  }
  case OP_JNE_NAT: {
    COMPARE_AND_JUMP(u32, !=);
  }
  case OP_JGT_NAT: {
    COMPARE_AND_JUMP(u32, >);
  }
  case OP_JLT_NAT: {
    COMPARE_AND_JUMP(u32, <);
  }
  case OP_JLE_NAT: {
    COMPARE_AND_JUMP(u32, <=);
  }
  case OP_JGE_NAT: {
    COMPARE_AND_JUMP(u32, >=);
  }
  case OP_JEQ_REAL: {
    COMPARE_AND_JUMP(i32, ==);
  }
  case OP_JNE_REAL: {
    COMPARE_AND_JUMP(i32, !=);
  }
  case OP_JGE_REAL: {
    COMPARE_AND_JUMP(i32, >=);
  }
  case OP_JGT_REAL: {
    COMPARE_AND_JUMP(i32, >);
  }
  case OP_JLT_REAL: {
    COMPARE_AND_JUMP(i32, <);
  }
  case OP_JLE_REAL: {
    COMPARE_AND_JUMP(i32, <=);
  }
  case OP_INT_TO_STR: {
    DECODE_A(instruction)
    
    u32 i = MAX_LEN_INT32;
    i32 v = (i32)frame->local[src1];
    char buffer[MAX_LEN_INT32];
    i32 n = v;
    bool neg = n < 0;
    USED(src2);

    if (neg)
      n = -n;

    do {
      buffer[--i] = radix_set[n % 10];
      n /= 10;
    } while (n > 0);
    if (neg)
      buffer[--i] = '-';
    /* Ensure at least one digit is written for 0 */
    if (v == 0)
      buffer[--i] = '0';

    /* Copy from buffer[i] to buffer + MAX_LEN_INT32 */
    frame->local[dest] = str_alloc(buffer + i, MAX_LEN_INT32 - i);

    return pc;
  }
  case OP_NAT_TO_STR: {
    DECODE_A(instruction)
    
    u32 v = (i32)frame->local[src1];
    char buffer[MAX_LEN_INT32];
    u32 n = v;
    u32 i = MAX_LEN_INT32;
    USED(src2);
    do {
      buffer[--i] = radix_set[n % 10];
      n /= 10;
    } while (n > 0);
    /* Ensure at least one digit is written for 0 */
    if (v == 0)
      buffer[--i] = '0';
    /* Copy from buffer[i] to buffer + MAX_LEN_INT32 */
    frame->local[dest] = str_alloc(buffer + i, MAX_LEN_INT32 - i);

    return pc;
  }
  case OP_REAL_TO_STR: {
    DECODE_A(instruction)
    
    u32 i = 0, j = 0;
    i32 q = (i32)frame->local[src1];
    char buffer[MAX_LEN_INT32];
    u32 int_part, frac_part;

    if (q < 0) {
      buffer[i++] = '-';
      q = -q;
    }

    int_part = q >> 16;
    frac_part = q & 0xFFFF;
    USED(src2);

    if (int_part == 0) {
      buffer[i++] = radix_set[0];
    } else {
      char tmp[16];
      i32 tmp_i = 0;
      while (int_part > 0) {
        tmp[tmp_i++] = radix_set[int_part % 10];
        int_part /= 10;
      }
      while (tmp_i > 0) {
        buffer[i++] = tmp[--tmp_i];
      }
    }

    buffer[i++] = '.';
    for (j = 0; j < 6; j++) {
      frac_part *= 10;
      buffer[i++] = radix_set[frac_part >> 16];
      frac_part &= 0xFFFF;           
    }

    frame->local[dest] = str_alloc(buffer + i, MAX_LEN_INT32 - i);
    
    return pc;
  }

  }

  /* something went very wrong */
  status = 255;
  return false;
}