#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 = 0;
byte type = 0;
byte vibrate = 0;

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

  // TODO check in loop to detect disconnected and reconnected controllers
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_ATT, PS2_DAT, pressures, rumble);

  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.clearDisplay();
  display.setCursor(0, 0);
  display.write("hello ps2!");
  display.display();

  switch (error) {
    case 0:
      onControllerDetected();
      break;
    case 1:
      Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
      break;
    case 2:
      Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
      break;
    case 3:
      Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
      break;
  }

  type = ps2x.readType();
  if (type == 0) {
    Serial.println("DualShock controller found");
    digitalWrite(17, HIGH);
    XInput.setJoystickRange(0, 255);
  }
}

void loop() {
  if (error == 1 || type != 0) {
    return; // Skip loop on error
  }

  ps2x.read_gamepad(false, vibrate);

  // 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);
  }

  // Left stick
  XInput.setJoystick(JOY_LEFT, ps2x.Analog(PSS_LX), 255 - ps2x.Analog(PSS_LY));

  // Right stick
  XInput.setJoystick(JOY_RIGHT, ps2x.Analog(PSS_RX), 255 - ps2x.Analog(PSS_RY));
}

void onControllerDetected() {
  Serial.print("Found Controller, configured successful ");
  Serial.print("pressures = ");
	if (pressures) {
	  Serial.println("true ");
	} else {
	  Serial.println("false");
  }
	Serial.print("rumble = ");
	if (rumble) {
	  Serial.println("true)");
	} else {
	  Serial.println("false");
  }
}