#include <Servo.h>

// Create Servo object
Servo myServo;

// Define pins
const int buttonPin = 2;   // Button connected to D2
const int servoPin = 9;    // Servo connected to D9
const int redLedPin = 3;   // Red LED connected to D3
const int greenLedPin = 4; // Green LED connected to D4

// Variables
int buttonState = 0;       // To store current button state
int lastButtonState = 0;   // To store previous button state
bool isDoorOpen = false;   // Track door status

void setup() {
  // Configure pins
  pinMode(buttonPin, INPUT);
  pinMode(redLedPin, OUTPUT);
  pinMode(greenLedPin, OUTPUT);

  // Initialize Servo and LEDs
  myServo.attach(servoPin);
  closeDoor();             // Start with door closed
  // Serial.begin(9600);      // For debugging
  
  // // Test LEDs (make sure both LEDs are functional)
  // testGreenLed();
  // delay(1000);
  // testRedLed();
  // delay(1000);
}

void loop() {
  // Read the current button state
  buttonState = digitalRead(buttonPin);
  
  // Check for a state change (button press)
  if (buttonState == HIGH && lastButtonState == LOW) {
    delay(50); // Debounce delay

    // Toggle the door state
    if (isDoorOpen) {
      closeDoor(); // Close the door if it's open
    } else {
      openDoor();  // Open the door if it's closed
    }
  }

  // Update the last button state
  lastButtonState = buttonState;

  // For debugging: print the button state to serial monitor
  Serial.print("Button State: ");
  Serial.println(buttonState);
}

void openDoor() {
  myServo.write(90);        // Move servo to 90° (open position)
  digitalWrite(greenLedPin, HIGH); // Turn on green LED
  digitalWrite(redLedPin, LOW);    // Turn off red LED
  isDoorOpen = true;         // Update door state
  Serial.println("Door Opened");
}

void closeDoor() {
  myServo.write(0);         // Move servo to 0° (closed position)
  digitalWrite(redLedPin, HIGH);  // Turn on red LED
  digitalWrite(greenLedPin, LOW); // Turn off green LED
  isDoorOpen = false;        // Update door state
  Serial.println("Door Closed");
}

// Function to test green LED independently
void testGreenLed() {
  Serial.println("Testing Green LED...");
  digitalWrite(greenLedPin, HIGH);  // Turn on green LED
  delay(1000);
  digitalWrite(greenLedPin, LOW);   // Turn off green LED
  Serial.println("Green LED Test Complete");
}

// Function to test red LED independently
void testRedLed() {
  Serial.println("Testing Red LED...");
  digitalWrite(redLedPin, HIGH);    // Turn on red LED
  delay(1000);
  digitalWrite(redLedPin, LOW);     // Turn off red LED
  Serial.println("Red LED Test Complete");
}