#include <Servo.h>

// Define pins
const int ledPin = 13;
const int btn1Pin = 22;  // Using internal pull-up: pressed = LOW.
const int btn2Pin = 23;
const int servo1Pin = 44;
const int servo2Pin = 45;

// Define debounce delay (in ms)
const unsigned long debounceDelay = 150;  
const unsigned long stateHoldTime = 2000;  // 2 seconds hold for given state

// Servo objects
Servo servo1;
Servo servo2;

// State machine for system modes
enum SystemState {
  IDLE,   // initial state, both servos at 0, LED off.
  BTN1,   // btn1 pressed => servo1 90°, LED on.
  BTN2,   // btn2 pressed => servo2 180°, LED off.
  CONFLICT  // conflict state: both servos at 0, LED flashing.
};

SystemState currentState = IDLE;
unsigned long stateStartTime = 0;

// Variables for debouncing buttons
unsigned long lastDebounceTimeBtn1 = 0;
unsigned long lastDebounceTimeBtn2 = 0;

void setup() {
  // Set LED pin as output.
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, LOW);  // LED off at startup
  
  // Set button pins as input with internal pull-up.
  pinMode(btn1Pin, INPUT_PULLUP);
  pinMode(btn2Pin, INPUT_PULLUP);
  
  // Attach servos to the corresponding pins.
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  
  // Set both servos to 0 degrees at startup.
  servo1.write(0);
  servo2.write(0);
  
  currentState = IDLE;
  stateStartTime = millis();
}

void loop() {
  unsigned long currentTime = millis();
  
  // Read the buttons with simple debounce.
  bool btn1Event = false;
  bool btn2Event = false;
  
  // btn1: if button is pressed (LOW) and debounce time passed => event.
  if(digitalRead(btn1Pin) == LOW){
    if(currentTime - lastDebounceTimeBtn1 > debounceDelay){
      btn1Event = true;
      lastDebounceTimeBtn1 = currentTime;
    }
  }
  
  // btn2
  if(digitalRead(btn2Pin) == LOW){
    if(currentTime - lastDebounceTimeBtn2 > debounceDelay){
      btn2Event = true;
      lastDebounceTimeBtn2 = currentTime;
    }
  }
  
  // State machine implementation
  switch(currentState) {
    case IDLE:
      // Ensure both servos at 0 degrees and LED off.
      servo1.write(0);
      servo2.write(0);
      digitalWrite(ledPin, LOW);
      
      if(btn1Event) {
        currentState = BTN1;
        stateStartTime = currentTime;
        servo1.write(90);
        servo2.write(0);  // remains at 0.
        digitalWrite(ledPin, HIGH);
      }
      else if(btn2Event) {
        currentState = BTN2;
        stateStartTime = currentTime;
        servo1.write(0);
        servo2.write(180);
        digitalWrite(ledPin, LOW);
      }
      break;
      
    case BTN1:
      // In BTN1 state servo1 is at 90 and LED is ON.
      // Hold this state for at least 2 seconds.
      if(currentTime - stateStartTime >= stateHoldTime) {
        // If the other button is pressed then that triggers conflict.
        if(btn2Event) {
          currentState = CONFLICT;
          stateStartTime = currentTime;
          // Reset both servos to 0.
          servo1.write(0);
          servo2.write(0);
        }
        // Optionally, repeated press of btn1 can be ignored.
      }
      break;
      
    case BTN2:
      // In BTN2 state, servo2 is at 180 and LED is OFF.
      if(currentTime - stateStartTime >= stateHoldTime) {
        if(btn1Event) {
          currentState = CONFLICT;
          stateStartTime = currentTime;
          servo1.write(0);
          servo2.write(0);
        }
      }
      break;
      
    case CONFLICT:
      // In conflict state, both servos remain at 0.
      servo1.write(0);
      servo2.write(0);
      
      // LED flashes at 0.25 Hz: on for 2 sec, off for 2 sec.
      if ((currentTime - stateStartTime) % 4000 < 2000) {
         digitalWrite(ledPin, HIGH);
      } else {
         digitalWrite(ledPin, LOW);
      }
      
      // Conflict state is maintained until either button is pressed.
      if(btn1Event) {
         currentState = BTN1;
         stateStartTime = currentTime;
         servo1.write(90);
         servo2.write(0);
         digitalWrite(ledPin, HIGH);
      }
      else if(btn2Event) {
         currentState = BTN2;
         stateStartTime = currentTime;
         servo1.write(0);
         servo2.write(180);
         digitalWrite(ledPin, LOW);
      }
      break;
  } // end switch

  // Small delay to help with debounce and reduce excessive looping.
  delay(10);
}