#define MODE 2
#define CSW 3
#define CSR 8
#define RESET 9
 
 
#define PORT_MANIPULATION

int bytePins[8] =   { 14, 15 , 16, 17, 4, 5, 6, 7 };

void setDBReadMode() {
  #ifdef PORT_MANIPULATION
  DDRD = DDRD & B00001111; // Set Pin 4..7 as inputs. High nibble of databus.
  DDRC = DDRC & B11110000; // Set Analog pin 0..3 as inputs     
  #else
  for (int i=0;i<8;i++) {
    pinMode(bytePins[i], INPUT);
  }
  #endif
}

void setDBWriteMode() {
  #ifdef PORT_MANIPULATION 
  DDRD = DDRD | B11110000; // Set Pin 4..7 as outputs. High nibble of databus.
  DDRC = DDRC | B00001111; // Set Analog pin 0..3 as outputs
  #else
  for (int i=0;i<8;i++) {
    pinMode(bytePins[i], OUTPUT);
  } 
  #endif
}

void reset() {
  Serial.println("Resetting");
  digitalWrite(RESET, HIGH);
  delayMicroseconds(100);
  digitalWrite(RESET, LOW);
  delayMicroseconds(5); 
  digitalWrite(RESET, HIGH);
}

inline void setPort(unsigned char value) {
    #ifdef PORT_MANIPULATION
    PORTD = (PIND & 0x0F) | (value & 0xF0);
    PORTC = (PINC & 0xF0) | (value & 0x0F);
    #else
    unsigned char mask = 1;
    for (int i=0;i<8;i++) {
      digitalWrite(bytePins[i], value & mask);
      mask = mask<<1;
    }   
    #endif   
}

inline unsigned char readPort() {
    unsigned char memByte = 0;   
    #ifdef PORT_MANIPULATION
    memByte  = (PIND & 0xF0) | (PINC & 0x0F);
    #else
    unsigned char mask = 1;
    for (int i=0;i<8;i++) {
      if (digitalRead(bytePins[i])) {
        memByte = memByte | mask;
      }
      mask = mask<<1;
    }       
    #endif 
   
    return memByte;
}

//Writes a byte to databus for register access
void writeByte( unsigned char value) { 
    setDBWriteMode();
    setPort(value);
    digitalWrite(MODE, HIGH);       
    digitalWrite(CSW, LOW);           
    delayMicroseconds(10);
    digitalWrite(CSW, HIGH);
    setDBReadMode();
}

//Reads a byte from databus for register access
unsigned char  readByte( ) {
    unsigned char memByte = 0;
    digitalWrite(MODE, HIGH);           
    digitalWrite(CSR, LOW);       
    delayMicroseconds(10);
    memByte = readPort();
    digitalWrite(CSR, HIGH);
    return memByte;
}

//Writes a byte to databus for vram access
void writeByteToVRAM( unsigned char value) { 
    setDBWriteMode();
    setPort(value);
    digitalWrite(MODE, LOW);       
    digitalWrite(CSW, LOW);           
    delayMicroseconds(10);
    digitalWrite(CSW, HIGH);
    setDBReadMode();
}

//Reads a byte from databus for vram access
unsigned char  readByteFromVRAM( ) {
    unsigned char memByte = 0;
    digitalWrite(MODE, LOW);           
    digitalWrite(CSR, LOW);       
    delayMicroseconds(8);
    memByte = readPort();
    digitalWrite(CSR, HIGH);
    return memByte;
}

void setRegister(unsigned char registerIndex, unsigned char value) {
  writeByte(value);
  writeByte(0x80 | registerIndex); 
}

void setWriteAddress( unsigned int address) {
  writeByte((address & 0xFFC0)>>6);
  writeByte(0x40 | (address & 0x3F)); 
}

void setReadAddress( unsigned int address) {
  writeByte((address & 0xFFC0)>>6);
  writeByte((address & 0x3F)); 
}

void setup() {
  setDBReadMode();
  Serial.begin(9600);
  pinMode(MODE, OUTPUT);
  pinMode(RESET, OUTPUT);
  pinMode(CSW, OUTPUT);
  pinMode(CSR, OUTPUT); 
 
  digitalWrite(RESET, HIGH);
  digitalWrite(MODE, HIGH); 
  digitalWrite(CSW, HIGH);   
  digitalWrite(CSR, HIGH); 

  reset();
  delay(2000);

  setRegister(0, 0);
  setRegister(1, 0xD0);
  setRegister(7, 0x56); 
 
  Serial.println("Clearing RAM");
  setWriteAddress(0);
  for (int i = 0;i<16384;i++) {
    writeByteToVRAM(0);
  }
  Serial.println("RAM Cleared, testing"); 
  setWriteAddress(0);
  for (int i = 0;i<256;i++) {
    writeByteToVRAM(i);
  }
  setReadAddress(0);
  for (int i = 0;i<256;i++) {
    unsigned char value = readByteFromVRAM();
    Serial.println(value);
  }
}

void loop() {
 
}