#define MASK_UP     0x80
#define MASK_DOWN   0x40
#define MASK_LEFT   0x20
#define MASK_RIGHT  0x10
#define MASK_UPC    0x08
#define MASK_DOWNC  0x04
#define MASK_LEFTC  0x02
#define MASK_RIGHTC 0x01

#define MASK_A      0x20
#define MASK_B      0x10
#define MASK_GRAB   0x08
#define MASK_SHIELD 0x04
#define MASK_TILT   0x02
#define MASK_START  0x01

byte incomingByte = 0;
bool left = false; // is pressing LEFT
bool right = false; // is pressing RIGHT

// the setup function runs once when you press reset or power the board
void setup() {
  pinMode(2, OUTPUT); //  UP
  pinMode(3, OUTPUT); //  DOWN
  pinMode(4, OUTPUT); //  LEFT
  pinMode(5, OUTPUT); //  RIGHT
  pinMode(8, OUTPUT); //  C UP
  pinMode(9, OUTPUT); //  C DOWN
  pinMode(10, OUTPUT); // C LEFT
  pinMode(11, OUTPUT); // C RIGHT
  pinMode(A5, OUTPUT); // A
  pinMode(A4, OUTPUT); // B
  pinMode(A3, OUTPUT); // GRAB
  pinMode(A2, OUTPUT); // SHIELD
  pinMode(A1, OUTPUT); // TILT
  pinMode(A0, OUTPUT); // START

  pinMode(13, OUTPUT); // onboard LED pin, just for testing

  // initialize serial communication at 9600 bits per second:
  Serial.begin(9600);
}

// the loop function is called repeatedly
void loop() {

  if (Serial.available() > 1) { // wait for at least 2 bytes
    // read the first byte:
    incomingByte = Serial.read();

    if ((incomingByte & MASK_DOWN) > 0) { // down takes priority (should make wavedashing easier)
      digitalWrite(2, LOW);
      digitalWrite(3, HIGH);
    } else {
      digitalWrite(3, LOW);
      if ((incomingByte & MASK_UP) > 0) digitalWrite(2, HIGH); else digitalWrite(2, LOW);
    }

    if ((incomingByte & MASK_RIGHT) > 0) { // if right is pressed
      if (left) {
        digitalWrite(4, LOW);
        digitalWrite(5, HIGH);
        left = false;
        right = true;
      } else {
        if ((incomingByte & MASK_LEFT) > 0) {
          if (right) {
            digitalWrite(5, LOW);
            digitalWrite(4, HIGH);
            right = false;
            left = true;
          } else {
            digitalWrite(4, LOW);
            digitalWrite(5, LOW);
          }
        } else {
          digitalWrite(4, LOW);
          digitalWrite(5, HIGH);
          right = true;
        }
      }
    } else { // if right is not pressed
      digitalWrite(5, LOW);
      if ((incomingByte & MASK_LEFT) > 0) {
        digitalWrite(4, HIGH);
        left = true;
      } else {
        digitalWrite(4, LOW);
        left = false;
      }
      right = false;
    }

    if ((incomingByte & MASK_UPC) > 0) { // up smash takes priority (shouldn't ever matter)
      digitalWrite(9, LOW);
      digitalWrite(8, HIGH);
    } else {
      digitalWrite(8, LOW);
      if ((incomingByte & MASK_DOWNC) > 0) digitalWrite(9, HIGH); else digitalWrite(9, LOW);
    }

    if ((incomingByte & MASK_LEFTC) > 0) { // left smash takes priority (the key is bigger)
      digitalWrite(11, LOW);
      digitalWrite(10, HIGH);
    } else {
      digitalWrite(10, LOW);
      if ((incomingByte & MASK_RIGHTC) > 0) digitalWrite(11, HIGH); else digitalWrite(11, LOW);
    }

    // read the second byte:
    incomingByte = Serial.read();

    if ((incomingByte & MASK_A      ) > 0) digitalWrite(A5, LOW); else digitalWrite(A5, HIGH);
    if ((incomingByte & MASK_B      ) > 0) digitalWrite(A4, LOW); else digitalWrite(A4, HIGH);
    if ((incomingByte & MASK_GRAB   ) > 0) digitalWrite(A3, LOW); else digitalWrite(A3, HIGH);
    if ((incomingByte & MASK_SHIELD ) > 0) digitalWrite(A2, HIGH); else digitalWrite(A2, LOW);
    if ((incomingByte & MASK_TILT   ) > 0) digitalWrite(A1, LOW); else digitalWrite(A1, HIGH);
    if ((incomingByte & MASK_START  ) > 0) digitalWrite(A0, LOW); else digitalWrite(A0, HIGH);

  }

}