#include <cpu/cpu.h>

#include <iostream>
#include <sstream>
#include <cstring>
#include <iomanip>

void Cpu_state::setAF(u16 v)
{
  A         = (u8)(v >> 8);
  zero      = (v & 0x80 != 0);
  subtract  = (v & 0x40 != 0);
  halfcarry = (v & 0x20 != 0);
  carry     = (v & 0x10 != 0);
}

u16 Cpu_state::getAF()
{
  return ((u16)A << 8) |
    (zero      ? 0x80 : 0) |
    (subtract  ? 0x40 : 0) |
    (halfcarry ? 0x20 : 0) |
    (carry     ? 0x10 : 0);
}

u8& Cpu_state::reg8(Cpu& cpu, u8 idx)
{
  switch(idx)
    {
    case 0x0: return B; break;
    case 0x1: return C; break;
    case 0x2: return D; break;
    case 0x3: return E; break;
    case 0x4: return H; break;
    case 0x5: return L; break;
    case 0x7: return A; break;
    default: throw CpuException(cpu, "Invalid 8-bit register access");
    }
}

u16& Cpu_state::reg16(Cpu& cpu, u8 idx)
{
  switch(idx)
    {
    case 0x0: return BC; break;
    case 0x1: return DE; break;
    case 0x2: return HL; break;
    case 0x3: return SP; break;
    default: throw CpuException(cpu, "Invalid 16-bit register access");
    }
}

Cpu::Cpu(Mem_device* bus)
  : bus(bus)
{
  reset();
}

void Cpu::step()
{
  if(state.stopped) return;

  if(!handleInterrupts()) // if no isr has been called, decode an instruction
    {
      if (state.halted)
        processed_mcycles += 4;
      else
        executeInstruction();
    }
}

void Cpu::reset()
{
  state.BC = 0;
  state.DE = 0;
  state.HL = 0;
  state.A = 0;
  state.SP = 0;
  state.PC = 0;

  state.zero = false;
  state.subtract = false;
  state.halfcarry = false;
  state.carry = false;

  state.IME = IME_OFF;
  state.IE = 0;
  state.IF = 0;

  state.halted = false;
  state.haltbug = false;
  state.stopped = false;
}

u8 Cpu::readPC8()
{
  u8 data = bus->read8(state.PC);
  if(!state.haltbug)
    state.PC++;
  else
    state.haltbug = false;
  return data;
}

u16 Cpu::readPC16()
{
  u16 data;
  if(state.haltbug)
    {
      data = bus->read8(state.PC);
      data |= data << 8; // Same byte twice
      state.PC++;
      state.haltbug = false;
    }
  else
    {
      data = bus->read16(state.PC);
      state.PC+=2;
    }
  return data;
}

void Cpu::pushStack8(u8 data)
{
  state.SP--;
  bus->write8(state.SP, data);
}

u8 Cpu::popStack8()
{
  u8 data = bus->read8(state.SP);
  state.SP++;
  return data;
}

void Cpu::pushStack16(u16 data)
{
  state.SP-=2;
  bus->write16(state.SP,data);
}

u16 Cpu::popStack16()
{
  u16 data = bus->read16(state.SP);
  state.SP+=2;
  return data;
}

void Cpu::doCall(u16 target)
{
  pushStack16(state.PC);
  state.PC = target;
}

void Cpu::doRet()
{
  state.PC = popStack16();
}

void Cpu::signalInterrupt(InterruptType it)
{
  state.IF |= it;
}

bool Cpu::decodeCond(u8 cc)
{
  switch(cc)
    {
    case COND_NZ: return !state.zero; break;
    case COND_Z:  return state.zero; break;
    case COND_NC: return !state.carry; break;
    case COND_C:  return state.carry; break;
    }

  return false;
}

