#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define i2c_Address 0x3c
#define SCREEN_WIDTH 128  // OLED display width, in pixels
#define SCREEN_HEIGHT 64  // OLED display height, in pixels
#define OLED_RESET -1     //   QT-PY / XIAO
Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);

#define left 12
#define right 13
#define up 27
#define down 14
#define d0 2
#define d1 4
#define d2 5
#define d3 18
#define d4 19
#define d5 15
#define d6 23
#define d7 26

// Hardware elements:
uint8_t PC;           //program counter
uint8_t AR, BR, IR;   //registers
uint8_t MAR;          //memory address register
uint8_t RAM[256];     //RAM
bool E, C, N, Z, V;   //flags
bool hlt = false;

bool inp[8];
bool enableRamUpdate = false;
int val;

//programming mode
int page = 0;
int pointer = 0;

void setup() {
  Serial.begin(9600);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  pinMode(left, INPUT_PULLUP);
  pinMode(right, INPUT_PULLUP);
  pinMode(up, INPUT_PULLUP);
  pinMode(down, INPUT_PULLUP);
  pinMode(d0, INPUT_PULLUP);
  pinMode(d1, INPUT_PULLUP);
  pinMode(d2, INPUT_PULLUP);
  pinMode(d3, INPUT_PULLUP);
  pinMode(d4, INPUT_PULLUP);
  pinMode(d5, INPUT_PULLUP);
  pinMode(d6, INPUT_PULLUP);
  pinMode(d7, INPUT_PULLUP);
  pinMode(25, INPUT_PULLUP); //mode
  pinMode(33, INPUT_PULLUP); //step
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  // RAM[0x00] = 0x02; RAM[0x01] = 0x01; //lda 01
  // RAM[0x02] = 0x09; RAM[0x03] = 0x00; //sub 00
  // RAM[0x04] = 0x11; RAM[0x05] = 0x07; //brz 07
  // RAM[0x06] = 0x01;                   //hlt
  // RAM[0x07] = 0x04; RAM[0x08] = 0xff; //ldb 255
  // RAM[0x09] = 0x01;                   //hlt
  reset();
}

void loop() {
  if (digitalRead(25)==LOW && digitalRead(33)==LOW) {
    runMode();
    while(digitalRead(33)==LOW){}
  }
  if (digitalRead(25)==LOW) {
    display.display();
    //delay(100);
    display.clearDisplay();
  }

  if (digitalRead(25)==HIGH) {
    programmingMode();
  }

  while(hlt){}
}




void programmingMode(){
  navInput();  //get nav buttons input
  byte addrs = 8*page+pointer;  //ram addrs
  //inputs = ram value at that location
  for (int i=0; i<8; i++){
    inp[i] = bitRead(RAM[addrs], i);
  }
  
  dataInput(); //get data from buttons
  //convert inp array to byte
  byte dataInput = (inp[7]*pow(2,7))+(inp[6]*pow(2,6))+(inp[5]*pow(2,5))+(inp[4]*pow(2,4))+(inp[3]*pow(2,3))+(inp[2]*pow(2,2))+(inp[1]*pow(2,1))+(inp[0]*pow(2,0));
  
  //update ram data only if button pressed
  if (enableRamUpdate){
    RAM[addrs] = dataInput;
    enableRamUpdate = false;
  }
  // Serial.print("dataInput: ");
  // Serial.print(dataInput);
  // Serial.print(", RAM[addrs]: ");
  // Serial.println(RAM[addrs]);
  //display output code
  display.setCursor(0, 0);
  for (int i = 8*page; i < 8*page+8; i++) {
    if (i==(addrs)){
      display.setTextColor(BLACK, WHITE);
    }
    else {
      display.setTextColor(WHITE);
    }
    if (i < 16) {
      display.print("0x0");
    } else {
      display.print("0x");
    }
    display.print(i, HEX);
    display.print(": ");
    String data = "00000000" + String(RAM[i], BIN);
    display.print(data.substring(data.length()-8,data.length()-4));
    display.print("-");
    display.println(data.substring(data.length()-4,data.length()));
  }

  display.display();
  //delay(100);
  display.clearDisplay();
}

void runMode(){
  fetch();
  execute();
  debug();
}



void navInput(){
  if (digitalRead(right)==LOW){
    page++;
    pointer = 0;
    delay(150);
    resetInp();
  }
  if (digitalRead(left)==LOW){
    page--;
    delay(150);
    resetInp();
  }
  if (page>=32){page = 0;}
  if (page<0){page = 31;}

  if (digitalRead(up)==LOW){
    pointer--;
    delay(150);
    resetInp();
  }
  if (digitalRead(down)==LOW){
    pointer++;
    delay(150);
    resetInp();
  }
  if (pointer>7){pointer = 0;}
  if (pointer<0){pointer = 7;}

}

void resetInp(){
  for (int i=0; i<7; i++){
    inp[i]=0;
  }
}

