#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Joysticks
#define JOY1_X 34
#define JOY1_Y 35
#define JOY2_X 33
#define JOY2_Y 32

// Pots
#define POT1 25
#define POT2 26

// Buttons
#define SW1 27
#define SW2 14
#define SW3 12
#define SW4 13

void setup() {
  Serial.begin(115200);

  pinMode(SW1, INPUT_PULLUP);
  pinMode(SW2, INPUT_PULLUP);
  pinMode(SW3, INPUT_PULLUP);
  pinMode(SW4, INPUT_PULLUP);

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  display.clearDisplay();
}

void loop() {
  int j1x = analogRead(JOY1_X);
  int j1y = analogRead(JOY1_Y);
  int j2x = analogRead(JOY2_X);
  int j2y = analogRead(JOY2_Y);
  int p1  = analogRead(POT1);
  int p2  = analogRead(POT2);

  bool b1 = !digitalRead(SW1);
  bool b2 = !digitalRead(SW2);
  bool b3 = !digitalRead(SW3);
  bool b4 = !digitalRead(SW4);

  // Serial Debug
  Serial.printf("J1(%d,%d) J2(%d,%d) P1:%d P2:%d SW:%d%d%d%d\n",
                j1x, j1y, j2x, j2y, p1, p2, b1, b2, b3, b4);

  // OLED Display
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.printf("J1: X=%d Y=%d\n", j1x, j1y);
  display.printf("J2: X=%d Y=%d\n", j2x, j2y);
  display.printf("P1=%d P2=%d\n", p1, p2);
  display.printf("SW: %d %d %d %d\n", b1, b2, b3, b4);
  display.display();

  delay(200);
}