#include <yemu/ocpu.h>
#include <yemu/stack.h>
#include <stdbool.h>
#include <stdlib.h>

struct OCPU ocpu;
struct OCPU_MEMORY ocpu_mem;
int stack_size = 64000;
stack_t *stack;
byte *sp;

void ocpu_memory_init () {
  for (int i = 0; i < MAX_MEMORY; i++){
    ocpu_mem.memory[i] = 0;
  }
}

byte ocpu_fetch_byte() {
  byte data = ocpu_mem.memory[ocpu.PC];
  ocpu.PC++;
  return data;
}

word ocpu_fetch_word() {
  word data = ocpu_mem.memory[ocpu.PC];
  ocpu.PC++;

  data |= (ocpu_mem.memory[ocpu.PC] << 8);
  ocpu.PC++;

  return data;
}

void ocpu_reset () {
  ocpu.PC = 0x0000; // FIXME
  ocpu.SP = 0x0000; //FIXME
  ocpu.A = ocpu.B = ocpu.C = ocpu.D = ocpu.E = ocpu.F = 0;
  ocpu.ZF = ocpu.NF = ocpu.CF = ocpu.OF = 0;
  ocpu_memory_init(ocpu_mem);
}

void ocpu_dump_ocpu(struct OCPU ocpu) { // FIXME
  printf("PC: %04X\n", ocpu.PC);
  printf("SP: %04X\n", ocpu.SP);
  printf("\n");
  printf("AL: %04X    BL: %04X\n", ocpu.AL, ocpu.BL);
  printf("AH: %04X    BH: %04X\n", ocpu.AH, ocpu.BH);
  printf("A:  %04X     B: %04X\n", ocpu.A, ocpu.B);
  printf("\n");
  printf("CL: %04X    DL: %04X\n", ocpu.CL, ocpu.DL);
  printf("CH: %04X    DH: %04X\n", ocpu.CH, ocpu.DH);
  printf("C:  %04X     D: %04X\n", ocpu.C, ocpu.D);
  printf("\n");
  printf("EL: %04X    FL: %04X\n", ocpu.EL, ocpu.FL);
  printf("EH: %04X    FH: %04X\n", ocpu.EH, ocpu.FH);
  printf("E:  %04X     F: %04X\n", ocpu.E, ocpu.F);
  printf("\n");
  printf("Flags: ZF: %X, NF: %X, CF: %X, OF: %X\n",  ocpu.ZF, ocpu.NF, ocpu.CF, ocpu.OF);
}

int match_register(byte reg) { // Function that returns register's value by taking register opcode
  switch(reg) {
  case REG_AL:
    return ocpu.AL;
  case REG_AH:
    return ocpu.AH;
  case REG_A:
    return ocpu.A;
    
  case REG_BL:
    return ocpu.BL;
  case REG_BH:
    return ocpu.BH;
  case REG_B:
    return ocpu.B;

  case REG_CL:
    return ocpu.CL;
  case REG_CH:
    return ocpu.CH;
  case REG_C:
    return ocpu.C;

  case REG_DL:
    return ocpu.DL;
  case REG_DH:
    return ocpu.DH;
  case REG_D:
    return ocpu.D;

  case REG_EL:
    return ocpu.EL;
  case REG_EH:
    return ocpu.EH;
  case REG_E:
    return ocpu.E;

  case REG_FL:
    return ocpu.FL;
  case REG_FH:
    return ocpu.FH;
  case REG_F:
    return ocpu.F;
  }
  return 0xFF; // FIXME: add some error handling
}

void set_flags(byte reg_value) {
  ocpu.ZF = (reg_value == 0);
  ocpu.NF = (reg_value & 0x40) > 0;
}

