#include "vcpu.h"
#include "mem.h"
#include "screen.h"
#include "keypad.h"

#include <stdio.h>
#include <stdlib.h>
#include <time.h>

// Current VCPU state
VCPU_State State;

void VCPUInit(){
  State.PC=ADDR_ROM;
  State.S=0;
  srand(time(NULL));
}

void VCPUFetch(){
  unsigned char byte[2];
  MemLoad(byte,2,State.PC); // Little indian to -1 no +1
  State.opcode=byte[0];
  State.opcode=State.opcode<<8;
  State.opcode=State.opcode | byte[1];
  State.PC+=2;
}

void VCPUDecode(){
  State.X=(State.opcode>>8) & 0xF;
  State.Y=(State.opcode>>4) & 0xF;
  State.N=State.opcode & 0x0F;
  State.NN=State.opcode & 0xFF;
  State.NNN=State.opcode & 0x0FFF;
}

void VCPUDoubleDabble(unsigned char x, unsigned char *u, unsigned char *t, unsigned char *h){
  unsigned int bcd=x;
  for(int i=0;i<8;i++){
    bcd=bcd<<1;
    unsigned char byte=bcd & 0xFF;
    unsigned char units=(bcd>>8) & 0xF;
    unsigned char tens=(bcd>>12) & 0xF;
    unsigned char hundreds=(bcd>>16) & 0xF;
    //printf("hundreds:%04b tens:%04b units:%04b byte:%08b\n",hundreds,tens,units,byte);
    if(i<7){
      if(units>4)
        units+=3;
      if(tens>4)
        tens+=3;
      if(hundreds>4)
        hundreds+=3;
    }
    bcd = (hundreds<<16) | (tens << 12) | (units << 8) | byte;
  }
  *u=bcd>>8 & 0xF;
  *t=bcd>>12 & 0xF;
  *h=bcd>>16 & 0xF;
}

