#include <U8g2lib.h>
#include <Servo.h>

// Use the hardware I2C interface with full framebuffer
U8G2_SSD1306_128X64_NONAME_F_HW_I2C display(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);

Servo myServo;

// Pin Definitions
const int VRx = A0;
const int VRy = A1;
const int SW = D2;
const int LED1 = D3;
const int LED2 = D4;
const int servoPin = D9;

void setup() {
  pinMode(SW, INPUT_PULLUP);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);

  myServo.attach(servoPin);

  display.begin();
  display.clearBuffer();          // Clear internal memory
  display.setFont(u8g2_font_6x10_tr);  // Set a readable font
  display.drawStr(0, 12, "Joystick Interface");
  display.sendBuffer();           // Transfer to display
  delay(1000);
}

void loop() {
  int xVal = analogRead(VRx);    // 0-4095
  int yVal = analogRead(VRy);    // 0-4095
  int swVal = digitalRead(SW);   // Button

  int angle = map(xVal, 0, 4095, 0, 180);
  myServo.write(angle);

  // LED Control based on Y position
  if (yVal < 1000) {
    digitalWrite(LED1, HIGH);
    digitalWrite(LED2, LOW);
  } else if (yVal > 3000) {
    digitalWrite(LED1, LOW);
    digitalWrite(LED2, HIGH);
  } else {
    digitalWrite(LED1, LOW);
    digitalWrite(LED2, LOW);
  }

  // Display joystick info
  display.clearBuffer();                  // Clear internal buffer
  display.setCursor(0, 12);
  display.print("X: "); display.println(xVal);
  display.setCursor(0, 24);
  display.print("Y: "); display.println(yVal);
  display.setCursor(0, 36);
  display.print("Angle: "); display.println(angle);
  display.setCursor(0, 48);
  display.print("Button: ");
  display.println(swVal == LOW ? "Pressed" : "Released");

  display.sendBuffer();                   // Transfer buffer to display
  delay(100);
}