#define SHIFT_DATA  2
#define SHIFT_CLK   3
#define SHIFT_LATCH 4
#define EEPROM_D0   5
#define EEPROM_D7   12
#define WRITE_EN    13

/*
 * Set EEPROM address lines.
 */
void
setAddress(int address, bool outputEnable) {
  shiftOut(SHIFT_DATA, SHIFT_CLK, MSBFIRST, (address >> 8) | (outputEnable ? 0x00 : 0x80));
  shiftOut(SHIFT_DATA, SHIFT_CLK, MSBFIRST, address);

  digitalWrite(SHIFT_LATCH, HIGH);
  delayMicroseconds(1);
  digitalWrite(SHIFT_LATCH, LOW);
}

/*
 * Read one byte from EEPROM.
 */
byte
readEEPROM(int address)
{
  for (int pin = EEPROM_D0; pin <= EEPROM_D7; pin++) {
    pinMode(pin, INPUT);
  }
  
  setAddress(address, true);
  byte data = 0;
  for (int pin = EEPROM_D7; pin >= EEPROM_D0; pin--) {
    data = (data << 1) + digitalRead(pin);
  }
  
  return data;
}

/*
 * Write one byte to EEPROM.
 */
void
writeEEPROM(int address, byte data) {
  for (int pin = EEPROM_D0; pin <= EEPROM_D7; pin++) {
    pinMode(pin, OUTPUT);
  }

  setAddress(address, false);
  for (int pin = EEPROM_D0; pin <= EEPROM_D7; pin++) {
    digitalWrite(pin, data & 1);
    data = data >> 1;
  }
  digitalWrite(WRITE_EN, LOW);
  delayMicroseconds(1);
  digitalWrite(WRITE_EN, HIGH);
  delay(5);
}

/*
 * Read 256 bytes from EEPROM and print it out.
 */
void
printContents(int start, int length) {
    for (int base = start; base < length; base += 16) {
      byte data[16];
      for (int offset = 0; offset < 16; offset++) {
        data[offset] = readEEPROM(base + offset);
      }

      char buf[80];
      sprintf(buf, "%03x: %02x %02x %02x %02x %02x %02x %02x %02x  %02x %02x %02x %02x %02x %02x %02x %02x",
        base,
        data[0], data[1],  data[2],  data[3],  data[4],  data[5],  data[6],  data[7],
        data[8], data[9], data[10], data[11], data[12], data[13], data[14], data[15]);
      Serial.println(buf);
    }
}

/*
 * 16-bit instruction word.
 */
#define HLT 0b1000000000000000  // Clock HALT.
#define MI  0b0100000000000000  // Memory Adress Register IN.
#define RI  0b0010000000000000  // RAM IN.
#define RO  0b0001000000000000  // RAM OUT.
#define IO  0b0000100000000000  // Instruction Register OUT.
#define II  0b0000010000000000  // Instruction Register IN.
#define AI  0b0000001000000000  // A-Register IN.
#define AO  0b0000000100000000  // A-Register OUT.
#define EO  0b0000000010000000  // SUM OUT.
#define SU  0b0000000001000000  // Substract.
#define BI  0b0000000000100000  // B-Register IN.
#define OI  0b0000000000010000  // Output (LED) IN.
#define CE  0b0000000000001000  // Program Counter ENABLE.
#define CO  0b0000000000000100  // Program Counter OUT.
#define J   0b0000000000000010  // Program Counter JUMP.
#define FI  0b0000000000000001  // Flags IN.

/*
 * 8-byte instruction op-codes.
 */ 
