#include <Servo.h>  // Include the Servo library

// Pin definitions
const int greenButtonPin = 7;
const int redButtonPin = 6;
const int greenLEDPin = 12;
const int redLEDPin = 11;
const int servoPin = A1; // Servo on analog pin A1

// Servo instance
Servo myServo;

// State variables
bool greenLEDOn = false; // Tracks whether the green LED is active
bool redLEDBlinking = false; // Tracks whether the red LED is blinking
unsigned long button2PressedTime = 0;
bool button2Pressed = false;
bool greenLEDSolid = false; // Tracks whether the green LED is solid

// Debounce variables
unsigned long debounceDelay = 50; // 50ms debounce delay
unsigned long lastGreenButtonPressTime = 0;
unsigned long lastRedButtonPressTime = 0;
bool greenButtonState = false;
bool redButtonState = false;

// Green LED blinking and solid timers (now global)
unsigned long greenLEDBlinkTimer = 0;
unsigned long greenLEDSolidTimer = 0;

void setup() {
  // Initialize pins
  pinMode(greenButtonPin, INPUT_PULLUP); // Use INPUT_PULLUP
  pinMode(redButtonPin, INPUT_PULLUP);   // Use INPUT_PULLUP
  pinMode(greenLEDPin, OUTPUT);
  pinMode(redLEDPin, OUTPUT);

  // Start with LEDs off
  digitalWrite(greenLEDPin, LOW);
  digitalWrite(redLEDPin, LOW);

  // Attach and initialize the servo
  myServo.attach(servoPin);
  myServo.write(0); // Move servo to low position (initial state)
}

void loop() {
  // Call the reset function
  handleGreenButtonReset();

  // Debounce Green Button (Button 1)
  bool currentGreenButtonState = !digitalRead(greenButtonPin); // Invert logic for INPUT_PULLUP
  if (currentGreenButtonState != greenButtonState && millis() - lastGreenButtonPressTime > debounceDelay) {
    greenButtonState = currentGreenButtonState;
    lastGreenButtonPressTime = millis();
    if (greenButtonState == HIGH && !greenLEDOn) {
      greenLEDOn = true;       // Arm the system
      greenLEDSolid = false;   // Reset the green LED solid state
      greenLEDBlinkTimer = 0;  // Reset blink timer
      greenLEDSolidTimer = 0;  // Reset solid timer
    }
  }

  // Green LED rapid flashing and solid logic
  if (greenLEDOn && !greenLEDSolid) {
    // Set the start time each time Green LED is armed
    if (greenLEDBlinkTimer == 0 && greenLEDSolidTimer == 0) {
      greenLEDBlinkTimer = millis();
      greenLEDSolidTimer = millis();
    }

    // Rapid blinking for 1 second, then turn solid
    if (millis() - greenLEDSolidTimer >= 1000) {
      greenLEDSolid = true; // After 1 second, make the LED solid
      digitalWrite(greenLEDPin, HIGH);
    } else if (millis() - greenLEDBlinkTimer >= 100) {
      greenLEDBlinkTimer = millis();
      digitalWrite(greenLEDPin, !digitalRead(greenLEDPin)); // Toggle green LED rapidly every 100ms
    }
  }

  // Debounce Red Button (Button 2)
  bool currentRedButtonState = !digitalRead(redButtonPin); // Invert logic for INPUT_PULLUP
  if (currentRedButtonState != redButtonState && millis() - lastRedButtonPressTime > debounceDelay) {
    redButtonState = currentRedButtonState;
    lastRedButtonPressTime = millis();
  }

  // Handle Red Button logic only if Green LED is active and solid
  if (greenLEDSolid) {
    if (redButtonState == HIGH) {
      if (!button2Pressed) {
        button2Pressed = true;
        button2PressedTime = millis();
      }
      unsigned long heldTime = millis() - button2PressedTime;
      if (heldTime >= 2000) {
        // Button 2 held for 2 seconds: Turn red LED on solid and move servo to high position
        digitalWrite(redLEDPin, HIGH);
        redLEDBlinking = false;
        myServo.write(180); // Move servo to high position
      } else {
        // Button 2 held less than 2 seconds: Blink red LED
        static unsigned long redLEDBlinkTimer = 0;
        if (millis() - redLEDBlinkTimer >= 100) {
          redLEDBlinkTimer = millis();
          redLEDBlinking = true;
          digitalWrite(redLEDPin, !digitalRead(redLEDPin)); // Toggle red LED
        }
      }
    } else if (button2Pressed) {
      // Button 2 released
      if (millis() - button2PressedTime < 2000) {
        // Released before 2 seconds: Reset red LED state
        digitalWrite(redLEDPin, LOW);
        redLEDBlinking = false;
      }
      button2Pressed = false;
      button2PressedTime = 0;
    }
  } else {
    // If Green LED is not solid, ensure Red LED is off and reset logic
    digitalWrite(redLEDPin, LOW);
    button2Pressed = false;
    redLEDBlinking = false;
    button2PressedTime = 0;
  }
}

// Reset function to restart the sequence if the green button is held for 2 seconds
void handleGreenButtonReset() {
  static unsigned long greenButtonHoldStartTime = 0;
  static bool isGreenButtonHeld = false;

  bool currentGreenButtonState = !digitalRead(greenButtonPin); // Invert logic for INPUT_PULLUP

  if (currentGreenButtonState) {
    if (!isGreenButtonHeld) {
      isGreenButtonHeld = true;
      greenButtonHoldStartTime = millis(); // Start the timer when button is pressed
    }
    if (millis() - greenButtonHoldStartTime >= 2000) {
      // Button held for 2 seconds: Reset the program
      greenLEDOn = false;
      greenLEDSolid = false;
      redLEDBlinking = false;
      button2Pressed = false;
      button2PressedTime = 0;
      digitalWrite(greenLEDPin, LOW); // Turn off green LED
      digitalWrite(redLEDPin, LOW);  // Turn off red LED
      myServo.write(0);             // Move servo to low position (reset state)
      isGreenButtonHeld = false;    // Reset hold flag

      // Reset blink timers to ensure the green LED blinks again after reset
      greenLEDSolidTimer = 0;
      greenLEDBlinkTimer = 0;
    }
  } else {
    isGreenButtonHeld = false; // Reset hold flag if button is released
  }
}