#include "Interpreter.hpp"
#include <cstring>
#include <stdexcept>

namespace chocochip8 {

// converts any 8-bit sprite row to its high-res 16-bit equivalent
constexpr uint16_t gcvLowResToHighResRowLookupTable[256] = {
  0x0000, 0x0003, 0x000C, 0x000F, 0x0030, 0x0033, 0x003C, 0x003F, 0x00C0, 0x00C3, 0x00CC, 0x00CF, 0x00F0, 0x00F3, 0x00FC, 0x00FF, 
  0x0300, 0x0303, 0x030C, 0x030F, 0x0330, 0x0333, 0x033C, 0x033F, 0x03C0, 0x03C3, 0x03CC, 0x03CF, 0x03F0, 0x03F3, 0x03FC, 0x03FF, 
  0x0C00, 0x0C03, 0x0C0C, 0x0C0F, 0x0C30, 0x0C33, 0x0C3C, 0x0C3F, 0x0CC0, 0x0CC3, 0x0CCC, 0x0CCF, 0x0CF0, 0x0CF3, 0x0CFC, 0x0CFF, 
  0x0F00, 0x0F03, 0x0F0C, 0x0F0F, 0x0F30, 0x0F33, 0x0F3C, 0x0F3F, 0x0FC0, 0x0FC3, 0x0FCC, 0x0FCF, 0x0FF0, 0x0FF3, 0x0FFC, 0x0FFF, 
  0x3000, 0x3003, 0x300C, 0x300F, 0x3030, 0x3033, 0x303C, 0x303F, 0x30C0, 0x30C3, 0x30CC, 0x30CF, 0x30F0, 0x30F3, 0x30FC, 0x30FF, 
  0x3300, 0x3303, 0x330C, 0x330F, 0x3330, 0x3333, 0x333C, 0x333F, 0x33C0, 0x33C3, 0x33CC, 0x33CF, 0x33F0, 0x33F3, 0x33FC, 0x33FF, 
  0x3C00, 0x3C03, 0x3C0C, 0x3C0F, 0x3C30, 0x3C33, 0x3C3C, 0x3C3F, 0x3CC0, 0x3CC3, 0x3CCC, 0x3CCF, 0x3CF0, 0x3CF3, 0x3CFC, 0x3CFF, 
  0x3F00, 0x3F03, 0x3F0C, 0x3F0F, 0x3F30, 0x3F33, 0x3F3C, 0x3F3F, 0x3FC0, 0x3FC3, 0x3FCC, 0x3FCF, 0x3FF0, 0x3FF3, 0x3FFC, 0x3FFF, 
  0xC000, 0xC003, 0xC00C, 0xC00F, 0xC030, 0xC033, 0xC03C, 0xC03F, 0xC0C0, 0xC0C3, 0xC0CC, 0xC0CF, 0xC0F0, 0xC0F3, 0xC0FC, 0xC0FF, 
  0xC300, 0xC303, 0xC30C, 0xC30F, 0xC330, 0xC333, 0xC33C, 0xC33F, 0xC3C0, 0xC3C3, 0xC3CC, 0xC3CF, 0xC3F0, 0xC3F3, 0xC3FC, 0xC3FF, 
  0xCC00, 0xCC03, 0xCC0C, 0xCC0F, 0xCC30, 0xCC33, 0xCC3C, 0xCC3F, 0xCCC0, 0xCCC3, 0xCCCC, 0xCCCF, 0xCCF0, 0xCCF3, 0xCCFC, 0xCCFF, 
  0xCF00, 0xCF03, 0xCF0C, 0xCF0F, 0xCF30, 0xCF33, 0xCF3C, 0xCF3F, 0xCFC0, 0xCFC3, 0xCFCC, 0xCFCF, 0xCFF0, 0xCFF3, 0xCFFC, 0xCFFF, 
  0xF000, 0xF003, 0xF00C, 0xF00F, 0xF030, 0xF033, 0xF03C, 0xF03F, 0xF0C0, 0xF0C3, 0xF0CC, 0xF0CF, 0xF0F0, 0xF0F3, 0xF0FC, 0xF0FF, 
  0xF300, 0xF303, 0xF30C, 0xF30F, 0xF330, 0xF333, 0xF33C, 0xF33F, 0xF3C0, 0xF3C3, 0xF3CC, 0xF3CF, 0xF3F0, 0xF3F3, 0xF3FC, 0xF3FF, 
  0xFC00, 0xFC03, 0xFC0C, 0xFC0F, 0xFC30, 0xFC33, 0xFC3C, 0xFC3F, 0xFCC0, 0xFCC3, 0xFCCC, 0xFCCF, 0xFCF0, 0xFCF3, 0xFCFC, 0xFCFF, 
  0xFF00, 0xFF03, 0xFF0C, 0xFF0F, 0xFF30, 0xFF33, 0xFF3C, 0xFF3F, 0xFFC0, 0xFFC3, 0xFFCC, 0xFFCF, 0xFFF0, 0xFFF3, 0xFFFC, 0xFFFF
};

// 4x5 sprites for hex digits 0-F
constexpr uint8_t gcvLowResFontData[80] = {
  0xF0, 0x90, 0x90, 0x90, 0xF0,
  0x20, 0x60, 0x20, 0x20, 0x70,
  0xF0, 0x10, 0xF0, 0x80, 0xF0,
  0xF0, 0x10, 0xF0, 0x10, 0xF0,
  0x90, 0x90, 0xF0, 0x10, 0x10,
  0xF0, 0x80, 0xF0, 0x10, 0xF0,
  0xF0, 0x80, 0xF0, 0x90, 0xF0,
  0xF0, 0x10, 0x20, 0x40, 0x40,
  0xF0, 0x90, 0xF0, 0x90, 0xF0,
  0xF0, 0x90, 0xF0, 0x10, 0xF0,
  0xF0, 0x90, 0xF0, 0x90, 0x90,
  0xE0, 0x90, 0xE0, 0x90, 0xE0,
  0xF0, 0x80, 0x80, 0x80, 0xF0,
  0xE0, 0x90, 0x90, 0x90, 0xE0,
  0xF0, 0x80, 0xF0, 0x80, 0xF0,
  0xF0, 0x80, 0xF0, 0x80, 0x80
};

Interpreter::Interpreter(unsigned ticksPerSecond, Display &display, Buzzer &buzzer, Keypad &keypad):
  mvMemory(scMemorySize),
  mCallStack{},
  mrDisplay{display},
  mrBuzzer{buzzer},
  mrKeypad{keypad},
  mcTicksPerSecond{ticksPerSecond},
  mvSpecialReg{},
  mvReg{},
  mIsHighResMode{false} {
  // Font sprite data goes into the interpreter reserved memory area
  memcpy(mvMemory.data() + scLowRestFontAddr, gcvLowResFontData, sizeof(gcvLowResFontData));
  // Initialize any non-zero registers at startup
  mvSpecialReg[SR_PC] = scResetVector;
}

void Interpreter::tick() {
  // decodes all possible machine code arithmetic opcodes
  constexpr Opcode opcodeMap[16] = {
    Opcode::SET, Opcode::OR,  Opcode::AND, Opcode::XOR,
    Opcode::ADD, Opcode::SUB, Opcode::RSH, Opcode::SUB2,
    Opcode::UNIMPL, Opcode::UNIMPL, Opcode::UNIMPL, Opcode::UNIMPL,
    Opcode::UNIMPL, Opcode::UNIMPL, Opcode::LSH, Opcode::UNIMPL,
  };

  // fetch instruction
  sreg_t iInstAddr = mvSpecialReg[SR_PC];
  unsigned inst = (mvMemory.at(iInstAddr) << 8) | mvMemory.at(iInstAddr + 1);

  // increment program counter
  mvSpecialReg[SR_PC] += 2;

  // extract fields
  unsigned iRegDst = (inst & 0x0F00) >> 8; // destination register index
  unsigned iRegSrc = (inst & 0x00F0) >> 4; // source register index
  unsigned opcode  = (inst & 0x000F);      // arithmetic opcode or sprite index
  unsigned imm8    = (inst & 0x00FF);      // 8-bit immediate
  unsigned imm12   = (inst & 0x0FFF);      // 12-bit immediate

  switch(inst & 0xF000) {
  case 0x0000: // 0NNN - call machine language routine
    switch(inst) {
    case 0x00E0: // clear display
      mrDisplay.clear();
      break;
    case 0x00EE: // return from subroutine
      mvSpecialReg[SR_PC] = mCallStack.top();
      mCallStack.pop();
      break;
    default:
      throw std::invalid_argument("not implemented");
      break;
    }
    break;
  case 0x1000: // 1NNN - unconditional jump
    mvSpecialReg[SR_PC] = imm12;
    break;
  case 0x2000: // 2NNN - call subroutine
    mCallStack.push(mvSpecialReg[SR_PC]);
    mvSpecialReg[SR_PC] = imm12;
    break;
  case 0x3000: // 3XNN - skip if equal immediate
    executeArithmetic(Opcode::JEQ, iRegDst, imm8);
    break;
  case 0x4000: // 4XNN - skip if nonequal immediate
    executeArithmetic(Opcode::JNEQ, iRegDst, imm8);
    break;
  case 0x5000: // 5XY0 - skip if equal
    executeArithmetic(Opcode::JEQ, iRegDst, mvReg[iRegSrc]);
    break;
  case 0x6000: // 6XNN - load immediate
    executeArithmetic(Opcode::SET, iRegDst, imm8);
    break;
  case 0x7000: // 7XNN - increment
    executeArithmetic(Opcode::ADD, iRegDst, imm8);
    break;
  case 0x8000: // 8XNN - general arithmetic
    executeArithmetic(opcodeMap[opcode], iRegDst, mvReg[iRegSrc]); 
    break;
  case 0x9000: // 9XY0 - skip if nonequal
    executeArithmetic(Opcode::JNEQ, iRegDst, mvReg[iRegSrc]);
    break;
  case 0xA000: // ANNN - load I
    mvSpecialReg[SR_I] = imm12;
    break;
  case 0xB000: // BNNN - jump indirect
    mvSpecialReg[SR_I] = mvReg[R_V0] + imm12;
    break;
  case 0xC000: // CXNN - load random
    executeArithmetic(Opcode::RAND, iRegDst, imm8);
    break;
  case 0xD000: // DXYN - draw
    executeDraw(mvReg[iRegDst], mvReg[iRegSrc], opcode);
    break;
  case 0xE000: // EX9E, EXA1 - keypad access
    break;
  case 0xF000: // several unique instructions
    switch(inst & 0xF0FF) {
    case 0xF007: // FX07 - read timer register
      mvReg[iRegDst] = (mvSpecialReg[SR_T1] * scTimerFreq + (mcTicksPerSecond - 1)) / mcTicksPerSecond;
      break;
    case 0xF015: // FX15 - set timer register
      mvSpecialReg[SR_T1] = (mcTicksPerSecond * mvReg[iRegDst]) / scTimerFreq;
      break;
    case 0xF018: // FX18 - set sound timer register
      if(mvSpecialReg[SR_T2] == 0 && mvReg[iRegDst] != 0) {
        mrBuzzer.on();
      }
      mvSpecialReg[SR_T2] = (mcTicksPerSecond * mvReg[iRegDst]) / scTimerFreq;
      break;
    case 0xF01E: // FX1E - add to I
      mvSpecialReg[SR_I] += mvReg[iRegDst];
      break;
    case 0xF029: // FX29 - set I to address of font sprite data for digit X
      mvSpecialReg[SR_I] = scLowRestFontAddr + 5 * mvReg[iRegDst];
      break;
    case 0xF033: // FX33 - convert to bcd
      mvMemory.at(mvSpecialReg[SR_I])     = (mvReg[iRegDst] / 100) % 10;
      mvMemory.at(mvSpecialReg[SR_I] + 1) = (mvReg[iRegDst] / 10) % 10;
      mvMemory.at(mvSpecialReg[SR_I] + 2) = mvReg[iRegDst] % 10;
      break;
    case 0xF055: // FX55 - dump registers
      for(int i = 0; i <= iRegDst - R_V0; i++) {
        mvMemory.at(mvSpecialReg[SR_I]++) = mvReg[R_V0 + i];
      }
      break;
    case 0xF065: // FX65 - restore registers
      for(int i = 0; i <= iRegDst - R_V0; i++) {
        mvReg[R_V0 + i] = mvMemory.at(mvSpecialReg[SR_I]++);
      }
      break;
    default:
      throw std::invalid_argument("not implemented");
      break;
    }
    break;
  }

  // decrement timers
  if(mvSpecialReg[SR_T1] > 0) {
    mvSpecialReg[SR_T1] -= 1;
  }
  if(mvSpecialReg[SR_T2] > 0) {
    mvSpecialReg[SR_T2] -= 1;
    if(mvSpecialReg[SR_T2] == 0) {
      mrBuzzer.off();
    }
  }
}

void Interpreter::loadProgram(char const* data, size_t count, size_t where) {
  if(where + count > scMemorySize) {
    throw std::out_of_range("program exceeds memory bounds or capacity");
  }
  memcpy(mvMemory.data() + where, data, count);
}

void Interpreter::executeArithmetic(Opcode opcode, int iReg, reg_t operand) {
  reg_t tmp;
  switch(opcode) {
  case Opcode::SET:  mvReg[iReg]  = operand; break;
  case Opcode::AND:  mvReg[iReg] &= operand; break;
  case Opcode::OR:   mvReg[iReg] |= operand; break;
  case Opcode::XOR:  mvReg[iReg] ^= operand; break;
  case Opcode::RAND: mvReg[iReg]  = rand() & operand; break;
  case Opcode::LSH:
    mvReg[iReg] = operand << 1;
    // VF = shifted out bit
    mvReg[R_VF] = (operand & 0x80) ? 1 : 0;
    break;
  case Opcode::RSH:
    mvReg[iReg] = operand >> 1;
    // VF = shifted out bit
    mvReg[R_VF] = (operand & 0x01) ? 1 : 0;
    break;
  case Opcode::ADD:
    mvReg[iReg] = mvReg[iReg] + operand;
    // VF = 1 if carry occurs, VF = 0 if no carry
    mvReg[R_VF] = (mvReg[iReg] < operand) ? 1 : 0;
    break;
  case Opcode::SUB:
    tmp = mvReg[iReg];
    mvReg[iReg] = mvReg[iReg] - operand;
    // VF = 0 if borrow occurs, VF = 1 if no borrow
    mvReg[R_VF] = (mvReg[iReg] > tmp) ? 0 : 1;
    break;
  case Opcode::SUB2:
    mvReg[iReg] = operand - mvReg[iReg];
    // VF = 0 if borrow occurs, VF = 1 if no borrow
    mvReg[R_VF] = (mvReg[iReg] > operand) ? 0 : 1;
    break;
  case Opcode::JEQ:
    if(mvReg[iReg] == operand) {
      mvSpecialReg[SR_PC] += 2;
    }
    break;
  case Opcode::JNEQ:
    if(mvReg[iReg] != operand) {
      mvSpecialReg[SR_PC] += 2;
    }
    break;
  case Opcode::UNIMPL:
    throw std::invalid_argument("invalid opcode");
    break;
  }
}

void Interpreter::executeDraw(uint8_t x, uint8_t y, uint8_t n) { 
  size_t iMemAddr = mvSpecialReg[SR_I]; // address of the sprite data in memory
  int collisionCount = 0; // number of scanlines in which any pixel changes from on to off

  // coordinates are doubled in low resolution mode
  if(!mIsHighResMode) {
    x *= 2;
    y *= 2;
  }
  x %= gcWidth;
  y %= gcHeight;
  // make x=0 be the right side of the screen instead of the left
  x = gcWidth - x;

  // draw each row of the sprite
  for(int i = 0; i < (mIsHighResMode && n == 0 ? 16 : n); i++) {
    uint16_t spriteRowBits;
    if(mIsHighResMode) {
      // draws an 8xN sprite
      spriteRowBits = mvMemory.at(iMemAddr++) << 8;
      if(n == 0) {
        // draws an 16xN sprite, so fetch another byte from sprite data
        spriteRowBits |= mvMemory.at(iMemAddr++);
      }
    } else {
      // in low-res mode, each sprite pixel draws two on-screen pixels
      spriteRowBits = gcvLowResToHighResRowLookupTable[mvMemory.at(iMemAddr++)];
    }

    // convert to bitset and shift into absolute horizontal position on the screen
    Scanline spriteScanline(spriteRowBits);
    if(x < 16) {
      spriteScanline >>= (16 - x);
    } else {
      spriteScanline <<= (x - 16);
    }

    // we draw one scanline per sprite row in high-res mode, but twice in low-res mode
    for(int j = 0; j < (mIsHighResMode ? 1 : 2); j++) {
      // blit the sprite bitset into the screen
      if(mrDisplay.blit(spriteScanline, y + i * (mIsHighResMode ? 1 : 2) + j) != 0) {
        collisionCount += 1;
      }
    }

    // update state flags
    if(mIsHighResMode) {
      mvReg[R_VF] = collisionCount;  
    } else {
      mvReg[R_VF] = (collisionCount > 0 ? 1 : 0);
    }
  }
}

}; // namespace chochochip8