const PROGMEM uint16_t UCODE_TEMPLATE[16][8] = {
  { MI|CO, RO|II|CE, 0,     0,     0,           0, 0, 0 },  // 0000 - NOP (No Operation)
  { MI|CO, RO|II|CE, IO|MI, RO|AI, 0,           0, 0, 0 },  // 0001 - LDA <memory address> (Load content of memory address to A-Register)
  { MI|CO, RO|II|CE, IO|MI, RO|BI, EO|AI|FI,    0, 0, 0 },  // 0010 - ADD <memory address> (Load content of memory address to B-Register, and load sum to A-Register)
  { MI|CO, RO|II|CE, IO|MI, RO|BI, EO|AI|SU|FI, 0, 0, 0 },  // 0011 - SUB <memory address> (Substract content of memory address)
  { MI|CO, RO|II|CE, IO|MI, AO|RI, 0,           0, 0, 0 },  // 0100 - STA <memory address> (Store A-Register to memory address)
  { MI|CO, RO|II|CE, IO|AI, 0,     0,           0, 0, 0 },  // 0101 - LDI <value>          (Load value to A-Register)
  { MI|CO, RO|II|CE, IO|J,  0,     0,           0, 0, 0 },  // 0110 - JMP <memory address> (Jump to memory address)
  { MI|CO, RO|II|CE, 0,     0,     0,           0, 0, 0 },  // 0111 - JC
  { MI|CO, RO|II|CE, 0,     0,     0,           0, 0, 0 },  // 1000 - JZ
  { MI|CO, RO|II|CE, 0,     0,     0,           0, 0, 0 },  // 1001
  { MI|CO, RO|II|CE, 0,     0,     0,           0, 0, 0 },  // 1010
  { MI|CO, RO|II|CE, 0,     0,     0,           0, 0, 0 },  // 1011
  { MI|CO, RO|II|CE, 0,     0,     0,           0, 0, 0 },  // 1100
  { MI|CO, RO|II|CE, 0,     0,     0,           0, 0, 0 },  // 1101
  { MI|CO, RO|II|CE, AO|OI, 0,     0,           0, 0, 0 },  // 1110 - OUT (Write A-Register content to output led)
  { MI|CO, RO|II|CE, HLT,   0,     0,           0, 0, 0 },  // 1111 - HLT (Halt Clock)
};

#define FLAGS_Z0C0 0
#define FLAGS_Z0C1 1
#define FLAGS_Z1C0 2
#define FLAGS_Z1C1 3

#define JC 0b0111
#define JZ 0b1000

uint16_t ucode[4][16][8];

void
initUCode()
{
  // ZF = 0, CF = 0
  memcpy_P(ucode[FLAGS_Z0C0], UCODE_TEMPLATE, sizeof(UCODE_TEMPLATE));

  // ZF = 0, CF = 1
  memcpy_P(ucode[FLAGS_Z0C1], UCODE_TEMPLATE, sizeof(UCODE_TEMPLATE));
  ucode[FLAGS_Z0C1][JC][2] = IO|J;
  
  // ZF = 1, CF = 0
  memcpy_P(ucode[FLAGS_Z1C0], UCODE_TEMPLATE, sizeof(UCODE_TEMPLATE));
  ucode[FLAGS_Z1C0][JZ][2] = IO|J;
 
  // ZF = 1, CF = 1
  memcpy_P(ucode[FLAGS_Z1C1], UCODE_TEMPLATE, sizeof(UCODE_TEMPLATE));
  ucode[FLAGS_Z1C1][JC][2] = IO|J;
  ucode[FLAGS_Z1C1][JZ][2] = IO|J;
}

/*
 * Main program.
 */
void
setup() {
  pinMode(SHIFT_DATA, OUTPUT);
  pinMode(SHIFT_CLK, OUTPUT);
  pinMode(SHIFT_LATCH, OUTPUT);
  digitalWrite(WRITE_EN, HIGH);
  pinMode(WRITE_EN, OUTPUT);
  Serial.begin(115200);

  initUCode();

  // Erase entire EEPROM.
  Serial.print("Erasing EEPROM ");
  for (int address = 0; address < 2048; address++) {
    writeEEPROM(address, 0xff);
    
    if (address % 64 == 0) {
      Serial.print(".");
    }
  }
  Serial.println(" done");

  // Program data bytes EEPROM first 8 bits.
  Serial.print("Programming EEPROM ...");

  for (int address = 0; address < 1024; address++) {
    int flags       = (address & 0b1100000000) >> 8;
    int byte_sel    = (address & 0b0010000000) >> 7;
    int instruction = (address & 0b0001111000) >> 3;
    int step        = (address & 0b0000000111);

    if (byte_sel) {
      writeEEPROM(address, ucode[flags][instruction][step]);
    } else {
      writeEEPROM(address, ucode[flags][instruction][step] >> 8);
    }
    
    if (address % 64 == 0) {
      Serial.print(".");
    }
  }

  Serial.println(" done");

  // Read and print out the contents of the EEPROM.
  Serial.println("Reading EEPROM");
  printContents(0, 1024);
}

void loop() {
  // put your main code here, to run repeatedly:
}