#include <Servo.h>

// Define pin assignments
const int btn1Pin = 2;
const int btn2Pin = 3;
const int ledPin = 13;
const int servo1Pin = 9;
const int servo2Pin = 10;

// Initialize servo objects
Servo servo1;
Servo servo2;

// Define system states
enum State {
  INIT,     // Initial state
  BTN1,     // Btn1 pressed
  BTN2,     // Btn2 pressed
  BOTH      // Both buttons were pressed (LED flashing)
};

// Initialize state
State currentState = INIT;

// Variables for button debouncing
unsigned long lastBtn1Press = 0;
unsigned long lastBtn2Press = 0;
const unsigned long debounceTime = 150; // 0.15 seconds debounce time

// Button state variables
bool btn1PrevState = false;
bool btn2PrevState = false;

// Variables for state timing
unsigned long stateStartTime = 0;
const unsigned long stateMinDuration = 2000; // Minimum 2 seconds for state verification

// Variables for LED flashing
unsigned long lastLedToggle = 0;
bool ledState = false;
const unsigned long ledFlashInterval = 2000; // 0.25 Hz (2 seconds on, 2 seconds off)

void setup() {
  // Initialize pin modes
  pinMode(btn1Pin, INPUT_PULLUP);
  pinMode(btn2Pin, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);
  
  // Attach servos to their pins
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);
  
  // Initialize servos to 0 degrees
  servo1.write(0);
  servo2.write(0);
  
  // Initialize LED to off
  digitalWrite(ledPin, LOW);
  
  // Set initial state time
  stateStartTime = millis();
}

void loop() {
  // Current time
  unsigned long currentTime = millis();
  
  // Current button states (true when pressed)
  bool btn1State = digitalRead(btn1Pin) == LOW;
  bool btn2State = digitalRead(btn2Pin) == LOW;
  
  // Check if minimum state duration has passed
  bool canChangeState = (currentTime - stateStartTime >= stateMinDuration);
  
  // Detect button press events with debouncing
  if (canChangeState) {
    if (btn1State && !btn1PrevState && (currentTime - lastBtn1Press > debounceTime)) {
      // Button 1 was just pressed
      lastBtn1Press = currentTime;
      
      switch (currentState) {
        case INIT:
        case BOTH:
          transitionToBtn1State();
          break;
        case BTN2:
          transitionToBothState();
          break;
        // If already in BTN1 state, do nothing
        default:
          break;
      }
    }
    
    if (btn2State && !btn2PrevState && (currentTime - lastBtn2Press > debounceTime)) {
      // Button 2 was just pressed
      lastBtn2Press = currentTime;
      
      switch (currentState) {
        case INIT:
        case BOTH:
          transitionToBtn2State();
          break;
        case BTN1:
          transitionToBothState();
          break;
        // If already in BTN2 state, do nothing
        default:
          break;
      }
    }
  }
  
  // Handle LED flashing in BOTH state
  if (currentState == BOTH) {
    if (currentTime - lastLedToggle >= ledFlashInterval) {
      ledState = !ledState;
      digitalWrite(ledPin, ledState ? HIGH : LOW);
      lastLedToggle = currentTime;
    }
  }
  
  // Update previous button states
  btn1PrevState = btn1State;
  btn2PrevState = btn2State;
}

// Transition to BTN1 state
void transitionToBtn1State() {
  currentState = BTN1;
  stateStartTime = millis();
  
  // Set servo1 to 90 degrees
  servo1.write(90);
  
  // Set servo2 to 0 degrees
  servo2.write(0);
  
  // Turn on LED
  digitalWrite(ledPin, HIGH);
}

// Transition to BTN2 state
void transitionToBtn2State() {
  currentState = BTN2;
  stateStartTime = millis();
  
  // Set servo1 to 0 degrees
  servo1.write(0);
  
  // Set servo2 to 180 degrees
  servo2.write(180);
  
  // Turn off LED
  digitalWrite(ledPin, LOW);
}

// Transition to BOTH state
void transitionToBothState() {
  currentState = BOTH;
  stateStartTime = millis();
  
  // Reset both servos to 0 degrees
  servo1.write(0);
  servo2.write(0);
  
  // Initialize LED flashing variables
  ledState = true;
  digitalWrite(ledPin, HIGH);
  lastLedToggle = millis();
}