// NOTICE!!!!! This CPU emulator uses Little-Endian byte ordering!!!

byte instruction = 0;
byte operand1 = 0;
byte operand2 = 0;
byte operand3 = 0;
long adresa = 0;  // program counter
uint8_t reljmp = 0;  // signed 8-bit integer, can jump down to -128 bytes, or up to 127 bytes relative to the address of the instruction

byte accumulator = 0; // the A register in assembly
byte xreg = 0;
byte yreg = 0;
byte statreg = 0;  // 4 flags: zero, ovf, irq disable, carry bit; 4 bottom-most bits are unused for now

int stack[258] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
byte stackpointer = 255;  // gets decremented on every PHA instruction, incremented on every PLA instruction (PHA - push A to stack, PLA - pull A from stack)

byte opmem[1026] = {0, 0, 0, 0};

unsigned long past = 0;
int clocktime = 1000;

byte var1h = 0;
byte var1l = 0;
unsigned int var2 = 0;

void setup() {

  pinMode(2, OUTPUT);  // srclk
  pinMode(3, OUTPUT);  // serial data
  pinMode(4, OUTPUT);  // ltclk
  pinMode(6, INPUT_PULLUP);
  pinMode(7, INPUT_PULLUP);
  pinMode(8, INPUT_PULLUP);
  pinMode(9, INPUT_PULLUP);
  pinMode(10, INPUT_PULLUP);
  pinMode(11, INPUT_PULLUP);
  pinMode(12, INPUT_PULLUP);
  pinMode(13, INPUT_PULLUP);
  pinMode(A0, INPUT_PULLUP);
  Serial.begin(9600);
delay(2100);
  /*for(int i = 0; i < 15; i = i + 1){
    Serial.println(MEMread(i));
    delay(100);
  }*/

delay(2100);

/*while(millis() < 2) {
adresa = 0b1111111111111111;
}*/
adresa = 65535;
//  Serial.println((MEMread(adresa)));
//var1l = MEMread(0b0111111111111100);
//var1h = MEMread(0b0111111111111101);  // reads reset vector at address 7ffc and 7ffd (little endian)
//var2 = ((var1h << 8) | (var1l));

//adresa = var2;

}

