#include "vm.h"
#define FRAME_HEADER_SIZE 12

u32 *stack;    /* stack */
u32 sp;        /* stack pointer */
u8  *code;     /* code */
u32 cp;        /* code pointer */
u8  *mem;      /* memory */
u32 mp;        /* memory pointer */
Frame *frames; /* call frames */
u32 fp;        /* frame pointer */
u32 pc;        /* program counter */
u8  status;    /* status flag */
u8  interrupt; /* device interrupt */

#define MAX_LEN_INT32 11
const char radix_set[11] = "0123456789";

u32 str_alloc(char *str, u32 length) {
  u32 str_addr = mp;
  u8 *ptr = &mem[mp];
  mcpy(ptr, &length, sizeof(u32));
  ptr += 4;
  mcpy(ptr, str, length);
  ptr[length] = '\0';
  mp += 4 + length;
  return str_addr;
}

bool step_vm() {

  u8 opcode = code[pc++];
  /*u32 *locals = (u32*)(&mem[fp]);*/
  u32 *globals = (u32*)(mem);

  switch (opcode) {
  case OP_HALT: {
    /* no need to decode, all are zeros */
    return false;
  }
  case OP_CALL: {
    /* function to jump to */
    u32 fn_ptr = stack[--sp]; 
    /* get mp in 'global indexing mode' */
    u32 *header = &globals[mp / 4];
    /* push parents frame value to reset the heap to */
    (*header++) = fp;
    /* increase the mp to new size */
    mp += FRAME_HEADER_SIZE;
    /* now set the frame pointer, where the locals start */
    fp = mp;    
    /* move mp forward by count many locals */
    mp += stack[--sp];
    /* jump to dest_ptr */
    pc = fn_ptr;
    return false;
  }
  case OP_RETURN: {
    /* TODO: Fix this one so it makes sense with a stack based system */
    u32 size = 0;
    u32 return_value = stack[--sp];
    bool is_ptr = (((u32)(1)) << 15) & return_value;

    /* reset mp to saved mp, use header size to get "real" start of frame */
    u32 *frame_start = &globals[(fp / 4) - 3];
    u32 parent_fp = *frame_start++;
    u32 return_address = *frame_start++;
    u32 parent_local_return_address = *frame_start++;

    /* reset memory to parents end of memory */
    mp = fp - FRAME_HEADER_SIZE;
    /* reset the frame pointer */
    fp = parent_fp;

    if (is_ptr) {
    /* copy value to end of mp if it is a pointer */
      globals[parent_local_return_address/4] = mp;
      size = globals[return_value/4];
      globals[mp/4] = size;
      mp += 4;
      mcpy(&mem[mp], &mem[return_value], size);
      mp += size;
    } else {
    /* otherwise just write the return value to its location */
      mcpy(&mem[parent_local_return_address], &return_value, sizeof(u32));
    }

    /* jump to parent frame */
    pc = return_address;
    return false;
  }
  case OP_SYSCALL: {
    u32 id = stack[--sp]; /* syscall id */
    u32 rd = stack[--sp]; /* the pointer */
    status = syscall(id, rd);
    return true;
  }
  case OP_PUSH_8: {
    u8 value = code[pc++];
    stack[sp++] = value;
    return true;
  }
  case OP_PUSH_16: {
    u16 *values = (u16*)(code);
    u16 value = values[pc/2];
    pc+=2;
    stack[sp++] = value;
    return true;
  }
  case OP_PUSH_32: {
    u32 value = ((u32)code[(pc) + 3] << 24) |
   ((u32)code[(pc) + 2] << 16) |
   ((u32)code[(pc) + 1] << 8) | ((u32)mem[(pc)]);
    pc+=4;
    stack[sp++] = value;
    return true;
  }
  case OP_POP: {
    --sp;
    return true;
  }
  case OP_LOAD_8: {
    u32 ptr = stack[--sp];
    u32 value = mem[ptr];
    stack[sp++] = value;
    return true;
  }
  case OP_LOAD_16: {
    u32 ptr = stack[--sp];
    u16 *values = (u16*)(&mem[ptr]);
    u32 value = values[0];
    stack[sp++] = value;
    return true;
  }
  case OP_LOAD_32: {
    u32 ptr = stack[--sp];
    u32 *values = (u32*)(&mem[ptr]);
    u32 value = values[0];
    stack[sp++] = value;
    return true;
  }
  case OP_STORE_8: {
    u32 ptr = stack[--sp];
    u8 value = (u8)stack[--sp];
    mem[ptr] = value;
    return true;
  }
  case OP_STORE_16: {
    u32 ptr = stack[--sp];
    u16 value = (u16)stack[--sp];
    u16 *values = (u16*)(&mem[ptr]);
    values[0] = value;
    return true;
  }
  case OP_STORE_32: {
    u32 ptr = stack[--sp];
    u32 value = stack[--sp];
    u32 *values = (u32*)(&mem[ptr]);
    values[0] = value;
    return true;
  }
  case OP_MEM_ALLOC: {
    u32 size = stack[--sp];
    stack[sp++] = mp;
    WRITE_U32(mp, size);
    mp += (size + 4);
    return true;
  }
  case OP_MEM_CPY_8: {
    u8 *ptr_src;
    u8 *ptr_dest;
    u32 count = stack[--sp];
    u32 src = stack[--sp];
    u32 dest = stack[--sp];

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

    ptr_dest = &mem[dest];
    ptr_src = &mem[src];
    mcpy(ptr_dest, ptr_src, count*sizeof(u8));

    status = 0;
    return true;
  }
  case OP_MEM_CPY_16: {
    u8 *ptr_src;
    u8 *ptr_dest;
    u32 count = stack[--sp];
    u32 src = stack[--sp];
    u32 dest = stack[--sp];

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

    ptr_dest = &mem[dest];
    ptr_src = &mem[src];
    mcpy(ptr_dest, ptr_src, count*sizeof(u16));

    status = 0;
    return true;
  }
  case OP_MEM_CPY_32: {
    u8 *ptr_src;
    u8 *ptr_dest;
    u32 count = stack[--sp];
    u32 src = stack[--sp];
    u32 dest = stack[--sp];

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

    ptr_dest = &mem[dest];
    ptr_src = &mem[src];
    mcpy(ptr_dest, ptr_src, count*sizeof(u32));

    status = 0;
    return true;
  }
  case OP_MEM_SET_8: {
    u8 *ptr_dest;
    u8 value = (u8)stack[--sp];
    u32 count = stack[--sp];
    u32 dest = stack[--sp];

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

    ptr_dest = &mem[dest];
    mcpy(ptr_dest, &value, count*sizeof(u8));

    status = 0;
    return true;
  }
  case OP_MEM_SET_16: {
    u8 *ptr_dest;
    u16 value = (u16)stack[--sp];
    u32 count = stack[--sp];
    u32 dest = stack[--sp];

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

    ptr_dest = &mem[dest];
    mcpy(ptr_dest, &value, count*sizeof(u16));

    status = 0;
    return true;
  }
  case OP_MEM_SET_32: {
    u8 *ptr_dest;
    u32 value = stack[--sp];
    u32 count = stack[--sp];
    u32 dest = stack[--sp];

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

    ptr_dest = &mem[dest];
    mcpy(ptr_dest, &value, count*sizeof(u32));

    status = 0;
    return true;
  }
  case OP_DUP: {
    u32 a = stack[--sp];
    stack[sp++] = a;
    stack[sp++] = a;
    return true;
  }
  case OP_EXCH: {
    u32 a = stack[--sp];
    u32 b = stack[--sp];
    stack[sp++] = b;
    stack[sp++] = a;
    return true;
  }
  case OP_OVER: {
    u32 a = stack[sp - 1];
    stack[sp++] = a;
    return true;
  }
  case OP_PICK: {
    u32 n = stack[--sp];
    u32 b = stack[sp - n];
    stack[sp++] = b;
    return true;
  }
  case OP_DEPTH: {
    u32 a = sp;
    stack[sp++] = a; 
    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: {
    i32 src1 = (i32)stack[--sp];
    i32 src2 = (i32)stack[--sp];

    i32 src1_whole = src1 >> 16;
    i32 src2_whole = src2 >> 16;

    i32 src1_decimal = src1 & 16;
    i32 src2_decimal = 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;
    stack[sp++] = result;
    return true;
  }
  case OP_DIV_REAL: {
    i32 result;
    i32 src1_val = (i32)stack[--sp];
    i32 src2_val = (i32)stack[--sp];

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

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

    stack[sp++] = result;
    return true;
  }
  case OP_INT_TO_REAL: {
    i32 result = (i32)stack[--sp] << 16;
    stack[sp++] = result;
    return true;
  }
  case OP_INT_TO_NAT: {
    u32 result = (u32)stack[--sp];
    stack[sp++] = result;
    return true;
  }
  case OP_NAT_TO_REAL: {
    i32 result = (i32)stack[--sp] << 16;
    stack[sp++] = result;
    return true;
  }
  case OP_NAT_TO_INT: {
    i32 result = (i32)stack[--sp];
    stack[sp++] = result;
    return true;
  }
  case OP_REAL_TO_INT: {
    i32 result = (i32)stack[--sp] >> 16;
    stack[sp++] = result;
    return true;
  }
  case OP_REAL_TO_NAT: {
    u32 result = (u32)stack[--sp] >> 16;
    stack[sp++] = result;
    return true;
  }
  case OP_NEG: {
    i32 a = (i32)stack[--sp];
    stack[sp++] = -a;
    return true;
  }  
  case OP_NOT: {
    u32 a = !stack[--sp];
    stack[sp++] = a;
    return true;
  }
  case OP_BIT_SHIFT_LEFT: {
    MATH_OP(u32, <<);
  }
  case OP_BIT_SHIFT_RIGHT: {
    MATH_OP(u32, >>);
  }
  case OP_BIT_SHIFT_R_EXT: {
    MATH_OP(i32, >>);
  }
  case OP_BIT_AND: {
    MATH_OP(u32, &);
  }
  case OP_BIT_OR: {
    MATH_OP(u32, |);
  }
  case OP_BIT_XOR: {
    MATH_OP(u32, ^);
  }
  case OP_EQS: {
    MATH_OP(i32, ==);
  }
  case OP_NES: {
    MATH_OP(i32, !=);
  }
  case OP_GTS: {
    MATH_OP(i32, >);
  }
  case OP_LTS: {
    MATH_OP(i32, <);
  }
  case OP_LES: {
    MATH_OP(i32, <=);
  }
  case OP_GES: {
    MATH_OP(i32, >=);
  }
  case OP_EQU: {
    MATH_OP(u32, ==);
  }
  case OP_NEU: {
    MATH_OP(u32, !=);
  }
  case OP_GTU: {
    MATH_OP(u32, >);
  }
  case OP_LTU: {
    MATH_OP(u32, <);
  }
  case OP_LEU: {
    MATH_OP(u32, <=);
  }
  case OP_GEU: {
    MATH_OP(u32, >=);
  }
  case OP_JMP: {
    pc = stack[--sp];
    return true;
  }
  case OP_JMP_FLAG: {
    u32 mask;
    u32 jmp_dest = stack[--sp];
    mask = -(u32)(status == 0);
    pc = (jmp_dest & mask) | (pc & ~mask);
    return true;
  }
  case OP_JNZ: {
    u32 mask, target;
    i32 cond = stack[--sp];
    target = stack[--sp];
    mask = -(u32)cond;
    pc = (target & mask) | (pc & ~mask);
    return true;
  }
  case OP_INT_TO_STR: {
    u32 i = MAX_LEN_INT32;
    i32 v = (i32)stack[--sp];
    char buffer[MAX_LEN_INT32];
    i32 n = v;
    bool neg = n < 0;
    
    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 */
    stack[sp++] = str_alloc(buffer + i, MAX_LEN_INT32 - i);

    return true;
  }
  case OP_NAT_TO_STR: {
    u32 v = (i32)stack[--sp];
    char buffer[MAX_LEN_INT32];
    u32 n = v;
    u32 i = MAX_LEN_INT32;

    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 */
    stack[sp++] = str_alloc(buffer + i, MAX_LEN_INT32 - i);

    return true;
  }
  case OP_REAL_TO_STR: {
    u32 i = 0, j = 0;
    i32 q = (i32)stack[--sp];
    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;

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

    stack[sp++] = str_alloc(buffer + i, MAX_LEN_INT32 - i);
    
    return true;
  }
  }

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