#include "Joystick2.h"

/*
#define ATARI
//DB9 (8=GND):                      1   2   3   4   5   6   7   8   9
const uint8_t inputPinsPort1[] =  {10, 16, 14, 15, A1,  0,  0,  0,  3};
const uint8_t inputPinsPort2[] =  { 5,  6,  7,  8, A2,  0,  0,  0,  4};
*/

/*
//#define NES
#define SNES
//Connector (Connect also GND and 5V):  CUP, OUT0,     D1
const uint8_t inputPinsPort1[] =       {  7,     8,     9 };
const uint8_t inputPinsPort2[] =       {  2,     3,     4 };
*/

/*
#define GENESIS_3
//#define GENESIS_6
//DB9 (8=GND, 5=VCC):               1   2   3   4   5   6   7   8   9
const uint8_t inputPinsPort1[] =  {10, 16, 14, 15, A0,  3, A3,  0, A1};
const uint8_t inputPinsPort2[] =  { 5,  6,  7,  8,  9,  4,  2,  0, A2};
*/

#define PSX
//PSX:                             DATA  CMD  ATT  CLK
const uint8_t inputPinsPort1[] =  {   2,    3,   4,   5 };
const uint8_t inputPinsPort2[] =  {   6,    7,   8,   9 };



//#define KONAMI


void KonamiCode(uint8_t j, uint8_t swap_ab = 0);

//-----PSX/PLAYSTATION-----
#ifdef PSX

#include <Psx.h>
Psx Psx;

//#define EVENTS_TOTAL 4+2 //4 directions, 2 fire-buttons
#define BITS 16
#define EVENTS_TOTAL BITS

void setupJoysticks() {
  /*pinMode(inputPinsPort1[0], INPUT);
  pinMode(inputPinsPort2[0], INPUT);
  pinMode(inputPinsPort1[1], OUTPUT);
  pinMode(inputPinsPort2[1], OUTPUT);
  pinMode(inputPinsPort1[2], OUTPUT);
  pinMode(inputPinsPort2[2], OUTPUT);
  pinMode(inputPinsPort1[3], OUTPUT);
  pinMode(inputPinsPort2[3], OUTPUT);*/
  Psx.setupPins(inputPinsPort1[0], inputPinsPort1[1], inputPinsPort1[2], inputPinsPort1[3], 10);
}

void readJoysticks(uint8_t *status1, uint8_t *status2) {
  uint16_t data = Psx.read();
  for (uint8_t i=0; i < BITS; i++) {
    status1[i] = ~bitRead(data,i) & 1;
    status2[i] = 1;
  }
}

void interpretJoystickState(uint8_t j, uint8_t *status) {
  Joystick[j].setYAxis(0);
  Joystick[j].setXAxis(0);
  if (!status[3]) Joystick[j].setYAxis(-127); //UP (0)
  if (!status[1]) Joystick[j].setYAxis(127); //DOWN (1)
  if (!status[0]) Joystick[j].setXAxis(-127); //LEFT (2)
  if (!status[2]) Joystick[j].setXAxis(127); //RIGHT (3)
  Joystick[j].setButton(0, !status[4]); //BUTTON1 (Start)
  Joystick[j].setButton(1, !status[7]); //BUTTON2 (Select)
  Joystick[j].setButton(2, !status[9]); //BUTTON3 (A)
  Joystick[j].setButton(3, !status[10]); //BUTTON4 (B)
  Joystick[j].setButton(4, !status[8]); //BUTTON5 (X)
  Joystick[j].setButton(5, !status[11]); //BUTTON6 (Y)
  Joystick[j].setButton(6, !status[15]); //BUTTON7 (LB)  
  Joystick[j].setButton(7, !status[14]); //BUTTON8 (RB)
  Joystick[j].setButton(8, !status[13]); //BUTTON9 (LT)
  Joystick[j].setButton(9, !status[12]); //BUTTON10 (RT)
}


#endif
//--------ATARI/SMS--------
#ifdef ATARI

#define EVENTS_TOTAL 4+2 //4 directions, 2 fire-buttons

void setupJoysticks() {
  for (int i=0; i < 9; i++) {
    pinMode(inputPinsPort1[i], INPUT_PULLUP);
    pinMode(inputPinsPort2[i], INPUT_PULLUP);
  }
}

void readJoysticks(uint8_t *status1, uint8_t *status2) {
  for (uint8_t i=0; i < 4; i++) {
    status1[i] = digitalRead(inputPinsPort1[i]); //AXES1
    status2[i] = digitalRead(inputPinsPort2[i]); //AXES2
  }
  status1[4] = digitalRead(inputPinsPort1[8]); //A1
  status1[5] = digitalRead(inputPinsPort1[4]); //B1
  status2[4] = digitalRead(inputPinsPort2[8]); //A2
  status2[5] = digitalRead(inputPinsPort2[4]); //B2
}

