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

// Define pins
const int servoPin = 26;       
const int startButtonPin = 18; 
const int emergencyButtonPin = 19;

Servo myServo;
LiquidCrystal_I2C lcd(0x27, 16, 2); // Adjust the address as needed

// Variables
bool moving = false;
int currentAngle = 70;

void setup() {
    myServo.attach(servoPin);
    pinMode(startButtonPin, INPUT_PULLUP);
    pinMode(emergencyButtonPin, INPUT_PULLUP);
    
    lcd.init();
    lcd.backlight();
    myServo.write(currentAngle); // Move to initial position
    lcd.print("Angle: ");
    lcd.print(currentAngle);
}

void loop() {
    // Check for the start button press
    if (digitalRead(startButtonPin) == LOW && !moving) {
        moving = true;
        moveServo(); // Start moving the servo
        moving = false;
    }
    checkEmergencyStop();
}

void moveServo() {
    // Move from 70 to 130 degrees to and fro
    for (currentAngle = 70; currentAngle <= 130; currentAngle++) {
        checkEmergencyStop();
        myServo.write(currentAngle);
        updateLCD();
        delay(15);
    }

    for (currentAngle = 130; currentAngle >= 70; currentAngle--) {
        checkEmergencyStop();
        myServo.write(currentAngle);
        updateLCD();
        delay(15); 
    }
}

void checkEmergencyStop() {
    // Check for the emergency button press
    if (digitalRead(emergencyButtonPin) == LOW) {
        myServo.write(90); // Stop the servo at 90 degrees
        lcd.clear();
        lcd.print("Emergency Stop");
        while (digitalRead(emergencyButtonPin) == LOW); // Wait for button release
        lcd.clear();
        lcd.print("Angle: ");
        lcd.print(currentAngle);
    }
}

void updateLCD() {
    lcd.setCursor(0, 1);
    lcd.print("Angle: ");
    lcd.print(currentAngle);
}