#include <ESP32Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// --- LCD ---
LiquidCrystal_I2C lcd(0x27, 16, 2); // I2C address 0x27, 16 chars, 2 lines

// --- Pin Configurations ---
const int button1Pin = 33;   // Button 1 (Right-to-Left, Servo 180→0)
const int button2Pin = 25;   // Button 2 (Left-to-Right, Servo 0→180)
const int ledPins[5] = {13, 12, 14, 27, 26}; // LED array
const int servoPin = 32;     // Servo signal pin

// --- Globals ---
Servo myServo;
int ledIndex = 0;                  
int servoPos = 90; 
bool lastButton1 = false;
bool lastButton2 = false;

// LED control
bool chaseActive = false;
bool chaseRightToLeft = true;
unsigned long lastLedUpdate = 0;
const int ledDelay = 200; // ms per LED step

// Servo control
bool servoActive = false;
int servoFrom = 90;
int servoTo = 90;
int servoStep = 1;
unsigned long lastServoUpdate = 0;
const int servoDelay = 0.5; // ms per servo step

void setup() {
  // Setup LEDs
  for (int i = 0; i < 5; i++) {
    pinMode(ledPins[i], OUTPUT);
    digitalWrite(ledPins[i], LOW);
  }

  // Setup buttons
  pinMode(button1Pin, INPUT_PULLUP);
  pinMode(button2Pin, INPUT_PULLUP);

  // Setup servo
  myServo.attach(servoPin);
  myServo.write(servoPos);

  // Setup LCD
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("System Ready");
}

void loop() {
  bool button1 = !digitalRead(button1Pin); 
  bool button2 = !digitalRead(button2Pin);

  // --- Button 1 pressed ---
  if (button1 && !lastButton1) {
    ledIndex = 4; // start from right
    startServoSweep(180, 0);
    chaseActive = true;
    chaseRightToLeft = true;

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Button 1 Pressed");
    lcd.setCursor(0, 1);
    lcd.print(" Left <-- Right");
  }

  // --- Button 2 pressed ---
  else if (button2 && !lastButton2) {
    ledIndex = 0; // start from left
    startServoSweep(0, 180);
    chaseActive = true;
    chaseRightToLeft = false;
    
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Button 2 Pressed");
    lcd.setCursor(0, 1);
    lcd.print(" Left --> Right");
  }

  // Update LEDs (non-blocking)
  if (chaseActive && millis() - lastLedUpdate >= ledDelay) {
    lastLedUpdate = millis();
    chaseLEDs(chaseRightToLeft);
  }

  // Update Servo (non-blocking)
  if (servoActive && millis() - lastServoUpdate >= servoDelay) {
    lastServoUpdate = millis();
    updateServo();
  }

  // Save button states
  lastButton1 = button1;
  lastButton2 = button2;
}

// --- LED Chasing ---
void chaseLEDs(bool rightToLeft) {
  for (int i = 0; i < 5; i++) digitalWrite(ledPins[i], LOW);

  if (rightToLeft) {
    ledIndex = (ledIndex - 1 + 5) % 5;
  } else {
    ledIndex = (ledIndex + 1) % 5;
  }

  digitalWrite(ledPins[ledIndex], HIGH);
}

// --- Start Servo Sweep ---
void startServoSweep(int from, int to) {
  servoFrom = from;
  servoTo = to;
  servoStep = (from < to) ? 1 : -1;
  servoPos = from;
  myServo.write(servoPos);
  servoActive = true;
  lastServoUpdate = millis();

  // Light first LED immediately
  for (int i = 0; i < 5; i++) digitalWrite(ledPins[i], LOW);
  digitalWrite(ledPins[ledIndex], HIGH);
}

// --- Update Servo Position ---
void updateServo() {
  if (servoPos != servoTo) {
    servoPos += servoStep;
    myServo.write(servoPos);
  } else {
    servoActive = false; // sweep finished
  }
}