#include <cpu/cpu.h>
#include <misc/panic.h>
#include <memory/mem_device.h>

static inline u16 make_u16(u8 msb, u8 lsb)
{
  return (((u16)msb << 8) | (u16)lsb);
}

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

void Cpu::executeInstruction()
{
  u16 currentpc = state.PC;
  opcode_t op = readPC8();
  int mcycles = 1;

#if 0
  printf("@0x%04x: opcode %02X\n",currentpc,op);
#endif

  if ((op & 0xC0) == 0x40 && op != 0x76) // LD r, r'; LD r, [HL]; LD [HL], r
    {
      u8 tmp;
      switch(op & 0x07)
        {
        case 0x6: tmp = bus->read8(state.HL); break;
        default: tmp = state.reg8(op & 0x07); break;
        };

      switch((op >> 3) & 0x7)
        {
        case 0x6: bus->write8(state.HL, tmp); break;
        default: state.reg8((op >> 3) & 0x7) = tmp; break;
        }
    }
  else if((op & 0xC7) == 0x06) // LD r, n
    {
      u8 imm = readPC8();

      switch((op >> 3) & 0x7)
        {
        case 0x6: bus->write8(state.HL, imm); break;
        default: state.reg8((op >> 3) & 0x7) = imm; break;
        }
    }
  else if((op & 0xCF) == 0x01) // LD rr, nn
    {
      u16 data = readPC16();
      state.reg16((op >> 4) & 0x3) = data;
      mcycles = 3;
    }
  else if((op & 0xCF) == 0xC5) // PUSH rr
    {
      u16 data;
      switch((op >> 4) & 0x3)
        {
        case 0x3: data = state.getAF(); break;
        default: data = state.reg16((op >> 4) & 0x3);
        }

      pushStack16(data);

      mcycles = 4;
    }
  else if((op & 0xCF) == 0xC1) // POP rr
    {
      u16 data = popStack16();

      switch((op >> 4) & 0x3)
        {
        case 0x3: state.setAF(data); break;
        default: state.reg16((op >> 4) & 0x3) = data; break;
        }

      mcycles = 4;
    }
  else if((op & 0xC0) == 0x80) // ADD, ADC, SUB, SBC, CP, AND, OR, XOR
    {
      AluOp aluop = (AluOp)((op >> 3) & 0x3);
      u8 reg = op & 0x7;

      u8 rhs;
      switch(reg)
        {
        case 0x6: rhs = bus->read8(state.HL); mcycles = 2; break;
        default: rhs = state.reg8(reg); break;
        }

      aluop8(aluop, rhs);
    }
  else if((op & 0xC7) == 0xC6) // ADD n, ADC n, SUB n, SBC n, AND n, XOR n, OR n, CP n
    {
      AluOp aluop = (AluOp)((op >> 3) & 0x3);
      aluop8(aluop, readPC8());
      mcycles = 2;
    }
  else if((op & 0xC6) == 0x04) // INC r; INC [HL]; DEC r; DEC [HL];
    {
      AluOp aluop = (op & 0x1) ? SUB : ADD;

      switch((op >> 3) & 0x7)
        {
        case 0x6:
          {
            u8 tmp = bus->read8(state.HL);
            aluop8(aluop, tmp, 1, tmp, false);
            bus->write8(state.HL, tmp);
            mcycles = 3;
          }
          break;
        default:
          {
            u8& reg = state.reg8((op >> 3) & 0x7);
            aluop8(aluop, reg, 1, reg, false); break;
          }
          break;
        }
    }
  else if((op & 0xC7) == 0x03) // INC rr; DEC rr
    {
      state.reg16((op >> 4) & 0x3) += ((op & 0x08) ? -1 : 1);
      mcycles = 2;
    }
  else if((op & 0xE7) == 0xC2) // JP cc, nn:
    {
      u16 nn = readPC16();

      if (decodeCond((op >> 3) && 0x3))
        {
          state.PC = nn;
          mcycles = 4;
        }
      else
        mcycles = 3;
    }
  else if((op & 0xE7) == 0x20) // JR cc, e
    {
      s8 e = readPC8();

      bool cond;
      if (decodeCond((op >> 3) & 0x3))
        {
          state.PC += e;
          mcycles = 3;
        }
      else
        mcycles = 2;
    }
  else if((op & 0xE7) == 0xC4) // CALL cc, nn
    {
      u16 nn = readPC16();

      if(decodeCond((op >> 3) & 0x3))
        {
          doCall(nn);
          mcycles = 6;
        }
      else
        mcycles = 3;
    }
  else if((op & 0xCF) == 0x09) // ADD HL, rr
    {
      add16(*this, state.HL, state.HL, state.reg16((op >> 4) & 0x3));
      mcycles = 2;
    }
  else if((op & 0xE7) == 0xC0) // RET cc
    {
      if(decodeCond((op >> 3) & 0x3))
        {
          doRet();
          mcycles = 5;
        }
      else
        mcycles = 2;
    }
  else if((op & 0xC7) == 0xC7) // RST
    {
      u16 rst_addr = op & 0x38;
      doCall(rst_addr);
    }
  else if(op == 0xCB) // PREFIX
    {
      currentpc = state.PC;
      opcode_t prefix_op = readPC8();

#if 0
      printf("@0x%04x: CB opcode %02X\n", currentpc, prefix_op);
#endif

      u8 reg = prefix_op & 0x7;

      u8 data;
      switch(reg)
        {
        case 0x6: data = bus->read8(state.HL); mcycles = 3; break;
        default: data = state.reg8(reg); mcycles = 2; break;
        }

      // For BIT, RES, SET
      u8 bit = (prefix_op >> 3) & 0x7;

      switch(prefix_op & 0xC0)
        {
        case 0x00:
          switch(prefix_op & 0xF1)
            {
            case 0x00: // RLC
              {
                bool msb_set = (data & 0x80);
                data = (data << 1) | (msb_set ? 0x1 : 0x0);
                state.carry = msb_set;
              }
              break;
            case 0x08: // RRC
              {
                bool lsb_set = (data & 0x01);
                data = (data >> 1) | (lsb_set ? 0x80 : 0x00);
                state.carry = lsb_set;
              }
              break;
            case 0x10: // RL
              {
                bool msb_set = (data & 0x80);
                data = (data << 1) | (state.carry ? 0x1 : 0x0);
                state.carry = msb_set;
              }
              break;
            case 0x18: // RR
              {
                bool lsb_set = (data & 0x01) != 0;
                data = (data >> 1) | (state.carry ? 0x80 : 0x00);
                state.carry = lsb_set;
              }
              break;
            case 0x20: // SLA
              state.carry = (data & 0x80);
              data = (data << 1);
              break;
            case 0x28: // SRA
              state.carry = (data & 0x01);
              data = (data >> 1) | (data & 0x80);
              break;
            case 0x30: // SWAP
              data = ((data >> 4) & 0x0F) | ((data << 4) & 0xF0);
              state.carry = false;
              break;
            case 0x38: // SRL
              state.carry = (data & 0x01);
              data = (data >> 1);
              break;
            }
          state.halfcarry = false;
          state.subtract = false;
          state.zero = (data == 0);
          break;
        case 0x40: // BIT
          state.zero = (data & (1 << bit)) == 0;
          state.subtract = false;
          state.halfcarry = true;
          break;
        case 0x80: // RES
          data &= ~(1 << bit);
          break;
        case 0xC0: // SET
          data |= (1 << bit);
          break;
        }

      // All ops except for BIT write the data back to where it came from
      if ((prefix_op & 0xC0) != 0x40)
        {
          switch(reg)
            {
            case 0x6: bus->write8(state.HL, data); mcycles = 4; break;
            default: state.reg8(reg) = data; break;
            }
        }
    }
  else
    {
      switch(op)
        {
        case 0xD3: // Undefined, treat as NOP
        case 0xE3:
        case 0xE4:
        case 0xF4:
        case 0xDB:
        case 0xEB:
        case 0xEC:
        case 0xFC:
        case 0xDD:
        case 0xED:
        case 0xFD:
          break;
        case 0x00: // defined NOP
          break;
        case 0x0A: // Load A, [BC]
          state.A = bus->read8(state.BC);
          mcycles = 2;
          break;
        case 0x1A: // Load A, [DE]
          state.A = bus->read8(state.DE);
          mcycles = 2;
          break;
        case 0x02: // Load [BC], A
          bus->write8(state.BC, state.A);
          mcycles = 2;
          break;
        case 0x12: // Load [DE], A
          bus->write8(state.DE, state.A);
          mcycles = 2;
          break;
        case 0x07: // RLCA
          {
            state.carry = (state.A & 0x80);
            state.A = (state.A << 1) | (state.carry ? 0x01 : 0x00);
            state.halfcarry = false;
            state.subtract = false;
            state.zero = false;
          }
          break;
        case 0x17: // RLA
          {
            bool msb_set = (state.A & 0x80);
            state.A = (state.A << 1) | (state.carry ? 0x01 : 0x00);
            state.carry = msb_set;
            state.halfcarry = false;
            state.subtract = false;
            state.zero = false;
          }
          break;
        case 0x0F: // RRCA
          {
            state.carry = (state.A & 0x01);
            state.A = (state.A >> 1) | (state.carry ? 0x80 : 0x00);
            state.halfcarry = false;
            state.subtract = false;
            state.zero = false;
          }
          break;
        case 0x1F: // RRA
          {
            bool lsb_set = (state.A & 0x01);
            state.A = (state.A >> 1) | (state.carry ? 0x80 : 0x00);
            state.carry = lsb_set;
            state.halfcarry = false;
            state.subtract = false;
            state.zero = false;
          }
          break;
        case 0xFA: // LD A, [nn]
          state.A = bus->read8(readPC16());
          mcycles = 4;
          break;
        case 0xEA: // LD [nn], A
          bus->write8(readPC16(), state.A);
          mcycles = 4;
          break;
        case 0xF2: // LD A, [0xFF : C]
          state.A = bus->read8(make_u16(0xFFu,state.C));
          mcycles = 2;
          break;
        case 0xE2: // LD [0xFF : C], A
          bus->write8(make_u16(0xFFu,state.C), state.A);
          mcycles = 2;
          break;
        case 0xF0: // LD A, [0xFF : n]
          state.A = bus->read8(make_u16(0xFFu,readPC8()));
          mcycles = 3;
          break;
        case 0xE0: // LD [0xFF : n], A
          bus->write8(make_u16(0xFFu,readPC8()), state.A);
          mcycles = 3;
          break;
        case 0x3A: // LD A, [HL-]
          state.A = bus->read8(state.HL); state.HL--; mcycles = 2; break;
        case 0x2A: // LD A, [HL+]
          state.A = bus->read8(state.HL); state.HL++; mcycles = 2; break;
        case 0x32: // LD [HL-], A
          bus->write8(state.HL, state.A); state.HL--; mcycles = 2; break;
        case 0x22: // LD [HL-], A
          bus->write8(state.HL, state.A); state.HL++; mcycles = 2; break;
        case 0x08: // LD [nn], SP
          bus->write16(readPC16(), state.SP); mcycles = 5; break;
        case 0xF9: // LD SP, HL
          state.SP = state.HL; mcycles = 2; break;
        case 0x3F: // CCF complement carry flag
          state.carry = !state.carry;
          state.subtract = false;
          state.halfcarry = false;
          break;
        case 0x37: // SCF Set carry flag
          state.carry = true;
          state.subtract = false;
          state.halfcarry = false;
          break;
        case 0x27: // DAA
          {
            u16 corr = 0;
            if (state.halfcarry || ((state.A & 0x0F) > 0x9))
              corr |= 0x06;
            if ((state.A & 0xF0) > 0x90)
              corr |= 0x06;

            u32 res16 = (u16)state.A + (state.subtract ? (-corr) : corr);
            state.A = (u8)res16;
            state.halfcarry = false;
            state.zero = (state.A == 0);
            state.carry = (res16 & 0x100);
          }
          break;
        case 0x2F: // CPL Complement accumulator
          state.A = ~state.A;
          state.subtract = true;
          state.halfcarry = true;
          break;
        case 0xC3: // JP nn
          state.PC = readPC16();
          mcycles = 4;
          break;
        case 0xE9: // JP HL
          state.PC = state.HL;
          break;
        case 0x18: // JR e
          state.PC += (s8)readPC8();
          break;
        case 0xCD: // CALL nn
          doCall(readPC16());
          mcycles = 6;
          break;
        case 0xC9: // RET
          doRet();
          mcycles = 4;
          break;
        case 0xD9: // RETI
          doRet();
          state.IME = IME_SCHEDULED;
          mcycles = 4;
          break;
        case 0xF3: // DI
          state.IME = IME_OFF;
          break;
        case 0xFB: // EI
          state.IME = IME_SCHEDULED;
          break;
        case 0x76: // HALT
          state.halted = true;
          break;
        case 0x10: // STOP n8
          (void)readPC8();
          state.stopped = true;
          break;
        case 0xE8: // ADD SP, e8
          add16(*this, state.SP, state.SP, (s8)readPC8());
          state.zero = false;
          mcycles = 4;
          break;
        case 0xF8: // LD HL, SP + e8
          add16(*this, state.HL, state.SP, (s32)((s8)readPC8()));
          state.zero = false;
          mcycles = 3;
          break;

        default:
          panic("Unknown opcode 0x%x\n",op);
        }
    }

  processed_mcycles += mcycles;
}