#include "CPU.h"

#include <stdexcept>
#include <iostream>
#include <iomanip>
#include <bitset>
#include <stdlib.h>
#include <array>
#include <cassert>

CPU::CPU(std::shared_ptr<Bus>& bus) : m_Bus(bus), m_IsHalted(false), m_Context({m_Instruction, m_InstructionPointer, m_Flags, m_Registers, m_Bus, m_IsHalted}) {
  m_InstructionPointer = 0x00008000;

  for(int i = 0; i < 8; i++) {
    m_Registers[i] = 0;
  }

  for(int i = 0; i < 8; i++) {
    m_SegmentRegisters[i] = 0;
  }
}

void CPU::Step() {
  FetchDecode();
  Execute();
}

void CPU::Dump() {
  std::cout << "--TRACE-- ";
  std::cout << std::endl;
  
  for(uint8_t i = 0; i < 8; i++)
  {
    std::cout << x86::Register2Str((x86::Register)i) << ": " << m_Registers[i] << " | ";
  }

  std::cout << std::endl;

  std::bitset<32> flags(m_Flags);
  std::cout << "IP: " << std::hex << m_InstructionPointer << " | FLAGS: " << flags;
  
  std::cout << std::endl;
}

void CPU::Reset() { 
  for(uint8_t i = 0; i < 8; i++)
  {
    m_Registers[i] = 0;
    m_SegmentRegisters[i] = 0;
  }
  
  m_InstructionPointer = 0x8000;
  m_Flags = 0;

  std::cout << "[CPU] State Flushed!" << std::endl;
  m_IsHalted = false;
  m_Instruction.m_Opcode = (Opcode)0;
  m_Instruction.m_Operand1 = 0;
  m_Instruction.m_Operand2 = 0;
  m_Instruction.m_Func = nullptr;
  std::memset(m_Instruction.m_Displacement, 0, 4);
  m_Instruction.optional.m_ModRM = (x86::ModRM){ .m_State = x86::ModRMState::INVALID, .m_Reg = 0, .m_Rm = 0 };
}

CPUStatus CPU::GetStatus() {
  return (CPUStatus){.m_Registers = m_Registers, .m_IP = m_InstructionPointer, .m_Instruction = m_Instruction};
}

void CPU::FetchDecode() {
  uint8_t opcode_raw = m_Bus->AccessX<uint8_t>(m_InstructionPointer);
  Opcode opcode = static_cast<Opcode>(opcode_raw);

  auto& instruction_table = GetInstructionTable();
  if(instruction_table[opcode_raw].m_Executor == nullptr) {
    std::cout << "Encoding: " << opcode_raw << std::endl;
    throw std::runtime_error("[CPU] [FetchDecode] Opcode is not in instruction table!");    
  }

  auto& instruction = instruction_table[opcode_raw];

  uint8_t encoding_mask = 0b00001111;
  uint8_t encoding = instruction.m_Encoding & encoding_mask;
  uint8_t immediate = instruction.m_Encoding & ~encoding_mask;

  switch(encoding) {
  case OI:
    if (immediate == I32) {
      m_Instruction.m_Operand1 = m_Bus->AccessX<uint32_t>(m_InstructionPointer + 1);
      m_Instruction.m_Length = 5;
    } else if (immediate = I16) {
      m_Instruction.m_Operand1 = m_Bus->AccessX<uint16_t>(m_InstructionPointer + 1);
      m_Instruction.m_Length = 3;
    } else if (immediate = I8) {
      m_Instruction.m_Operand1 = m_Bus->AccessX<uint8_t>(m_InstructionPointer + 1);
      m_Instruction.m_Length = 2;
    } else {
      throw std::runtime_error("[CPU] [FetchDecode] OI instruction encoding only supports imm8,imm16,imm32.");
    }
    break;
  case ZO:
    m_Instruction.m_Length = 1;
    break;
  case RM:
  case MR:
    m_Instruction.optional.m_ModRM = x86::process_modrm(m_Bus->AccessX<uint8_t>(m_InstructionPointer + 1));
    m_Instruction.m_Length = 2;
    FetchModRMFields();
    break;
  default:
    throw std::runtime_error("[CPU] [FetchDecode] Encoding was not found!");
  }

  m_Instruction.m_Func = instruction.m_Executor;
  m_Instruction.m_Opcode = opcode;
  m_InstructionPointer += m_Instruction.m_Length;
}

void CPU::Execute() {
  uint8_t opcode_value = static_cast<uint8_t>(m_Instruction.m_Opcode);
  
  if(m_Instruction.m_Func != nullptr)
  {
    m_Instruction.m_Func(m_Context);
    return;
  }
  throw std::runtime_error("[CPU] [Execute] m_Func is nullptr!");
}

void CPU::FetchModRMFields() {
  assert(m_Instruction.m_Length != 0); // FetchDecode() must set m_Length before calling FetchModRMFields()
  x86::ModRMState state = m_Instruction.optional.m_ModRM.m_State;
  switch(state) {
  case x86::ModRMState::LR:
  case x86::ModRMState::R:
    break;
  case x86::ModRMState::DISP32:
  case x86::ModRMState::LR_DISP32:
  {
    uint32_t disp = m_Bus->AccessX<uint32_t>(m_InstructionPointer + m_Instruction.m_Length);
    std::memcpy(&m_Instruction.m_Displacement, &disp, 4);
    m_Instruction.m_DisplacementSize = 4;
    m_Instruction.m_Length += 4;
    break;
  }
  case x86::ModRMState::LR_DISP8:
    m_Instruction.m_Displacement[0] = m_Bus->AccessX<uint8_t>(m_InstructionPointer + m_Instruction.m_Length);
    m_Instruction.m_DisplacementSize = 1;
    m_Instruction.m_Length += 1;
    break;
  default:
    throw std::runtime_error("Instruction could not be modified according to the modrm field!");
  }
}