#include <ESP32Servo.h>

#define Green_PIN 0
#define Red_PIN 2
#define SERVO_PIN 23

int ledPins[] = {14, 27, 26, 25, 33};
const int leds = 5;

int mode = 0;

int lastGreen = HIGH, lastRed = HIGH;
unsigned long lastPressGreen = 0, lastPressRed = 0;
const unsigned long debounce = 50;

int k = 0;
unsigned long lastStep = 0;
unsigned long step = 150;

Servo myServo;
const int SERVO_LEFT = 0, SERVO_RIGHT = 180, SERVO_CENTER = 90;

void clearLEDs() {
  for (int i = 0; i < leds; i++) digitalWrite(ledPins[i], LOW);
}

void updateServo() {
  if (mode == 2) myServo.write(SERVO_RIGHT);
  else if (mode == 1) myServo.write(SERVO_LEFT);
  else myServo.write(SERVO_CENTER);
}

void setup() {
  Serial.begin(115200);
  for (int i = 0; i < leds; i++) {
    pinMode(ledPins[i], OUTPUT);
    digitalWrite(ledPins[i], LOW);
  }
  pinMode(Green_PIN, INPUT_PULLUP);
  pinMode(Red_PIN, INPUT_PULLUP);
  myServo.attach(SERVO_PIN, 500, 2400);
  myServo.write(SERVO_CENTER);
}

void loop() {
  int Green = digitalRead(Green_PIN);
  if (lastGreen == HIGH && Green == LOW && millis() - lastPressGreen > debounce) {
    lastPressGreen = millis();
    if (mode == 1) { mode = 0; clearLEDs(); Serial.println("Green Mode: OFF"); }
    else { mode = 1; k = leds - 1; lastStep = millis(); Serial.println("Green: Right to Left"); }
    updateServo();
  }
  lastGreen = Green;

  int Red = digitalRead(Red_PIN);
  if (lastRed == HIGH && Red == LOW && millis() - lastPressRed > debounce) {
    lastPressRed = millis();
    if (mode == 2) { mode = 0; clearLEDs(); Serial.println("Red Mode: OFF"); }
    else { mode = 2; k = 0; lastStep = millis(); Serial.println("Red: Left to Right"); }
    updateServo();
  }
  lastRed = Red;

  if (mode == 1 || mode == 2) {
    if (millis() - lastStep >= step) {
      lastStep = millis();
      clearLEDs();
      digitalWrite(ledPins[k], HIGH);
      if (mode == 1) k = (k - 1 + leds) % leds;
      else k = (k + 1) % leds;
    }
  } else {
    clearLEDs();
  }
}