 {
   currentState[player] = 0;
 }
- 
-void read3buttons(int player)
+
+void readButtons(int player, int cycle)
 {
-  // Set SELECT LOW and read lowFlag
-  digitalWrite(SELECT[player], LOW);
-     
-  delayMicroseconds(20);
-     
-  for (int i = 0; i < sizeof(inputMap) / sizeof(input); i++)
+  digitalWrite(SELECT[player], cycle % 2 == 0 ? LOW : HIGH);
+  
+  // Read flags
+  switch (cycle)
   {
-    if (inputMap[i].player == player && digitalRead(inputMap[i].pin) == LOW)
-    {
-      currentState[player] |= inputMap[i].lowFlag;
-    }
+    case 2:
+      currentState[player] |= (digitalRead(inputPins[player][0]) == LOW) ? UP : 0;
+      currentState[player] |= (digitalRead(inputPins[player][1]) == LOW) ? DOWN : 0;
+      currentState[player] |= (digitalRead(inputPins[player][2]) == LOW) ? ON : 0;
+      currentState[player] |= (digitalRead(inputPins[player][3]) == LOW) ? ON : 0;
+      currentState[player] |= (digitalRead(inputPins[player][4]) == LOW) ? A : 0;
+      currentState[player] |= (digitalRead(inputPins[player][5]) == LOW) ? START : 0;
+      break;
+    case 3:
+      currentState[player] |= (digitalRead(inputPins[player][0]) == LOW) ? UP : 0;
+      currentState[player] |= (digitalRead(inputPins[player][1]) == LOW) ? DOWN : 0;
+      currentState[player] |= (digitalRead(inputPins[player][2]) == LOW) ? LEFT : 0;
+      currentState[player] |= (digitalRead(inputPins[player][3]) == LOW) ? RIGHT : 0;
+      currentState[player] |= (digitalRead(inputPins[player][4]) == LOW) ? B : 0;
+      currentState[player] |= (digitalRead(inputPins[player][5]) == LOW) ? C : 0;
+      break;
+    case 4:
+      sixButtonMode[player] = (digitalRead(inputPins[player][0]) == LOW && digitalRead(inputPins[player][1]) == LOW);
+      break;
+    case 5:
+      if (sixButtonMode[player])
+      {
+        currentState[player] |= (digitalRead(inputPins[player][0]) == LOW) ? Z : 0;
+        currentState[player] |= (digitalRead(inputPins[player][1]) == LOW) ? Y : 0;
+        currentState[player] |= (digitalRead(inputPins[player][2]) == LOW) ? X : 0;
+        currentState[player] |= (digitalRead(inputPins[player][3]) == LOW) ? MODE : 0;
+      }
+      break;
   }
- 
-  // Set SELECT HIGH and read highFlag
-  digitalWrite(SELECT[player], HIGH);
-     
-  delayMicroseconds(20);
-     
-  for (int i = 0; i < sizeof(inputMap) / sizeof(input); i++)
-  {
-    if (inputMap[i].player == player && digitalRead(inputMap[i].pin) == LOW)
-    {
-      currentState[player] |= inputMap[i].highFlag;
-    }
-  }
-    
-  // When a six-button first connects, it'll spam UP and DOWN,
-  // which signals the game to switch to 6-button polling
-  if (currentState[player] == (ON | UP | DOWN))
-  {
-    sixButtonMode[player] = true;
-  }
-  // When a controller disconnects, revert to three-button polling
-  else if ((currentState[player] & ON) == 0)
-  {
-    sixButtonMode[player] = false;
-  }
-  
-  delayMicroseconds(20);
-}
- 
-void read6buttons(int player)
-{
-  // Poll for three-button states twice
-  read3buttons(player);
-  read3buttons(player);
-  
-  // After two three-button polls, pulse the SELECT line
-  // so the six-button reports the higher button states
-  digitalWrite(SELECT[player], LOW);
-  delayMicroseconds(20);
-  digitalWrite(SELECT[player], HIGH);
-  
-  for(int i = 0; i < sizeof(inputMap) / sizeof(input); i++)
-  {
-    if (inputMap[i].player == player && digitalRead(inputMap[i].pin) == LOW)
-    {
-      currentState[player] |= inputMap[i].pulse3Flag;
-    }
-  }
-  
-  delayMicroseconds(1000);
 }
  
 void sendStates()

  // Only report controller states if at least one has changed
  boolean hasChanged = false;
  
  for (int i = 0; i < PLAYERS; i++)
  {
    if (currentState[i] != lastState[i])
    {
      hasChanged = true;
    }

 */
 
const int PLAYERS = 2;
const int INPUT_PINS_PER_PLAYER = 6;

// Mapping controller DB9 pin 7 to Arduino pin
const int SELECT[] = {
  8, // Player 1
  9 // Player 2
};

// Mapping controller DB9 input pins to Arduino pins
int inputPins[][INPUT_PINS_PER_PLAYER] = {
  // { DB9 Pin 1, DB9 Pin 2, DB9 Pin 3, DB9 Pin 4, DB9 Pin 6, DB9 Pin 9 }
  { 2, 3, 4, 5, 6, 7 }, // Player 1
  { A0, A1, A2, A3, A4, A5 } // Player 2
};

// Controller button flags
enum flags {
  ON = 1,
  UP = 2,
  DOWN = 4,
  LEFT = 8,
  RIGHT = 16,
  START = 32,
  A = 64,
  B = 128,
  C = 256,
  X = 512,
  Y = 1024,
  Z = 2048,
  MODE = 4096
};
 
// Controller states
word currentState[] = { 0, 0 };
word lastState[] = { -1, -1 };
 
// Default to three-button mode until six-button connects
boolean sixButtonMode[] = { false, false };

const int CYCLES = 8;

void setup()
{
  // Setup input pins
  for (int player = 0; player < PLAYERS; player++)
  {
    for (int pin = 0; pin < INPUT_PINS_PER_PLAYER; pin++)
    {
      pinMode(inputPins[player][pin], INPUT);
      digitalWrite(inputPins[player][pin], HIGH);
    }
  }
  
  // Setup select pins
  for (int player = 0; player < PLAYERS; player++)
  {
    pinMode(SELECT[player], OUTPUT);
    digitalWrite(SELECT[player], HIGH);
  }
  
  Serial.begin(9600);