void interpretJoystickState(uint8_t j, uint8_t *status) {
  Joystick[j].setYAxis(0);
  Joystick[j].setXAxis(0);
  if (!status[0]) Joystick[j].setYAxis(-127); //UP
  if (!status[1]) Joystick[j].setYAxis(127); //DOWN
  if (!status[2]) Joystick[j].setXAxis(-127); //LEFT
  if (!status[3]) Joystick[j].setXAxis(127); //RIGHT
  Joystick[j].setButton(2, !status[4]); //BUTTON3 (A)
  Joystick[j].setButton(3, !status[5]); //BUTTON4 (B)
}
#endif


//--------Megadrive/Genesis (6-button)--------
//https://www.cs.cmu.edu/~chuck/infopg/segasix.txt
#if defined(GENESIS_3) || defined(GENESIS_6)

#define MODE_SELECT_PORT1 inputPinsPort1[6]
#define MODE_SELECT_PORT2 inputPinsPort2[6]
#define VCC_PORT1 inputPinsPort1[4]
#define VCC_PORT2 inputPinsPort2[4]

#ifdef GENESIS_3
#define EVENTS_TOTAL 4+3+1 //4 directions, 3 fire-buttons and Start
#else
#define EVENTS_TOTAL 4+6+1 //4 directions, 6 fire-buttons and Start
#endif

void modeSelect(uint8_t m) {
  digitalWrite(MODE_SELECT_PORT1, m);
  digitalWrite(MODE_SELECT_PORT2, m);
  delayMicroseconds(20);
}

void setupJoysticks() {
  if (VCC_PORT1 != 0) {
    pinMode(VCC_PORT1, OUTPUT);
    digitalWrite(VCC_PORT1, HIGH);
  }
  if (VCC_PORT2 != 0) {
    pinMode(VCC_PORT2, OUTPUT);
    digitalWrite(VCC_PORT2, HIGH);
  }

  const uint8_t inputlist[] = {0,1,2,3,5,8};
  for (int i=0; i < 6; i++) {
    pinMode(inputPinsPort1[inputlist[i]], INPUT);
    pinMode(inputPinsPort2[inputlist[i]], INPUT);
  }
  
  pinMode(MODE_SELECT_PORT1, OUTPUT);
  pinMode(MODE_SELECT_PORT2, OUTPUT);
  modeSelect(HIGH);
}

void readJoysticks(uint8_t *status1, uint8_t *status2) {
  modeSelect(LOW);
  
  status1[4] = digitalRead(inputPinsPort1[5]); //A1
  status1[5] = digitalRead(inputPinsPort1[8]); //Start1
  status2[4] = digitalRead(inputPinsPort2[5]); //A2
  status2[5] = digitalRead(inputPinsPort2[8]); //Start2  

  modeSelect(HIGH);

  for (uint8_t i=0; i < 4; i++) {
    status1[i] = digitalRead(inputPinsPort1[i]); //AXES1
    status2[i] = digitalRead(inputPinsPort2[i]); //AXES2
  }
  status1[6] = digitalRead(inputPinsPort1[5]); //B1
  status1[7] = digitalRead(inputPinsPort1[8]); //C1
  status2[6] = digitalRead(inputPinsPort2[5]); //B2
  status2[7] = digitalRead(inputPinsPort2[8]); //C2

  #ifdef GENESIS_6
  //read X,Y,Z
  modeSelect(LOW);
  modeSelect(HIGH);
  modeSelect(LOW);
  modeSelect(HIGH);
  status1[8] = digitalRead(inputPinsPort1[2]); //X1
  status1[9] = digitalRead(inputPinsPort1[1]); //Y1
  status1[10] = digitalRead(inputPinsPort1[0]); //Z1
  status2[8] = digitalRead(inputPinsPort2[2]); //X2
  status2[9] = digitalRead(inputPinsPort2[1]); //Y2
  status2[10] = digitalRead(inputPinsPort2[0]); //Z2
  #endif
  
  delayMicroseconds(1000);
}

