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

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

// Servo settings
#define SERVO_PIN 13
Servo myServo;
int angle = 90;

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

  // Servo init
  myServo.attach(SERVO_PIN);
  myServo.write(angle);

  // OLED init
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("SSD1306 allocation failed!");
    for (;;); // Stop if OLED not found
  }

  // Splash screen
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(20, 15);
  display.println("ESP32");
  display.setTextSize(1);
  display.setCursor(15, 40);
  display.println("Servo + OLED Demo");
  display.display();
  delay(2000);
}

void loop() {
  // Sweep servo forward
  for (angle = 0; angle <= 180; angle += 10) {
    myServo.write(angle);
    showAngle(angle, "Forward");
    delay(500);
  }

  // Sweep servo backward
  for (angle = 180; angle >= 0; angle -= 10) {
    myServo.write(angle);
    showAngle(angle, "Backward");
    delay(500);
  }
}

void showAngle(int angle, const char* direction) {
  display.clearDisplay();

  // Show direction at the top
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
  display.print("Dir: ");
  display.println(direction);

  // Show angle in big font
  display.setTextSize(2);
  display.setCursor(0, 25);
  display.print("Angle:");
  display.setCursor(0, 45);
  display.println(angle);

  display.display();

  Serial.print("Servo Angle: ");
  Serial.print(angle);
  Serial.print(" | Direction: ");
  Serial.println(direction);
}