// Define NES/SNES pin connections and variables
#define LATCH 19
#define CLOCK 18
#define DATA 5

#define NES 0
#define SNES 1

#define NES_BUTTONS 4
#define SNES_BUTTONS 8

static uint8_t stored_bits[2], bits, mask, idx, cnt;
bool data_state, controller_type;
unsigned long enterTime;

void setup() {
  //initiate controller type
  controller_type = SNES;

  switch (controller_type) {
    case NES:
      bits = 8;
      break;
    case SNES:
      bits = 16;
      break;
  }
  
  //initiate pin input
  pinMode(LATCH, OUTPUT);
  pinMode(CLOCK, OUTPUT);
  pinMode(DATA, INPUT_PULLUP);
  
  Serial.begin(115200);

  digitalWrite(CLOCK, HIGH);
  digitalWrite(LATCH, HIGH);

  delay(5);
}

void readControllerData() {
  //Sample data
  if (idx == 1 && mask <= 0x08) data_state = 0;
  else data_state = !digitalRead(DATA);
  stored_bits[idx] |= data_state *mask;
  //Shift mask
  mask >>= 1;
  if (!mask) {
    mask = 0x80;
    idx++;
  }
}

void emulateConsole() {
  //Set latch, NES/SNES shift registers logic is inverted
  digitalWrite(LATCH, LOW);
  mask = 0x80;
  idx = 0;
  stored_bits[0] = 0;
  stored_bits[1] = 0;
  delayMicroseconds(12);
  digitalWrite(LATCH, HIGH);
  delayMicroseconds(12);
  //Set clock
  cnt = 0;
  while (cnt < bits) {
    digitalWrite(CLOCK, LOW);
    readControllerData();
    delayMicroseconds(5);
    digitalWrite(CLOCK, HIGH);
    delayMicroseconds(5);
    cnt++;
  }
}

void loop() {
  enterTime = micros() + 16666;
  // Emulate console and read data from SNES controller
  emulateConsole();

  //DEBUG
  switch (controller_type) {
    case NES:
      for (mask = 0x80; mask > 0x00; mask >>= 1) {
        if(stored_bits[0] & mask) Serial.print(1); else Serial.print(0);
      }
      Serial.println();
      break;
    case SNES:
      for (mask = 0x80; mask > 0x00; mask >>= 1) {
        if(stored_bits[0] & mask) Serial.print(1); else Serial.print(0);
      }
      Serial.print(" ");
      for (mask = 0x80; mask > 0x00; mask >>= 1) {
        if(stored_bits[1] & mask) Serial.print(1); else Serial.print(0);
      }
      Serial.println();
      break;
  }

  while(micros() < enterTime); //make a pause
}