void interpretJoystickState(uint8_t j, uint8_t *status) {

  #ifdef KONAMI
  if (!status[8] && !status[9] && !status[10] && !status[0]) { //X+Y+Z+UP
    KonamiCode(0,0);
    return;
  }
  if (!status[8] && !status[9] && !status[10] && !status[1]) { //X+Y+Z+DOWN
    KonamiCode(0,1);
    return;
  }
  #endif

  Joystick[j].setYAxis(0);
  Joystick[j].setXAxis(0);
  if (!status[0]) Joystick[j].setYAxis(-127); //UP
  if (!status[1]) Joystick[j].setYAxis(127); //DOWN
  if (!status[2]) Joystick[j].setXAxis(-127); //LEFT
  if (!status[3]) Joystick[j].setXAxis(127); //RIGHT
  Joystick[j].setButton(0, !status[5]); //BUTTON1 (START)
  Joystick[j].setButton(1, !status[7]); //BUTTON2 (C) ("Select")
  Joystick[j].setButton(2, !status[4]); //BUTTON3 (A)
  Joystick[j].setButton(3, !status[6]); //BUTTON4 (B)
  #ifdef GENESIS_6
  Joystick[j].setButton(4, !status[8]); //BUTTON5 (X)
  Joystick[j].setButton(5, !status[9]); //BUTTON6 (Y)
  Joystick[j].setButton(6, !status[10]); //BUTTON7 (Z) ("LB")
  #endif
}

#endif


//--------NES--------
// http://www.mit.edu/~tarvizo/nes-controller.html
#if defined(NES) || defined(SNES)

#define CLOCK1 inputPinsPort1[0]
#define LATCH1 inputPinsPort1[1]
#define DATA1 inputPinsPort1[2]

#define CLOCK2 inputPinsPort2[0]
#define LATCH2 inputPinsPort2[1]
#define DATA2 inputPinsPort2[2]

#ifdef NES
#define BITS 8
#else
#define BITS 16
#endif

#define EVENTS_TOTAL BITS

void setupJoysticks() {
  pinMode(LATCH1, OUTPUT);
  pinMode(CLOCK1, OUTPUT);
  pinMode(DATA1, INPUT);
  pinMode(LATCH2, OUTPUT);
  pinMode(CLOCK2, OUTPUT);
  pinMode(DATA2, INPUT);
}

#define latchlow digitalWrite(LATCH1, LOW); digitalWrite(LATCH2, LOW);
#define latchhigh digitalWrite(LATCH1, HIGH); digitalWrite(LATCH2, HIGH)
#define clocklow digitalWrite(CLOCK1, LOW); digitalWrite(CLOCK2, LOW)
#define clockhigh digitalWrite(CLOCK1, HIGH); digitalWrite(CLOCK2, HIGH)
#define wait delayMicroseconds(12)


void readJoysticks(uint8_t *status1, uint8_t *status2) {
  latchlow;
  clocklow;
  latchhigh;
  wait;
  latchlow;

  for (int i = 0; i < BITS; i++) {
    status1[i] = digitalRead(DATA1);
    status2[i] = digitalRead(DATA2);
    clockhigh;
    wait;
    clocklow;
    wait;
  }
}

void interpretJoystickState(uint8_t j, uint8_t *status) {

  #ifdef KONAMI
  if (!status[1] && !status[9] && !status[11] && !status[4]) { //X+Y+Z+UP
    KonamiCode(0,0);
    return;
  }
  if (!status[1] && !status[9] && !status[11] && !status[5]) { //X+Y+Z+DOWN
    KonamiCode(0,1);
    return;
  }
  #endif
  
  Joystick[j].setYAxis(0);
  Joystick[j].setXAxis(0);
  if (!status[4]) Joystick[j].setYAxis(-127); //UP
  if (!status[5]) Joystick[j].setYAxis(127); //DOWN
  if (!status[6]) Joystick[j].setXAxis(-127); //LEFT
  if (!status[7]) Joystick[j].setXAxis(127); //RIGHT
  #ifdef NES
  Joystick[j].setButton(0, !status[3]); //BUTTON1 (Start)  
  Joystick[j].setButton(1, !status[2]); //BUTTON2 (Select)
  Joystick[j].setButton(2, !status[0]); //BUTTON3 (A)
  Joystick[j].setButton(3, !status[1]); //BUTTON4 (B)
  #else
  Joystick[j].setButton(0, !status[3]); //BUTTON1 (Start)  
  Joystick[j].setButton(1, !status[2]); //BUTTON2 (Select)
  Joystick[j].setButton(2, !status[8]); //BUTTON3 (A)
  Joystick[j].setButton(3, !status[0]); //BUTTON4 (B)
  Joystick[j].setButton(4, !status[9]); //BUTTON5 (X)
  Joystick[j].setButton(5, !status[1]); //BUTTON6 (Y)
  Joystick[j].setButton(6, !status[10]); //BUTTON7 (L1) ("LB")
  Joystick[j].setButton(7, !status[11]); //BUTTON8 (R1) ("RB")
  #endif
}

#endif



