int clkPin = 23; //Clock pin
int wePin = 22; //Write Enable
int read_addr1[] = {0, 1}; // Address1 pins
int read_addr2[] = {2, 3}; // Address2 pins
int write_addr[] = {4, 5};
int read_data1[] = {24,25,26,27,28,29,30,31};
int read_data2[] = {32,33,34,35,36,37,38,39};
int write_data[] = {6,7,8,9,10,11,12,13};

void setup() {
  // initialize serial communication
  Serial.begin(9600);

  // initialize pins as output
  pinMode(clkPin, OUTPUT);
  pinMode(wePin, OUTPUT);
  digitalWrite(clkPin, LOW);
  digitalWrite(wePin, LOW);

  //initialize address 1 & 2
  for (int i=0; i<2; i++) {
      pinMode(read_addr1[i], OUTPUT);
      digitalWrite(read_addr1[i], LOW);
      pinMode(read_addr2[i], OUTPUT);
      digitalWrite(read_addr2[i], LOW);
      pinMode(write_addr[i], OUTPUT);
      digitalWrite(write_addr[i], LOW);
  }

  for (int i=0; i<8; i++) {
      pinMode(read_data1[i], INPUT);
      pinMode(read_data2[i], INPUT);
      pinMode(write_data[i], INPUT);
  }

}

#define BUFFER_SIZE 32
char buffer[BUFFER_SIZE];
char *ptoken;
int pos = 0, address1,address2, valueToWrite;

void loop() {
  int incomingByte;

  // see if there's incoming serial data:
  if (Serial.available() > 0) {
      delay(50);

      while (Serial.available() > 0) {
          incomingByte = Serial.read();
          if (pos < (BUFFER_SIZE-1)) {
              buffer[pos] = incomingByte;
              pos++;
          }
      }
      buffer[pos] = 0;

      pos = 0;
      ptoken = strtok(buffer, " ");
      if (strcmp(ptoken, "write") == 0) {
          ptoken = strtok (NULL, " ");
          address1 = IntAddress(atoi(ptoken));
          ptoken = strtok (NULL, " ");
          valueToWrite = atoi(ptoken);

          Serial.print("write address = ");
          Serial.print(address1);
          Serial.print(" value=");
          Serial.println(valueToWrite);

          writeToRegister(address1, valueToWrite);

      } else if (strcmp(ptoken, "read") == 0) {
          ptoken = strtok (NULL, " ");
          address1 = IntAddress(atoi(ptoken));
          ptoken = strtok (NULL, " ");
          address2 = IntAddress(atoi(ptoken));

          Serial.print("read from ");
          Serial.print(address1);
          Serial.print("=");
          Serial.println(readFromRegister(address1, read_addr1, read_data1));

          Serial.print("read from ");
          Serial.print(address2);
          Serial.print("=");
          Serial.println(readFromRegister(address2, read_addr2, read_data2));

      } else {
          Serial.print("Invalid command '");
          Serial.print(ptoken);
          Serial.println("'");
      }

  }

}

int IntAddress(char *address){
         if(strcmp(address,"r0")==0){
              return 0;
          }else if(strcmp(address,"r1")==0){
              return 1;
          }else if(strcmp(address,"r2")==0){
              return 2;
          }else if(strcmp(address,"r3")==0){
              return 3;
          }
    return -1;
}

void writeToRegister(int addr, int value)
{
    digitalWrite(wePin, HIGH);
    writeValue(addr, write_addr, 2);
    writeValue(value, write_data, 8);

    //Clock Pulse
    digitalWrite(clkPin, LOW);
    digitalWrite(clkPin, HIGH);

    digitalWrite(clkPin, LOW);
    digitalWrite(wePin, LOW);
}

int readFromRegister(int addr, int addrPin[], int dataOutPin[])
{
    digitalWrite(wePin, LOW);
    writeValue(addr, addrPin, 2);

    //Clock Pulse
    digitalWrite(clkPin, LOW);
    digitalWrite(clkPin, HIGH);
    digitalWrite(clkPin, LOW);

    //Read data from pins
    return readValue(dataOutPin, 8);
}

void writeValue(int value, int pin[], int pinCount) {

    for (int i = 0; i < pinCount; i++) {
        if ((value & (1 << i)) != 0)
            digitalWrite(pin[i], HIGH);
        else
            digitalWrite(pin[i], LOW);
    }
}

int readValue(int pin[], int pinCount) {
    int value = 0;
    int mask = 1;

    for (int i = 0; i < pinCount; i++) {
        unsigned char d = digitalRead(pin[i]);

        if (d == HIGH)
            value |= mask;

        mask <<= 1;
    }

    return value;
}