#include <Joystick.h>

// Microseconds
#define LATCH_TIME 12
#define MEASURE_DELAY 6

// Leonardo Pins
#define CLOCK 2
#define LATCH 3
#define P1_DATA 4
#define P2_DATA 5

// Machine States
#define STATE_LATCH 0
#define STATE_WAIT_LATCH 1
#define STATE_WAIT_MEASURE 2
#define STATE_MEASURE_RISE 3
#define STATE_MEASURE_FALL 4
#define STATE_WAIT_LATCH 5

// SNES Map
#define BUTTON_A      8
#define BUTTON_B      0
#define BUTTON_X      9
#define BUTTON_Y      1
#define BUTTON_START  3
#define BUTTON_SELECT 2
#define BUTTON_L     10
#define BUTTON_R     11
#define BUTTON_UP     4
#define BUTTON_DOWN   5
#define BUTTON_LEFT   6
#define BUTTON_RIGHT  7

unsigned long t0, t1, t2;

bool P1[16], P2[16];

int state;
int curPin;

Joystick_ Joy1(0x03, JOYSTICK_TYPE_GAMEPAD,
  8, 0,                  // Button Count, Hat Switch Count
  true, true, false,     // X and Y, but no Z Axis
  false, false, false,   // No Rx, Ry, or Rz
  false, false,          // No rudder or throttle
  false, false, false);  // No accelerator, brake, or steering

Joystick_ Joy2(0x04, JOYSTICK_TYPE_GAMEPAD,
  8, 0,                  // Button Count, Hat Switch Count
  true, true, false,     // X and Y, but no Z Axis
  false, false, false,   // No Rx, Ry, or Rz
  false, false,          // No rudder or throttle
  false, false, false);  // No accelerator, brake, or steering

void setup() {
  pinMode(CLOCK, OUTPUT);
  pinMode(LATCH, OUTPUT);
  pinMode(P1_DATA, INPUT);
  pinMode(P2_DATA, INPUT);

  for (int i=0; i<10; i++) {
    P1[i] = false;
    P2[i] = false;
  }

  digitalWrite(CLOCK, HIGH);
  digitalWrite(LATCH, LOW);
  digitalWrite(P1_DATA, HIGH);
  digitalWrite(P2_DATA, HIGH);

  state = STATE_LATCH;
  curPin = 0;

  Joy1.setXAxisRange(-1, 1);
  Joy1.setYAxisRange(-1, 1);
  Joy2.setXAxisRange(-1, 1);
  Joy2.setYAxisRange(-1, 1);
  
  
  Joy1.begin(false);
  Joy2.begin(false);
}

void updateJoys() {
  Joy1.setYAxis(0);
  Joy2.setYAxis(0);
  Joy1.setXAxis(0);
  Joy2.setXAxis(0);
  
  for (int i=0; i<12; i++) {
    switch (i) {
      case BUTTON_A:
        Joy1.setButton(0, P1[i]);
        Joy2.setButton(0, P2[i]);
        break;
      case BUTTON_B:
        Joy1.setButton(1, P1[i]);
        Joy2.setButton(1, P2[i]);
        break;
      case BUTTON_X:
        Joy1.setButton(2, P1[i]);
        Joy2.setButton(2, P2[i]);
        break;
      case BUTTON_Y:
        Joy1.setButton(3, P1[i]);
        Joy2.setButton(3, P2[i]);
        break;
      case BUTTON_START:
        Joy1.setButton(4, P1[i]);
        Joy2.setButton(4, P2[i]);
        break;
      case BUTTON_SELECT:
        Joy1.setButton(5, P1[i]);
        Joy2.setButton(5, P2[i]);
        break;
      case BUTTON_L:
        Joy1.setButton(6, P1[i]);
        Joy2.setButton(6, P2[i]);
        break;
      case BUTTON_R:
        Joy1.setButton(7, P1[i]);
        Joy2.setButton(7, P2[i]);
        break;
      case BUTTON_UP:
        if (P1[i]) {
          Joy1.setYAxis(-1);
        }
        if (P2[i]) {
          Joy2.setYAxis(-1);
        }
        break;
      case BUTTON_DOWN:
        if (P1[i]) {
          Joy1.setYAxis(1);
        }
        if (P2[i]) {
          Joy2.setYAxis(1);
        }
        break;
      case BUTTON_LEFT:
        if (P1[i]) {
          Joy1.setXAxis(-1);
        }
        if (P2[i]) {
          Joy2.setXAxis(-1);
        }
        break;
      case BUTTON_RIGHT:
        if (P1[i]) {
          Joy1.setXAxis(1);
        }
        if (P2[i]) {
          Joy2.setXAxis(1);
        }
        break;
    }
  }
  Joy1.sendState();
  Joy2.sendState();
}

void loop() {
  switch(state) {
    case STATE_LATCH:
      digitalWrite(LATCH, HIGH);
      t0 = micros();
      state = STATE_WAIT_LATCH;
      break;
    case STATE_WAIT_LATCH:
      if (micros() - t0 >= LATCH_TIME) {
        digitalWrite(LATCH, LOW);
        t0 = micros();
        state = STATE_WAIT_MEASURE;
      }
      break;
    case STATE_WAIT_MEASURE:
      if(micros() - t0 >= MEASURE_DELAY) {
        t0 = micros();
        state = STATE_MEASURE_FALL;
        digitalWrite(CLOCK, LOW);
      }
      break;
    case STATE_MEASURE_RISE:
      if (micros() - t0 >= MEASURE_DELAY) {
        if (curPin == 16) {
          state = STATE_LATCH;
          curPin = 0;
          updateJoys();
        } else {
          digitalWrite(CLOCK, LOW);
          t0 = micros();
          state = STATE_MEASURE_FALL;
        }
      }
      break;
    case STATE_MEASURE_FALL:
      if (micros() - t0 >= MEASURE_DELAY) {
        P1[curPin] = !digitalRead(P1_DATA);
        P2[curPin] = !digitalRead(P2_DATA);
        curPin++;
        digitalWrite(CLOCK, HIGH);
        t0 = micros();
        state = STATE_MEASURE_RISE;
      }
      break;
  }
}