void dataInput() {
  if (digitalRead(d0)==LOW){
    inp[0] = !inp[0];
    delay(150);
    enableRamUpdate = true;
  }
  else if (digitalRead(d1)==LOW){
    inp[1] = !inp[1];
    delay(150);
    enableRamUpdate = true;
  }
  else if (digitalRead(d2)==LOW){
    inp[2] = !inp[2];
    delay(150);
    enableRamUpdate = true;
  }
  else if (digitalRead(d3)==LOW){
    inp[3] = !inp[3];
    delay(150);
    enableRamUpdate = true;
  }
  else if (digitalRead(d4)==LOW){
    inp[4] = !inp[4];
    delay(150);
    enableRamUpdate = true;
  }
  else if (digitalRead(d5)==LOW){
    inp[5] = !inp[5];
    delay(150);
    enableRamUpdate = true;
  }
  else if (digitalRead(d6)==LOW){
    inp[6] = !inp[6];
    delay(150);
    enableRamUpdate = true;
  }
  else if (digitalRead(d7)==LOW){
    inp[7] = !inp[7];
    delay(150);
    enableRamUpdate = true;
  }
  

}





//-----reset-----//
void reset() {
  PC = 0;
  MAR = 0;
  AR = 0;
  BR = 0;
  IR = 0;
  N = 0;
  C = 0;
  Z = 0;
}

//-----fetch-----//
void fetch() {
  MAR = PC;
  IR = RAM[MAR];
}

//-----execute-----//
void execute() {
  switch (IR) {
    
    //No Operation
    case 0x00:
    break;
    
    //HLT
    case 0x01:
    hlt = true;
    break;
    
    //LDA imm
    case 0x02:
    PC++;
    MAR = PC;
    AR = RAM[MAR];
    break;
    
    //LDA abs
    case 0x03:
    PC++;
    MAR = PC;
    MAR = RAM[MAR];
    AR = RAM[MAR];
    break;
    
    //LDB imm
    case 0x04:
    PC++;
    MAR = PC;
    BR = RAM[MAR];
    break;
    
    //LDB abs
    case 0x05:
    PC++;
    MAR = PC;
    MAR = RAM[MAR];
    BR = RAM[MAR];
    break;
    
    //STA
    case 0x06:
    PC++;
    MAR = PC;
    MAR = RAM[MAR];
    RAM[MAR] = AR;
    break;
    
    //STB
    case 0x07:
    PC++;
    MAR = PC;
    MAR = RAM[MAR];
    RAM[MAR] = BR;
    break;
    
    //ADD imm
    case 0x08:
    PC++;
    MAR = PC;
    val = AR + RAM[MAR];
    AR = AR + RAM[MAR];
    flagUpdate(val);
    break;
    
    //SUB imm
    case 0x09:
    PC++;
    MAR = PC;
    val = AR - RAM[MAR];
    AR = AR - RAM[MAR];
    flagUpdate(val);
    break;
    
    //ADD B
    case 0x0a:
    val = AR + BR;
    AR = AR + BR;
    flagUpdate(val);
    break;
    
    //SUB B
    case 0x0b:
    val = AR - BR;
    AR = AR - BR;
    flagUpdate(val);
    break;
    
    //JMP imm
    case 0x0c:
    PC++;
    MAR = PC;
    PC = RAM[MAR]-1;
    break;
    
    //CMP
    case 0x0d:
    if (AR == BR) {
      E = 1;
    }
    else {
      E = 0;
    }
    if (AR > BR) {
      C = 1;
    }
    else {
      C = 0;
    }
    break;
    
    //BRE
    case 0x0e:
    if (E == 1) {
      PC++;
      MAR = PC;
      PC = RAM[MAR]-1;
    }
    else {
      PC++;
    }
    break;
    
    //BRG
    case 0x0f:
    if (C == 1) {
      PC++;
      MAR = PC;
      PC = RAM[MAR]-1;
    }
    else {
      PC++;
    }
    break;
    
    //BRN
    case 0x10:
    if (N == 1) {
      PC++;
      MAR = PC;
      PC = RAM[MAR]-1;
    }
    else {
      PC++;
    }
    break;
    
    //BRZ
    case 0x11:
    if (Z == 1) {
      PC++;
      MAR = PC;
      PC = RAM[MAR]-1;
    }
    else {
      PC++;
    }
    break;
    
    //BRV
    case 0x12:
    if (V == 1) {
      PC++;
      MAR = PC;
      PC = RAM[MAR]-1;
    }
    else {
      PC++;
    }
    break;
    
    
    
    
      
  }
  PC++;
}

void flagUpdate(int value){
  if (value==0){
    Z = 1;
  }
  else {
    Z = 0;
  }
  if (value > 255){
    V = 1;
  }
  else {
    V = 0;
  }
  if (value < 0){
    N = 1;
  }
  else {
    N = 0;
  }
  
}

void debug(){
  Serial.print("A: "); Serial.println(AR);
  Serial.print("B: "); Serial.println(BR);
  Serial.print("NVZ: "); 
  Serial.print(N);
  Serial.print(V);
  Serial.println(Z);
  Serial.print("PC: "); Serial.println(PC);
  Serial.print("MAR: "); Serial.println(MAR);
  Serial.print("IR: "); Serial.println(IR);
  Serial.print("RAM: "); Serial.println(RAM[MAR]);
  Serial.println();
}