#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

#define BUTTON1_PIN 2
#define BUTTON2_PIN 3
#define SERVO1_PIN 9
#define SERVO2_PIN 10

LiquidCrystal_I2C lcd(0x27, 16, 2); // Change address if needed
Servo servo1;
Servo servo2;

unsigned long lastDebounceTime1 = 0;
unsigned long lastDebounceTime2 = 0;
const unsigned long debounceDelay = 50; // Debounce time

void setup() {
    pinMode(BUTTON1_PIN, INPUT_PULLUP);
    pinMode(BUTTON2_PIN, INPUT_PULLUP);
    
    servo1.attach(SERVO1_PIN);
    servo2.attach(SERVO2_PIN);
    
    // Initialize servos to 0 degrees
    servo1.write(0);
    servo2.write(0);
    
    lcd.begin(16, 2); // Set the number of columns and rows
    lcd.backlight();
    lcd.print("PRESS"); // Initial message
}

void loop() {
    int button1State = digitalRead(BUTTON1_PIN);
    int button2State = digitalRead(BUTTON2_PIN);
    
    // Check button 1
    if (button1State == LOW && (millis() - lastDebounceTime1) > debounceDelay) {
        lastDebounceTime1 = millis();
        lcd.clear();
        lcd.print("Button 1 Pressed");
        moveServo(servo1);
    } 
    // Check button 2
    else if (button2State == LOW && (millis() - lastDebounceTime2) > debounceDelay) {
        lastDebounceTime2 = millis();
        lcd.clear();
        lcd.print("Button 2 Pressed");
        moveServo(servo2);
    } 
    // Default display
    else {
        lcd.clear();
        lcd.print("PRESS"); // Display when no button is pressed
    }
}

void moveServo(Servo &servo) {
    servo.write(90); // Move to 90 degrees
    delay(500);      // Wait for 500 milliseconds
    servo.write(0);  // Move back to 0 degrees
}