void VCPUExecute(){
  //  VCPUDump();
  switch(State.opcode >> 12){
  case 0x0: // Clear screen or return from subroutine
    if(State.N == 0x0){ // Clear screen
      ScreenClear();
    }
    else if(State.N == 0xE) { // Return from subroutine
      State.PC=State.stack[State.S];
      State.S--;
    }
    break;
    
  case 0x1: // Jump
    State.PC=State.NNN;
    break;
    
  case 0x2: // Call
    State.S++;
    State.stack[State.S]=State.PC;
    State.PC=State.NNN;
    break;
      
  case 0x3: // SE: VX, byte
    if(State.V[State.X]==State.NN)
      State.PC+=2;
    break;
    
  case 0x4: // SNE: VX, byte
    if(State.V[State.X]!=State.NN)
      State.PC+=2;
    break;
    
  case 0x5: // SE: VX, VY
    if(State.N == 0){
      if(State.V[State.X]==State.V[State.Y])
        State.PC+=2;
    }
    break;
    
  case 0x6:
    State.V[State.X]=State.NN;
    break;
      
  case 0x7:
    State.V[State.X]=State.V[State.X] + State.NN;
    break;
    
  case 0x8: // Register operations
    switch(State.N){
    case 0x0: // VX = VY
      State.V[State.X]=State.V[State.Y];
      break;
      
    case 0x1: // VX = VX OR VY
      State.V[State.X]=State.V[State.X] | State.V[State.Y];
      break;
      
    case 0x2: // VX = VX AND VY
      State.V[State.X]=State.V[State.X] & State.V[State.Y];
      break;
      
    case 0x3: // VX = VX XOR VY
      State.V[State.X]=State.V[State.X] ^ State.V[State.Y];
      break;
      
    case 0x4: // VX = VX + VY
      unsigned char x=State.V[State.X];
      unsigned char y=State.V[State.Y];
      State.V[State.X]=x+y;
      State.V[REG_FLAG]=((x+y) > 255);
      break;
      
    case 0x5: // VX = VX - VY
      unsigned char x2=State.V[State.X];
      unsigned char y2=State.V[State.Y];
      State.V[State.X]=x2-y2;
      State.V[REG_FLAG]=(x2>=y2);
      break;
      
    case 0x6: // VX = VX SHR 1
      char flag=State.V[State.X] & 0x1 == 1;
      State.V[State.X]=State.V[State.X] >> 1;
      State.V[REG_FLAG]=flag;
      break;
      
    case 0x7: // VX = VY - VX
      unsigned char x3=State.V[State.X];
      unsigned char y3=State.V[State.Y];
      State.V[State.X]=y3-x3;
      State.V[REG_FLAG]=(y3>=x3);
      break;
      
    case 0xE: // VX = VX SHL 1
      char flag2=State.V[State.X] >> 7 == 1;
      State.V[State.X]=State.V[State.X] << 1;
      State.V[REG_FLAG]=flag2;
      break;
      
    }    
    break;
    
  case 0x9: // SNE: VX, VY
    if(State.N==0){
      if(State.V[State.X]!=State.V[State.Y])
        State.PC+=2;
    }
    break;
    
  case 0xA:
    State.I=State.NNN;
    break;
    
  case 0xB:
    State.PC=State.V[0]+State.NNN;
    break;
    
  case 0xC:
    unsigned short n = rand() % 255 + 1;
    State.V[State.X]=n & State.NN;
    break;
      
  case 0xD: // Draw a sprite
    State.V[REG_FLAG]=0; // Set flag to 0
    int width, height;
    ScreenWH(&width,&height);
    int X=State.V[State.X]%width;
    int Y=State.V[State.Y]%height;
    for(char row=0;row<State.N;row++){
      // Stop if row out of screen
      if(Y+row>=height)
        break;
      unsigned char sprite;
      MemLoad(&sprite,1,State.I+row); // Load sprite
      // Draw sprite
      for(int shift=0;shift<8;shift++){
        // Stop if column is out of screen
        if(X+shift >= width)
          break;
        if(ScreenPixelApply(X+shift,Y+row,(sprite>>(7-shift))&0x1))
          State.V[REG_FLAG]=1;
      }
    }
    break;
    
  case 0xE:
    if(State.NN==0x9E){ // Skip if keypress in VX
      if(KeypadIsPressed(State.V[State.X]&0x0F)){
        State.PC+=2;
      }
    }else if(State.NN==0xA1){ // Skip if not keypress in VX
      if(!KeypadIsPressed(State.V[State.X]&0x0F))
        State.PC+=2;
    }
    break;
    
  case 0xF:
    switch(State.NN){
    case 0x07: // Get timer
      State.V[State.X]=State.DT;
      break;
      
    case 0x0A:
      int key=KeypadGetPressed();
      if(key >= 0){
        State.V[State.X]=key&0x0F;
      }
      else
        State.PC-=2; // Go back to last instruction (loop until key is pressed)
        
      break;
      
    case 0x15: // Set timer
      State.DT=State.V[State.X];
      break;
      
    case 0x18: // Set sound timer
      State.ST=State.V[State.X];
      break;
      
    case 0x1E: // I = I + VX
      State.I=State.I+State.V[State.X];
      break;
      
    case 0x29:
      State.I=ADDR_FONT+5*(State.V[State.X]&0x0F);
      break;
      
    case 0x33:
      unsigned char units, tens, hundreds;
      VCPUDoubleDabble(State.V[State.X],&units,&tens,&hundreds);
      MemStore(&hundreds,1,State.I);
      MemStore(&tens,1,State.I+1);
      MemStore(&units,1,State.I+2);
      //printf("hundreds:%d tens:%d units:%d byte:%d\n",hundreds,tens,units,State.V[State.X]);
      break;
      
    case 0x55:
      MemStore(State.V,State.X+1,State.I);
      break;
      
    case 0x65:
      MemLoad(State.V,State.X+1,State.I);
      break;
      
    }
    break;
  }
}

void VCPUDump(){
  printf("opcode: 0x%04x\n",State.opcode&0xFFFF);
  printf("X: 0x%01x\n",State.X);
  printf("Y: 0x%01x\n",State.Y);
  printf("N: 0x%01x\n",State.N);
  printf("NN: 0x%02x\n",State.NN);
  printf("NNN: 0x%03x\n",State.NNN);
  for(int i=0;i<16;i++){
    printf("V%d: 0x%02x\n",i,State.V[i]);
  }
}