#include <stdint.h>

// GPIO pins used for connected gamepad
#define CLOCK 21
#define LATCH 20
#define DATA  19

#define DEVICE_TYPE_AUTO 0
#define DEVICE_TYPE_NES  1
#define DEVICE_TYPE_SNES 2
#define DEVICE_METHOD_JOYSTICK 0
#define DEVICE_METHOD_KEYBOARD 1

enum {
  NES_BUTTON_A,
  NES_BUTTON_B,
  NES_BUTTON_SELECT,
  NES_BUTTON_START,
  NES_DPAD_UP,
  NES_DPAD_DOWN,
  NES_DPAD_LEFT,
  NES_DPAD_RIGHT
} NES_BUTTON_MAP;

enum {
  SNES_BUTTON_B,
  SNES_BUTTON_Y,
  SNES_BUTTON_SELECT,
  SNES_BUTTON_START,
  SNES_DPAD_UP,
  SNES_DPAD_DOWN,
  SNES_DPAD_LEFT,
  SNES_DPAD_RIGHT,
  SNES_BUTTON_A,
  SNES_BUTTON_X,
  SNES_BUTTON_L,
  SNES_BUTTON_R,
} SNES_BUTTON_MAP;

uint8_t BUTTON_STATE[16];
uint8_t DEVICE_TYPE = DEVICE_TYPE_AUTO;            // Device Type (configurable)
uint8_t DEVICE_METHOD = DEVICE_METHOD_JOYSTICK;   // Device Method (configurable)

const int16_t NES_JOYSTICK_MAP[4][2] = {
  {NES_BUTTON_A,        1},
  {NES_BUTTON_B,        2},
  {NES_BUTTON_SELECT,   7},
  {NES_BUTTON_START,    8},
};

// NES Keyboard Map (configurable)
const int16_t NES_KEYBOARD_MAP[8][2] = {
  {NES_BUTTON_A,        KEY_X},
  {NES_BUTTON_B,        KEY_Z},
  {NES_BUTTON_SELECT,   KEY_A},
  {NES_BUTTON_START,    KEY_S},
  {NES_DPAD_UP,         KEY_UP},
  {NES_DPAD_DOWN,       KEY_DOWN},
  {NES_DPAD_LEFT,       KEY_LEFT},
  {NES_DPAD_RIGHT,      KEY_RIGHT},
};

const int16_t SNES_JOYSTICK_MAP[8][2] = {
  {SNES_BUTTON_A,       1},
  {SNES_BUTTON_B,       2},
  {SNES_BUTTON_X,       3},
  {SNES_BUTTON_Y,       4},
  {SNES_BUTTON_L,       5},
  {SNES_BUTTON_R,       6},
  {SNES_BUTTON_SELECT,  7},
  {SNES_BUTTON_START,   8},
};

// SNES Keyboard Map (configurable)
const int16_t SNES_KEYBOARD_MAP[16][2] = {
  {SNES_BUTTON_A,        KEY_X},
  {SNES_BUTTON_B,        KEY_Z},
  {SNES_BUTTON_X,        KEY_D},
  {SNES_BUTTON_Y,        KEY_F},
  {SNES_BUTTON_L,        KEY_C},
  {SNES_BUTTON_R,        KEY_V},
  {SNES_BUTTON_SELECT,   KEY_A},
  {SNES_BUTTON_START,    KEY_S},
  {SNES_DPAD_UP,         KEY_UP},
  {SNES_DPAD_DOWN,       KEY_DOWN},
  {SNES_DPAD_LEFT,       KEY_LEFT},
  {SNES_DPAD_RIGHT,      KEY_RIGHT},
};

void (*PROCESS_INPUT_FN)(void);

// Delay for approx 100ns. Assumes 16Mhz AtMega CPU
#define delay100ns()
  __asm__ __volatile__ ("nop\n\t");\
  __asm__ __volatile__ ("nop\n\t");

#define SendCmdTakeSample(clock_pin, latch_pin)\
  digitalWrite(clock_pin, HIGH);\
  digitalWrite(latch_pin, HIGH);\
  delay100ns();\
  digitalWrite(latch_pin, LOW);

#define SendCmdSendData(clock_pin)\
  delay100ns();\
  digitalWrite(clock_pin, LOW);

#define SendCmdShiftOut(clock_pin)\
  delay100ns();\
  digitalWrite(clock_pin, HIGH);