//--------Konami code---------

void releaseAll(uint8_t j) {
  delay(50);
  Joystick[j].setButton(0, 0); //BUTTON1 (Start)
  Joystick[j].setButton(1, 0); //BUTTON2 (Select)
  Joystick[j].setButton(2, 0); //BUTTON3 (A)
  Joystick[j].setButton(3, 0); //BUTTON4 (B)
  Joystick[j].setButton(4, 0); //BUTTON5 (X)
  Joystick[j].setButton(5, 0); //BUTTON6 (Y)
  Joystick[j].setButton(6, 0); //BUTTON7 (LB)
  Joystick[j].setButton(7, 0); //BUTTON6 (RB)
  Joystick[j].setButton(8, 0); //BUTTON7 (LT)
  Joystick[j].setButton(9, 0); //BUTTON6 (RT)
  Joystick[j].setButton(10, 0); //BUTTON7 (L-thumb)
  Joystick[j].setButton(11, 0); //BUTTON7 (R-thumb)
  Joystick[j].setYAxis(0);
  Joystick[j].setXAxis(0);
  Joystick[j].sendState();
  delay(50);
}

void KonamiCode(uint8_t j, uint8_t swap_ab = 0) {
  //https://en.wikipedia.org/wiki/Konami_Code
  //https://en.wikipedia.org/wiki/List_of_Konami_code_games
  //UP, UP, DOWN, DOWN, LEFT, RIGHT, LEFT, RIGHT, B, A
  releaseAll(j);
  delay(200);
  Joystick[j].setYAxis(-127); Joystick[j].sendState(); //UP
  releaseAll(j);
  Joystick[j].setYAxis(-127); Joystick[j].sendState(); //UP
  releaseAll(j);
  Joystick[j].setYAxis(127); Joystick[j].sendState(); //DOWN
  releaseAll(j);
  Joystick[j].setYAxis(127); Joystick[j].sendState(); //DOWN
  releaseAll(j);
  Joystick[j].setXAxis(-127); Joystick[j].sendState(); //LEFT
  releaseAll(j);
  Joystick[j].setXAxis(127); Joystick[j].sendState(); //RIGHT
  releaseAll(j);
  Joystick[j].setXAxis(-127); Joystick[j].sendState(); //LEFT
  releaseAll(j);
  Joystick[j].setXAxis(127); Joystick[j].sendState(); //RIGHT
  releaseAll(j);
  if (swap_ab) {
    Joystick[j].setButton(2, 1); Joystick[j].sendState(); //BUTTON1 (A)
    releaseAll(j);
    Joystick[j].setButton(3, 1); Joystick[j].sendState(); //BUTTON2 (B)
    releaseAll(j);
  } else {
    Joystick[j].setButton(3, 1); Joystick[j].sendState(); //BUTTON2 (B)
    releaseAll(j);
    Joystick[j].setButton(2, 1); Joystick[j].sendState(); //BUTTON1 (A)
    releaseAll(j);
  }
  delay(200);
}



uint8_t lastStatusPort1[EVENTS_TOTAL];
uint8_t newStatusPort1[EVENTS_TOTAL];
uint8_t lastStatusPort2[EVENTS_TOTAL];
uint8_t newStatusPort2[EVENTS_TOTAL];


void setup() {
  //clear statusarrays (1=OFF, 0=ON)
  for (uint8_t i = 0; i < EVENTS_TOTAL; i++) {
    lastStatusPort1[i] = 1;
    newStatusPort1[i] = 1;
    lastStatusPort2[i] = 1;
    newStatusPort2[i] = 1;
  }
  
  setupJoysticks();

  Joystick[0].begin(false);
  Joystick[1].begin(false);
}


uint8_t flag1 = 0;
uint8_t flag2 = 0;


void loop() {

  readJoysticks(newStatusPort1, newStatusPort2);

  //check for changes - do not raise a flag if nothing changes
  for (uint8_t i=0; i < EVENTS_TOTAL; i++) {
    if (newStatusPort1[i] != lastStatusPort1[i]) {
      lastStatusPort1[i] = newStatusPort1[i]; 
      flag1 = 1;
    }
    if (newStatusPort2[i] != lastStatusPort2[i]) {
      lastStatusPort2[i] = newStatusPort2[i]; 
      flag2 = 1;
    }
  }
  
  if (flag1) interpretJoystickState(0, newStatusPort1);
  if (flag2) interpretJoystickState(1, newStatusPort2);

  //minimize port1 time advantage - try to send both states as simultaneous as possible
  if (flag1) Joystick[0].sendState();
  if (flag2) Joystick[1].sendState();
  flag1 = 0;
  flag2 = 0;

}