void loop() {

if(micros() - past > clocktime) {
  adresa = adresa + 1;  // increment the program counter 1000 times per second
  adresa = adresa & 0b0111111111111111;
  past = micros();
instruction = MEMread(adresa);
 // Serial.println(instruction);

switch(instruction) {

  case 0:
   // brk - brake instruction (permanently halts execution, recoverable only through reset)
   while(1) {delay(1);}
    break;

  case 1:  // lda immediate
    accumulator = MEMread(adresa + 1);
    adresa = adresa + 1;
    break;

  case 2:  // lda from address
    var1h = MEMread(adresa + 2);
    var1l = MEMread(adresa + 1);
    var2 = ((var1h << 8) | (var1l));
    accumulator = opmem[var2];
    adresa = adresa + 2;
    break;

  case 3:  // tay
    yreg = accumulator;
    break;

  case 4:  // tax
    xreg = accumulator;
    break;

  case 5:  // tya
    accumulator = yreg;
    break;

  case 6:  // txa
    accumulator = xreg;
    break;

  case 7:  // sta (there is just one, and it stores to a RAM address)
    var1h = MEMread(adresa + 2);
    var1l = MEMread(adresa + 1);
    var2 = ((var1h << 8) | (var1l));
    opmem[var2] = accumulator;
    adresa = adresa + 2;
    break;

  case 8:  // pha
    stack[stackpointer] = accumulator;
    stackpointer = stackpointer - 1;
    break;

  case 9:  // pla
    stackpointer = stackpointer + 1;
    accumulator = stack[stackpointer];
    stack[stackpointer] = 0;
    break;

  case 10:  // swr - send character over serial port
    Serial.write(MEMread(adresa + 1));
    adresa = adresa + 1;
    break;

  case 11:  // jsr - jump to subroutine
    stack[stackpointer] = adresa;
    stackpointer = stackpointer - 1;
    break;

  case 12:  // rts - return from subroutine
    adresa = stack[stackpointer];
    stackpointer = stackpointer + 1;
    break;

  case 13:  // bne - branch (if) not equal
    reljmp = MEMread(adresa + 1);
    if(!(statreg & 128)) {
      adresa = adresa + reljmp;
    }
    break;

  case 14:  // beq - branch (if) equal
    reljmp = MEMread(adresa + 1);
    if((statreg & 128)) {
      adresa = adresa + reljmp;
    }
    break;

  case 15:  // cmp - compare immediate (so far the only compare instruction) uses zero flag if num goes below 1
    byte comparator = MEMread(adresa + 1);
    int result = accumulator - comparator;
    if(result) {
      statreg = statreg & 127;
    } else {
      statreg = statreg | 128;
    }
    adresa = adresa + 1;
    break;

  case 16:  // adc - add immediate to accumulator with carry, save result to accumulator and carry bit
    byte toadd = MEMread(adresa + 1);
    if((toadd + accumulator) > 255) {statreg = statreg | 16;} else {statreg = statreg & 0b11101111;}  // sets or clears carry bit
    accumulator = accumulator + toadd;
    adresa = adresa + 1;
    break;

  case 17:  // sbc - subtract immediate from accumulator, and save result to accumulator (borrow the carry bit in case num becomes negative)
    byte subtr = MEMread(adresa + 1);
    statreg = statreg | 16;
    accumulator = accumulator - subtr;
    if((accumulator & 128)) {statreg = statreg & 0b11101111;}
    adresa = adresa + 1;
    break;

  case 18:  // iny - increments the Y register
    yreg = yreg + 1;
    break;

  case 19:  // dey - decrements the Y register
    yreg = yreg - 1;
    break;

  case 20:  // inc - increments a byte in memory (at an address)
    var1h = MEMread(adresa + 2);
    var1l = MEMread(adresa + 1);
    var2 = ((var1h << 8) | (var1l));
    opmem[var2] = opmem[var2] + 1;
    adresa = adresa + 2;
    break;

  case 21:  // dec - decrements a byte in memory (at an address)
    var1h = MEMread(adresa + 2);
    var1l = MEMread(adresa + 1);
    var2 = ((var1h << 8) | (var1l));
    opmem[var2] = opmem[var2] - 1;
    adresa = adresa + 2;
    break;

  case 22:  // jmp - jump to location (absolute)
    var1h = MEMread(adresa + 2);
    var1l = MEMread(adresa + 1);
    var2 = ((var1h << 8) | (var1l));
    adresa = var2;
    break;

  case 23:  // swa - Send byte from accumulator over serial port
    Serial.write(accumulator);
    break;

  default:
    adresa = adresa;
    break;

}

}

}

byte dataRD(unsigned int addr) {
byte daata = 0;

  daata = (128 * digitalRead(13)) + (64 * digitalRead(12)) + (32 * digitalRead(11)) + (16 * digitalRead(10)) + (8 * digitalRead(9)) + (4 * digitalRead(8)) + (2 * digitalRead(7)) + (1 * digitalRead(6));

return daata;
}

void shift(unsigned int adres) {
  digitalWrite(4, LOW);
  byte addres1 = adres >> 8;
  unsigned int addresint = adres << 8;
  byte addres2 = addresint >> 8;

  shiftOut(3, 2, MSBFIRST, addres1);
  shiftOut(3, 2, MSBFIRST, addres2);

  digitalWrite(4, HIGH);
}

byte MEMread(unsigned int addrs) {
  // reads a byte at an address, also takes amount of bits (both values are mandatory)
  byte udaj = 0;
  shift(addrs);
  udaj = dataRD(addrs);

  return udaj;
}