void WriteS(int s3, int s2, int s1);
void WriteK(int w4, int w3, int w2, int w1);
void ReadK();

sPins[] = {2, 3, 4};
kPins[] = {8, 9, 10, 11};

int kReadState[] = {0, 0, 0, 0};
int kWriteState[] = {0, 0, 0, 0};
int sWriteState[] = {0, 1, 0};

int sWriteCommand[] = {0, 0, 0, 0};
int kWriteCommand[] = {0, 0, 0, 0, 0};

int i;
char command;

void setup() {
  for (i=0; i<=2; i++) {
    pinMode(sPins[i], OUTPUT);
    digitalWrite(sPins[i], sWriteState[i]);
  }
  for (i=0; i<=3; i++) {
    pinMode(kPins[i], INPUT);
  }
  beginSerial(9600);
  printString("time machine GO!");
  printNewline();
}

void loop() {
  command = serialRead();
  if (command != -1) {
    printByte(command);    
  }
  if (command == 's') {
    i = 3;
    while (i >= 0) {
      command = serialRead();
      if (command != -1) {
        sWriteCommand[i] = command - 48;
        i--;
      }
    }
    sWriteCommand[0] = sWriteCommand[0] + 48;    
    if (sWriteCommand[0] == 's') {
      WriteS(sWriteCommand[3], sWriteCommand[2], sWriteCommand[1]);
      printString("!");
    } else {
      printString("x");
    }
  }
  if (command == 'w') {
    i = 4;
    while (i >= 0) {
      command = serialRead();
      if (command != -1) {
        kWriteCommand[i] = command - 48;
        i--;
      }
    }
    kWriteCommand[0] = kWriteCommand[0] + 48;
    if (kWriteCommand[0] == 'w') {
      WriteK(kWriteCommand[4], kWriteCommand[3], kWriteCommand[2], kWriteCommand[1]);
      printString("!");
    } else {
      printString("x");
    }
  }
  if (command == 'r') {
    ReadK();
    for (i=3; i>=0; i--) {
      printInteger(kReadState[i]);
    }
    printString("!");
  }
}

void WriteS(int s3, int s2, int s1) {
  sWriteState[2] = s3;
  sWriteState[1] = s2;
  sWriteState[0] = s1;
  for (i=2; i>=0; i--) {
    digitalWrite(sPins[i], sWriteState[i]);    
  }
}

void WriteK(int w4, int w3, int w2, int w1) {
  kWriteState[3] = w4;
  kWriteState[2] = w3;
  kWriteState[1] = w2;
  kWriteState[0] = w1;
  for (i=3; i>=0; i--) {
    pinMode(kPins[i], OUTPUT);
    digitalWrite(kPins[i], kWriteState[i]);    
  }
}

void ReadK() {
  for (i=3; i>=0; i--) {
    pinMode(kPins[i], INPUT);
    kReadState[i] = digitalRead(kPins[i]);
  }
}