void ReadInput(uint8_t *data, int num)
{
  int i;
  SendCmdTakeSample(CLOCK, LATCH);

  // Read output
  for (i = 0; i < num; i++) {
    SendCmdSendData(CLOCK);

    // 1 = Off, 0 = On... Bit must be inverted
    data[i] = (~digitalRead(DATA)) & 0x1;
    SendCmdShiftOut(CLOCK);
  }
}

int GetHatState()
{

  /*

          0
          UP
      315    45
 270 LT        RT 90
      225    135
          DN
          180

  */

  uint8_t x, y;
  const static int16_t dpad_lookup[4][4] = {
    { -1, 270,  90, -1},
    {  0, 315,  45, -1},
    {180, 225, 135, -1},
    { -1,  -1,  -1, -1}
  };

  y = BUTTON_STATE[NES_DPAD_UP] | (BUTTON_STATE[NES_DPAD_DOWN] << 1);
  x = BUTTON_STATE[NES_DPAD_LEFT] | (BUTTON_STATE[NES_DPAD_RIGHT] << 1);

  return dpad_lookup[y][x];
}

void ProcessInputNES()
{
  int i, hat0;

  ReadInput((uint8_t *)&BUTTON_STATE, 8);
  hat0 = GetHatState();

  for (i = 0; i < 4; i++) {
    Joystick.button(
      NES_JOYSTICK_MAP[i][1],
      BUTTON_STATE[NES_JOYSTICK_MAP[i][0]]);
  }

  Joystick.hat(hat0);
}

void ProcessInputNESUsingKeyboard()
{
  int i;
  ReadInput((uint8_t *)&BUTTON_STATE, 8);

  for (i = 0; i < 8; i++) {
    if (BUTTON_STATE[NES_KEYBOARD_MAP[i][0]]) {
      Keyboard.press(NES_KEYBOARD_MAP[i][1]);
    } else {
      Keyboard.release(NES_KEYBOARD_MAP[i][1]);
    }
  }
}

void ProcessInputSNES()
{
  int i, hat0;

  ReadInput((uint8_t *)&BUTTON_STATE, 16);
  hat0 = GetHatState();

  for (i = 0; i < 8; i++) {
    Joystick.button(
      SNES_JOYSTICK_MAP[i][1],
      BUTTON_STATE[SNES_JOYSTICK_MAP[i][0]]);
  }

  Joystick.hat(hat0);
}

void ProcessInputSNESUsingKeyboard()
{
  int i;
  ReadInput((uint8_t *)&BUTTON_STATE, 16);

  for (i = 0; i < 16; i++) {
    if (BUTTON_STATE[SNES_KEYBOARD_MAP[i][0]]) {
      Keyboard.press(SNES_KEYBOARD_MAP[i][1]);
    } else {
      Keyboard.release(SNES_KEYBOARD_MAP[i][1]);
    }
  }
}

void ConfigureSelf()
{
  int i, d;

  if (DEVICE_TYPE == DEVICE_TYPE_AUTO) {
    // Auto detect device type
    SendCmdTakeSample(CLOCK, LATCH);

    for (i = 0; i < 16; i++) {
      SendCmdSendData(CLOCK);
      d = digitalRead(DATA);

      if (i < 8 && d == 1) {
        DEVICE_TYPE = DEVICE_TYPE_NES;
      } else if (i > 7 && d == 1) {
        DEVICE_TYPE = DEVICE_TYPE_SNES;
      }

      SendCmdShiftOut(CLOCK);
    }
  };

  if (DEVICE_TYPE == DEVICE_TYPE_NES) {
    if (DEVICE_METHOD == DEVICE_METHOD_JOYSTICK) {
      // NES, Joystick
      PROCESS_INPUT_FN = &ProcessInputNES;
    } else {
      // NES, Keybaord
      PROCESS_INPUT_FN = &ProcessInputNESUsingKeyboard;
    }
  } else {
    if (DEVICE_METHOD == DEVICE_METHOD_JOYSTICK) {
      // SNES, Joystick
      PROCESS_INPUT_FN = &ProcessInputSNES;
    } else {
      // SNES, Keybaord
      PROCESS_INPUT_FN = &ProcessInputSNESUsingKeyboard;
    }
  }
}

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

  ConfigureSelf();
}

void loop() {
  if (DEVICE_TYPE == DEVICE_TYPE_AUTO) {
    ConfigureSelf();
  }

  PROCESS_INPUT_FN();
  delayMicroseconds(1);
}