void write_register(byte reg, byte value, word word_value) {
  switch(reg) {
    case REG_AL:
      ocpu.A = (ocpu.A & 0xFF00) | (value & 0xFF);
      ocpu.AL = ocpu.A & 0xFF;
      set_flags(ocpu.AL);
      break;
    case REG_AH:
      ocpu.A = ((value & 0xFF) << 8) | (ocpu.A & 0xFF);
      ocpu.AH = ocpu.A >> 8;
      set_flags(ocpu.AH);
      break;
    case REG_A:
      ocpu.A = word_value;
      set_flags(ocpu.A);
      break;
    case REG_BL:
      ocpu.B = (ocpu.B & 0xFF00) | (value & 0xFF);
      ocpu.BL = ocpu.B & 0xFF;
      set_flags(ocpu.BL);
      break;
    case REG_BH:
      ocpu.B = ((value & 0xFF) << 8) | (ocpu.B & 0xFF);
      ocpu.BH = ocpu.B >> 8;
      set_flags(ocpu.BH);
      break;
    case REG_B:
      ocpu.B = word_value;
      set_flags(ocpu.B);
      break;
    case REG_CL:
      ocpu.C = (ocpu.C & 0xFF00) | (value & 0xFF);
      ocpu.CL = ocpu.C & 0xFF;
      set_flags(ocpu.CL);
      break;
    case REG_CH:
      ocpu.C = ((value & 0xFF) << 8) | (ocpu.C & 0xFF);
      ocpu.CH = ocpu.C >> 8;
      set_flags(ocpu.CH);
      break;
    case REG_C:
      ocpu.C = word_value;
      set_flags(ocpu.C);
      break;
    case REG_DL:
      ocpu.D = (ocpu.D & 0xFF00) | (value & 0xFF);
      ocpu.DL = ocpu.D & 0xFF;
      set_flags(ocpu.DL);
      break;
    case REG_DH:
      ocpu.D = ((value & 0xFF) << 8) | (ocpu.D & 0xFF);
      ocpu.DH = ocpu.D >> 8;
      set_flags(ocpu.DH);
      break;
    case REG_D:
      ocpu.D = word_value;
      set_flags(ocpu.D);
      break;
    case REG_EL:
      ocpu.E = (ocpu.E & 0xFF00) | (value & 0xFF);
      ocpu.EL = ocpu.E & 0xFF;
      set_flags(ocpu.EL);
      break;
    case REG_EH:
      ocpu.E = ((value & 0xFF) << 8) | (ocpu.E & 0xFF);
      ocpu.EH = ocpu.E >> 8;
      set_flags(ocpu.EH);
      break;
    case REG_E:
      ocpu.E = word_value;
      set_flags(ocpu.E);
      break;
    case REG_FL:
      ocpu.F = (ocpu.F & 0xFF00) | (value & 0xFF);
      ocpu.FL = ocpu.F & 0xFF;
      set_flags(ocpu.FL);
      break;
    case REG_FH:
      ocpu.F = ((value & 0xFF) << 8) | (ocpu.F & 0xFF);
      ocpu.FH = ocpu.F >> 8;
      set_flags(ocpu.FH);
      break;
    case REG_F:
      ocpu.F = word_value;
      set_flags(ocpu.F);
      break;
    default:
     	printf("Wrong register: %02X\n", reg);
     	return;
  }
}

bool is_word_reg(byte reg) {
  return (reg == REG_A) || (reg == REG_B) || \
         (reg == REG_C) || (reg == REG_D) || \
         (reg == REG_E) || (reg == REG_F);
}

