#include <Wire.h>
#include <SSD1306Ascii.h>
#include <SSD1306AsciiWire.h>
#include <Servo.h>

// OLED
SSD1306AsciiWire oled;

// Joystick
const int joyX = PA0;
const int joyY = PA1;
const int joyBtn = PA2;

// LEDs (updated pins)
const int ledUP    = PA5;
const int ledDOWN  = PA4;
const int ledLEFT  = PB2;
const int ledRIGHT = PB3;

// Servo
const int servoPin = PA8;
Servo myServo;

void setup() {
  // Joystick button input
  pinMode(joyBtn, INPUT_PULLUP);

  // LED pins
  pinMode(ledUP, OUTPUT);
  pinMode(ledDOWN, OUTPUT);
  pinMode(ledLEFT, OUTPUT);
  pinMode(ledRIGHT, OUTPUT);

  // Servo setup
  myServo.attach(servoPin);

  // OLED setup
  Wire.begin(); // SDA = PA10, SCL = PA9
  oled.begin(&Adafruit128x64, 0x3C);
  oled.setFont(System5x7);
  oled.clear();
}

void loop() {
  int xVal = analogRead(joyX);
  int yVal = analogRead(joyY);
  bool btnPressed = digitalRead(joyBtn) == LOW;

  // Servo control (X-axis)
  int angle = map(xVal, 0, 4095, 0, 180);
  myServo.write(angle);

  // Joystick direction detection
  int dx = xVal - 2048;
  int dy = yVal - 2048;
  int threshold = 600;

  // Reset all LEDs
  digitalWrite(ledUP, LOW);
  digitalWrite(ledDOWN, LOW);
  digitalWrite(ledLEFT, LOW);
  digitalWrite(ledRIGHT, LOW);

  // Activate only one LED based on strongest direction
  if (abs(dx) > abs(dy) && abs(dx) > threshold) {
    if (dx < 0) digitalWrite(ledLEFT, HIGH);
    else        digitalWrite(ledRIGHT, HIGH);
  } else if (abs(dy) > threshold) {
    if (dy < 0) digitalWrite(ledDOWN, HIGH);
    else        digitalWrite(ledUP, HIGH);
  }

  // OLED output
  oled.clear();
  oled.setCursor(0, 0);
  oled.print("Angle: ");
  oled.println(angle);

  oled.print("Dir: ");
  if (digitalRead(ledUP)) oled.print("UP ");
  if (digitalRead(ledDOWN)) oled.print("DOWN ");
  if (digitalRead(ledLEFT)) oled.print("LEFT ");
  if (digitalRead(ledRIGHT)) oled.print("RIGHT ");
  if (!digitalRead(ledUP) && !digitalRead(ledDOWN) &&
      !digitalRead(ledLEFT) && !digitalRead(ledRIGHT)) {
    oled.print("CENTER");
  }

  oled.setCursor(0, 40);
  oled.print("Btn: ");
  oled.println(btnPressed ? "Pressed" : "Released");

  delay(100);
}