void Cpu::aluop8(AluOp op, u8 lhs, u8 rhs, u8& out, bool update_carry)
{
  u16 rhs16 = rhs;
  u16 res16;
  u8 res;

  if ((op == ADC || op == SBC) && state.carry)
    rhs16++;

  u16 lhs_lower = lhs & 0x0F;
  u16 lhs_upper = lhs & 0xF0;
  u16 rhs_lower = rhs16 & 0x0F;
  u16 rhs_upper = rhs16 & 0x1F0;

  switch(op)
    {
    case ADD:
    case ADC:
      res16 = lhs_lower + rhs_lower;
      break;
    case SUB:
    case SBC:
    case CP:
      res16 = lhs_lower - rhs_lower;
      break;
    case AND:
      res16 = lhs_lower & rhs_lower;
      break;
    case OR:
      res16 = lhs_lower | rhs_lower;
      break;
    case XOR:
      res16 = lhs_lower ^ rhs_lower;
      break;
    }

  state.halfcarry = (res16 & 0x10 != 0) || op == AND;
  state.subtract = (op == SUB) || (op == SBC) || (op == CP);

  switch(op)
    {
    case ADD:
    case ADC:
      res16 += lhs_upper + rhs_upper;
      break;
    case SUB:
    case SBC:
    case CP:
      res16 += lhs_upper - rhs_upper;
      break;
    case AND:
      res16 |= lhs_upper & rhs_upper;
      break;
    case OR:
      res16 |= lhs_upper | rhs_upper;
      break;
    case XOR:
      res16 |= lhs_upper ^ rhs_upper;
      break;
    }

  res = (u8)(res16 & 0xFF);

  if(update_carry)
    state.carry = (res16 & 0x100 != 0);

  state.zero = (res == 0);

  if (op != CP)
    out = res;
}

void Cpu::add16(u16& out, u16 lhs, u16 rhs)
{
  u16 res11 = (lhs & 0x0FFF) + (rhs & 0x0FFF);
  state.halfcarry = (res11 & 0x1000);
  u32 res32 = lhs + rhs;
  state.carry = (res32 & 0x10000);
  state.subtract = false;
  lhs = (u16)res32;
}

bool Cpu::handleInterrupts()
{
  // Once there's an interrupt we exit halt mode
  if (state.SI())
    state.halted = false;

  if (state.IME == IME_SCHEDULED)
    state.IME = IME_ON;
  else if (state.IME == IME_ON && state.SI() != 0)
    {
      u16 isr;
      InterruptType it;

      if      (state.SI() & INT_VBlank)  { it = INT_VBlank;  isr = 0x40; }
      else if (state.SI() & INT_LCDSTAT) { it = INT_LCDSTAT; isr = 0x48; }
      else if (state.SI() & INT_Timer)   { it = INT_Timer;   isr = 0x50; }
      else if (state.SI() & INT_Serial)  { it = INT_Serial;  isr = 0x58; }
      else if (state.SI() & INT_Joypad)  { it = INT_Joypad;  isr = 0x60; }
      else throw CpuException(*this, "Unable to find interrupt");

      state.IME = IME_OFF; // Disable IME
      state.IF &= ~it; // clear interrupt in IF
      doCall(isr);     // Call interrupt service routine

      processed_mcycles += 5;

      return true;
    }

  return false;
}

CpuException::CpuException(Cpu& cpu, const char* msg)
  : EmulatorException(msg), state(cpu.state), instaddr(cpu.last_instr_addr)
{
  for(u16 offset; offset < 4; offset++)
    instmem[offset] = cpu.bus->read8(cpu.last_instr_addr + offset);
}

const char* CpuException::what() const noexcept
{
  std::stringstream s;

#define FORMAT16(x) std::uppercase << std::hex << std::setfill('0') << std::setw(4) << x
#define FORMAT8(x) std::uppercase << std::hex << std::setfill('0') << std::setw(2) << ((unsigned)(x))

  s << "CpuException: " << std::runtime_error::what() << std::endl
    << "Last Instruction @" << FORMAT16(instaddr) << " : "
    << FORMAT8(instmem[0]) << " "
    << FORMAT8(instmem[1]) << " "
    << FORMAT8(instmem[2]) << " "
    << FORMAT8(instmem[3]) << std::endl
    << "Registers:" << std::endl
    << "  A=" << FORMAT8(state.A) << " Z=" << state.zero << " N=" << state.subtract << " H=" << state.halfcarry << " C=" << state.carry
    << " IME=" << state.IME << " IE=" << FORMAT8(state.IE) << " IF=" << FORMAT8(state.IF) << std::endl
    << "  BC=" << FORMAT16(state.BC) << " DE=" << FORMAT16(state.DE) << " HL=" << FORMAT16(state.HL) << " SP=" << FORMAT16(state.SP) << std::endl
    << "  PC=" <<  FORMAT16(state.PC)
    << " HALT=" << state.halted << " HALTBUG=" << state.haltbug << " STOP=" << state.stopped;

  const std::string str = s.str();
  char* buf = new char[str.length()];
  std::strcpy(buf, str.c_str());
  return buf;
}