#include <PS2X_lib.h>
#include <XInput.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define PS2_CMD 15
#define PS2_CLK 14
#define PS2_DAT 16
#define PS2_ATT 10

#define pressures false
#define rumble false

#define OLED_RESET     -1
#define SCREEN_ADDRESS 0x3C

PS2X ps2x;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

int error = -1;
byte type = 0;
byte vibrate = 0;
byte waitTick = 0;
char waitTextOutput[32];
bool isControllerConnected;

int lx, ly, rx, ry;
int deadzone = 20;

void setup() {
  Serial.begin(57600);
  delay(300);

  pinMode(17, OUTPUT);

  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  
  display.setTextColor(WHITE);
  display.cp437(true);
  display.setTextSize(2);
  display.display();
}

void loop() {
  if (!(error == 0 && type == 0)) {
    checkControllerConnection();
    XInput.setJoystick(JOY_LEFT, 0, 0);
    XInput.setJoystick(JOY_RIGHT, 0, 0);
    delay(50);
    return;
  }
  
  isControllerConnected = ps2x.read_gamepad(false, vibrate);
  if (!isControllerConnected) {
    error = -1;
  }

  // Face buttons
  // Triangle ∆
  if (ps2x.ButtonPressed(PSB_TRIANGLE)) {
    XInput.press(BUTTON_Y);
  }

  if (ps2x.ButtonReleased(PSB_TRIANGLE)) {
    XInput.release(BUTTON_Y);
  }

  // Circle O
  if (ps2x.ButtonPressed(PSB_CIRCLE)) {
    XInput.press(BUTTON_B);
  }

  if (ps2x.ButtonReleased(PSB_CIRCLE)) {
    XInput.release(BUTTON_B);
  }

  // Square □
  if (ps2x.ButtonPressed(PSB_SQUARE)) {
    XInput.press(BUTTON_X);
  }

  if (ps2x.ButtonReleased(PSB_SQUARE)) {
    XInput.release(BUTTON_X);
  }

  // Cross X
  if (ps2x.ButtonPressed(PSB_CROSS)) {
    XInput.press(BUTTON_A);
  }

  if (ps2x.ButtonReleased(PSB_CROSS)) {
    XInput.release(BUTTON_A);
  }

  // Shoulder buttons
  // L1
  if (ps2x.ButtonPressed(PSB_L1)) {
    XInput.press(BUTTON_LB);
  }

  if (ps2x.ButtonReleased(PSB_L1)) {
    XInput.release(BUTTON_LB);
  }

  // L2
  if (ps2x.ButtonPressed(PSB_L2)) {
    XInput.press(TRIGGER_LEFT);
  }

  if (ps2x.ButtonReleased(PSB_L2)) {
    XInput.release(TRIGGER_LEFT);
  }

  // R1
  if (ps2x.ButtonPressed(PSB_R1)) {
    XInput.press(BUTTON_RB);
  }

  if (ps2x.ButtonReleased(PSB_R1)) {
    XInput.release(BUTTON_RB);
  }

  // R2
  if (ps2x.ButtonPressed(PSB_R2)) {
    XInput.press(TRIGGER_RIGHT);
  }

  if (ps2x.ButtonReleased(PSB_R2)) {
    XInput.release(TRIGGER_RIGHT);
  }

  // Special buttons
  // Start
  if (ps2x.ButtonPressed(PSB_START)) {
    XInput.press(BUTTON_START);
  }

  if (ps2x.ButtonReleased(PSB_START)) {
    XInput.release(BUTTON_START);
  }

  // Select
  if (ps2x.ButtonPressed(PSB_SELECT)) {
    XInput.press(BUTTON_BACK);
  }

  if (ps2x.ButtonReleased(PSB_SELECT)) {
    XInput.release(BUTTON_BACK);
  }

  // Analogue sticks
  // L3
  if (ps2x.ButtonPressed(PSB_L3)) {
    XInput.press(BUTTON_L3);
  }

  if (ps2x.ButtonReleased(PSB_L3)) {
    XInput.release(BUTTON_L3);
  }

  // R3
  if (ps2x.ButtonPressed(PSB_R3)) {
    XInput.press(BUTTON_R3);
  }

  if (ps2x.ButtonReleased(PSB_R3)) {
    XInput.release(BUTTON_R3);
  }

  // D-Pad
  // D-Left
  if (ps2x.ButtonPressed(PSB_PAD_LEFT)) {
    XInput.press(DPAD_LEFT);
  }

  if (ps2x.ButtonReleased(PSB_PAD_LEFT)) {
    XInput.release(DPAD_LEFT);
  }
  // D-Right
  if (ps2x.ButtonPressed(PSB_PAD_RIGHT)) {
    XInput.press(DPAD_RIGHT);
  }

  if (ps2x.ButtonReleased(PSB_PAD_RIGHT)) {
    XInput.release(DPAD_RIGHT);
  }
  // D-Up
  if (ps2x.ButtonPressed(PSB_PAD_UP)) {
    XInput.press(DPAD_UP);
  }

  if (ps2x.ButtonReleased(PSB_PAD_UP)) {
    XInput.release(DPAD_UP);
  }
  // D-Down
  if (ps2x.ButtonPressed(PSB_PAD_DOWN)) {
    XInput.press(DPAD_DOWN);
  }

  if (ps2x.ButtonReleased(PSB_PAD_DOWN)) {
    XInput.release(DPAD_DOWN);
  }

  // Left stick
  lx = ps2x.Analog(PSS_LX) - 128;
  ly = 127 - ps2x.Analog(PSS_LY);
  if (lx < deadzone && lx > -deadzone && ly < deadzone && ly > -deadzone) {
    lx = 0;
    ly = 0;
  }
  XInput.setJoystick(JOY_LEFT, lx, ly);

  // Right stick
  rx = ps2x.Analog(PSS_RX) - 128;
  ry = 127 - ps2x.Analog(PSS_RY);
  if (rx < deadzone && rx > -deadzone && ry < deadzone && ry > -deadzone) {
    rx = 0;
    ry = 0;
  }
  XInput.setJoystick(JOY_RIGHT, rx, ry);
}

void checkControllerConnection() {
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_ATT, PS2_DAT, pressures, rumble);

  display.setCursor(0, 0);
  display.clearDisplay();
  switch (error) {
    case 0:
      type = ps2x.readType();
      if (type == 0) {
        display.write("DualShock\n2\nconnected!");
        XInput.setJoystickRange(-128, 127);
      } else {
        display.write("Controller\nnot\nrecognised");
      }
      break;
    default:
      memset(waitTextOutput, 0, sizeof(waitTextOutput));
      strcat(waitTextOutput, "Looking\nfor\ncontroller");
      for (byte i = 0; i < waitTick; i += 1) {
        strcat(waitTextOutput, ".");
      }
      display.write(waitTextOutput);

      waitTick += 1;
      if (waitTick == 4) {
        waitTick = 0;
      }
      break;
  }
  display.display();
}