#include <ESP32Servo.h>

// LED Pins
const int ledPins[5] = {13, 12, 11, 10, 9};

// Button Pins
const int buttonPin1 = 17; // left to right
const int buttonPin2 = 16; // right to left

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

// State: 0 = idle, 1 = left to right, 2 = right to left
int state = 0;

// Debounce
int lastState1 = HIGH;
int lastState2 = HIGH;
unsigned long lastPressTime1 = 0;
unsigned long lastPressTime2 = 0;
const unsigned long debounceDelay = 50;

// LED timing
unsigned long lastLedUpdate = 0;
const unsigned long ledInterval = 300;
int leftPos = 0; //initial position of led and servo
int rightPos = 4; 

void setup() {
  pinMode(buttonPin1, INPUT);
  pinMode(buttonPin2, INPUT);

  for (int i = 0; i < 5; i++) pinMode(ledPins[i], OUTPUT);

  myServo.attach(servoPin);
  myServo.write(90); // center
}

void loop() {
  int currentState1 = digitalRead(buttonPin1);
  int currentState2 = digitalRead(buttonPin2);
  unsigned long currentMillis = millis();
  // FUNCTION /Ensures that a uniform state is implemented throughout/
  // BUTTON 1: LEFT-TO-RIGHT
  if (currentState1 == LOW && lastState1 == HIGH && currentMillis - lastPressTime1 > debounceDelay) {
    lastPressTime1 = currentMillis;
    if (state == 0) { //initial (idle)       
      state = 1; //set state to 1 (left to right)
      leftPos = 0;
      turnOffAll();
    } else if (state == 1) {
      state = 0; //loop back to 0 (idle)
      turnOffAll();
    }
  }

  // BUTTON 2: RIGHT-TO-LEFT
  if (currentState2 == LOW && lastState2 == HIGH && currentMillis - lastPressTime2 > debounceDelay) {
    lastPressTime2 = currentMillis;
    if (state == 0) { //initial (idle)
      state = 2; //set state to 2 (right to left)
      rightPos = 4;
      turnOffAll();
    } else if (state == 2) {
      state = 0; //loop back to 0 (idle)
      turnOffAll();
    }
  }

  lastState1 = currentState1;
  lastState2 = currentState2;

  // LED and Servo update
  if (currentMillis - lastLedUpdate >= ledInterval) {
    lastLedUpdate = currentMillis;

    if (state == 1) { // left-to-right
      digitalWrite(ledPins[leftPos], HIGH);
      digitalWrite(ledPins[(leftPos == 0 ? 4 : leftPos - 1)], LOW);
      leftPos = (leftPos + 1) % 5;

      // Servo moves 180 → 0
      int servoPos = map(leftPos,0,4,0,180);
      myServo.write(servoPos);
    }

    if (state == 2) { // right-to-left
      digitalWrite(ledPins[rightPos], HIGH);
      digitalWrite(ledPins[(rightPos == 4 ? 0 : rightPos + 1)], LOW);
      rightPos = (rightPos - 1 + 5) % 5;

      // Servo moves 0 → 180
      int servoPos = map(rightPos,4,0,180,0);
      myServo.write(servoPos);
    }
  }
}

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