void ocpu_execute() {
  byte reg;
  byte value;
  word word_value;
  while (1) {
    byte instruction = ocpu_fetch_byte();
    switch(instruction) {
    case INS_MOV_IM:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = ocpu_fetch_byte();
        write_register(reg, value, 0);
      } else {
        word_value = ocpu_fetch_word();
        write_register(reg, 0, word_value); 
      }
      break;
    case INS_MOV_REG:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(ocpu_fetch_byte());
        write_register(reg, value, 0);
      } else {
        word_value = match_register(ocpu_fetch_byte());
        write_register(reg, 0, word_value);
      }
      break;
    case INS_ADD_IM:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) + ocpu_fetch_byte();
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) + ocpu_fetch_word();
        write_register(reg, 0, word_value); 
      }
      break;
    case INS_ADD_REG:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) + match_register(ocpu_fetch_byte());
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) + match_register(ocpu_fetch_byte());
        write_register(reg, 0, word_value);
      }
      break;
    case INS_ADC_IM:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) + ocpu_fetch_byte() + ocpu.CF;
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) + ocpu_fetch_word() + ocpu.CF;
        write_register(reg, 0, word_value); 
      }
      break;
    case INS_ADC_REG:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) + match_register(ocpu_fetch_byte()) + ocpu.CF;
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) + match_register(ocpu_fetch_byte()) + ocpu.CF;
        write_register(reg, 0, word_value);
      }
      break;
    case INS_SUB_IM:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) - ocpu_fetch_byte();
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) - ocpu_fetch_word();
        write_register(reg, 0, word_value); 
      }
      break;
    case INS_SUB_REG:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) - match_register(ocpu_fetch_byte());
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) - match_register(ocpu_fetch_byte());
        write_register(reg, 0, word_value);
      }
      break;
    case INS_MUL_IM:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) * ocpu_fetch_byte();
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) * ocpu_fetch_word();
        write_register(reg, 0, word_value); 
      }
      break;
    case INS_MUL_REG:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) * match_register(ocpu_fetch_byte());
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) * match_register(ocpu_fetch_byte());
        write_register(reg, 0, word_value);
      }
      break;  
    case INS_DIV_IM:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) / ocpu_fetch_byte();
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) / ocpu_fetch_word();
        write_register(reg, 0, word_value); 
      }
      break;
    case INS_DIV_REG:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) / match_register(ocpu_fetch_byte());
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) / match_register(ocpu_fetch_byte());
        write_register(reg, 0, word_value);
      }
      break;
    case INS_INC:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) + 1;
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) + 1; 
        write_register(reg, 0, word_value);
      }
      break;
    case INS_DEC:
      reg = ocpu_fetch_byte();      
      if (!is_word_reg(reg)) {
        value = match_register(reg) - 1;
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) - 1; 
        write_register(reg, 0, word_value);
      }
      break;
    case INS_NOT:
      reg = ocpu_fetch_byte();      
      if (!is_word_reg(reg)) {
        value = ~(match_register(reg));
        write_register(reg, value, 0);
      } else {
        word_value = ~(match_register(reg)); 
        write_register(reg, 0, word_value);
      }
      break;
    case INS_AND_IM:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) & ocpu_fetch_byte();
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) & ocpu_fetch_byte();
        write_register(reg, 0, word_value);
      }
      break;
    case INS_AND_REG:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) & match_register(ocpu_fetch_byte());
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) & match_register(ocpu_fetch_byte());
        write_register(reg, 0, word_value);
      }
      break;
    case INS_OR_IM:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) | ocpu_fetch_byte();
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) | ocpu_fetch_byte();
        write_register(reg, 0, word_value);
      }
      break;
    case INS_OR_REG:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) | match_register(ocpu_fetch_byte());
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) | match_register(ocpu_fetch_byte());
        write_register(reg, 0, word_value);
      }
      break;
    case INS_XOR_IM:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) ^ ocpu_fetch_byte();
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) ^ ocpu_fetch_byte();
        write_register(reg, 0, word_value);
      }
      break;
    case INS_XOR_REG:
      reg = ocpu_fetch_byte();
      if (!is_word_reg(reg)) {
        value = match_register(reg) ^ match_register(ocpu_fetch_byte());
        write_register(reg, value, 0);
      } else {
        word_value = match_register(reg) ^ match_register(ocpu_fetch_byte());
        write_register(reg, 0, word_value);
      }
      break;
    case INS_CMP_IM:
      int reg_value = match_register(ocpu_fetch_byte());
      value = ocpu_fetch_byte();
      if (reg_value == value) {
        ocpu.ZF = 1;
        ocpu.CF = 0;
      } else if (reg_value < value) {
        ocpu.ZF = 0;
        ocpu.CF = 1;
      } else {
        ocpu.ZF = 0;
        ocpu.CF = 0;
      }
      break;
    case INS_CMP_REG:
      int reg1_value = match_register(ocpu_fetch_byte());
      int reg2_value = match_register(ocpu_fetch_byte());
      if (reg1_value == reg2_value) {
        ocpu.ZF = 1;
        ocpu.CF = 0;
      } else if (reg1_value < reg2_value) {
        ocpu.ZF = 0;
        ocpu.CF = 1;
      } else {
        ocpu.ZF = 0;
        ocpu.CF = 0;
      }
      break;
    case INS_PUSH_IM:
      push(ocpu_fetch_byte());
      printf("STACK: push 0x%x to stack address 0x%x\n", get_last_item_from_stack(), ocpu.SP);
      break;
    case INS_POP:
      printf("STACK: pop 0x%x from stack address 0x%x\n", pop(), ocpu.SP);
      break;
    case INS_OCPU_NOP:
      break;
    case INS_OCPU_SEC:
      ocpu.CF = 1;
      break;
    case INS_OCPU_CLC:
      ocpu.CF = 0;
      break;
    case 0x00:
      return;
    default:
      printf("Unrecognized instruction: %04X\n", instruction);
      return;
    }
  }
}

void initOCPU(FILE *infile) {
  ocpu_reset();
  int ch;
  int pos = 0;
  stack = malloc(stack_size*sizeof(byte));
  ocpu.SP = 0;
  sp = &ocpu.SP;
  while (1) { // FIXME
    ch = fgetc(infile);
    if (ch == EOF)
      break;
    ocpu_mem.memory[pos] = ch;
    pos++;
  }
      
  ocpu_execute();
  ocpu_dump_ocpu(ocpu);
  free(stack);
  return;
}