#include <Wire.h> // Include the Wire library for I2C communication
#include <LiquidCrystal_I2C.h> // Include the I2C LCD library
#include <ESP32Servo.h>

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;

const int trigPin = 16; // Ultrasonic sensor trigger pin (Connect to GPIO 16)
const int echoPin = 17; // Ultrasonic sensor echo pin (Connect to GPIO 17)

LiquidCrystal_I2C lcd(0x27, 16, 2); // Set the LCD I2C address to 0x27 (change if different)
bool stopLoop = false; // Flag variable to control the loop

void setup()
{
  servo1.attach(13);  //Attach servo 1 to GPIO 13 of ESP32
  servo2.attach(14);  //Attach servo 2 to GPIO 14 of ESP32
  servo3.attach(26);  //Attach servo 3 to GPIO 26 of ESP32
  servo4.attach(12);  //Attach servo 4 to GPIO 12 of ESP32
  servo5.attach(2);   //Attach servo 5 to GPIO 2 of ESP32

  pinMode(trigPin, OUTPUT); // Set ultrasonic sensor trigger pin as output
  pinMode(echoPin, INPUT);  // Set ultrasonic sensor echo pin as input

  lcd.init();
  lcd.backlight();

  lcd.setCursor(16,2); //  Initialize the LCD with 16 columns and 2 rows
  lcd.clear();      // Clear the LCD screen
  lcd.print("STARTING..."); // Display "Distance:" on the first line of the LCD
  delay(5000);
  lcd.clear();      // Clear the LCD screen
  // Initialize the serial communication for debugging
  Serial.begin(115200);
}

void loop(){
  if (!stopLoop) // Check if the flag is not set to true
  
  { 
    // FIRST TUPI EXECUTING
    lcd.clear();
    lcd.setCursor(3, 0);
    lcd.print("FIRST TUPI");
    lcd.setCursor(3, 1);
    lcd.print("EXECUTING!"); 

    // Default Position
      servo1.write(75);
      delay(1000);  // Wait for 1 second
      servo2.write(0);
      delay(1000);  // Wait for 1 second
      servo3.write(0);
      delay(1000);  // Wait for 1 second
      servo4.write(0);
      delay(1000);  // Wait for 1 second
      servo5.write(0);
      delay(1000);  // Wait for 1 second
     
      //Gripping of Hook 1
      servo1.write(43);
      delay(2000);  // Wait for 2 seconds
      servo2.write(40);
      delay(2000);  // Wait for 2 seconds
      servo2.write(50);
      delay(2000);  // Wait for 2 seconds
      servo2.write(60);
      delay(2000);  // Wait for 2 seconds
      servo4.write(45);
      delay(2000);  // Wait for 2 seconds
      servo3.write(40);
      delay(2000);  // Wait for 2 seconds
      
      //Moving Left to Fold
      servo1.write(60);
      delay(1000);  // Wait for 1 second
      servo1.write(70);
      delay(1000);  // Wait for 1 second
      servo1.write(90);
      delay(1000);  // Wait for 1 second
      
      //Going back to the original position
      servo1.write(70);
      delay(1000);  // Wait for 1 second
      servo1.write(60);
      delay(1000);  // Wait for 1 second
      servo1.write(43);
      delay(2000);  // Wait for 2 seconds
      servo4.write(45);
      delay(1000);  // Wait for 1 second
     
      //Releasing of Gripper
      servo3.write(0);
      delay(1000);  // Wait for 1 second
      servo2.write(50);
      delay(1000);  // Wait for 1 second
      servo2.write(40);
      delay(1000);  // Wait for 1 second

      // FIRST TUPI
     lcd.clear();
     lcd.setCursor(3, 0);
     lcd.print("FIRST TUPI");
     lcd.setCursor(6, 1);
     lcd.print("DONE");
     delay(3000);
     lcd.clear();
      
      // SECOND TUPI EXECUTING
     lcd.clear();
     lcd.setCursor(3, 0);
     lcd.print("SECOND TUPI");
     lcd.setCursor(3, 1);
     lcd.print("EXECUTING!");

      // Default Position
      servo1.write(75);
      delay(1000);  // Wait for 1 second
      servo2.write(0);
      delay(1000);  // Wait for 1 second
      servo3.write(0);
      delay(1000);  // Wait for 1 second
      servo4.write(0);
      delay(1000);  // Wait for 1 second
      servo5.write(0);
      delay(1000);  // Wait for 1 second

      //Gripping of Hook 2
      servo1.write(112);
      delay(2000);  // Wait for 2 seconds
      servo2.write(50);
      delay(2000);  // Wait for 2 seconds
      servo2.write(60);
      delay(2000);  // Wait for 2 seconds
      servo2.write(65);
      delay(2000);  // Wait for 2 seconds
      servo4.write(45);
      delay(2000);  // Wait for 2 seconds
      servo3.write(40);
      delay(2000);  // Wait for 2 seconds
    
     //Moving Right to Fold
      servo1.write(100);
      delay(1000);  // Wait for 1 second
      servo1.write(80);
      delay(1000);  // Wait for 1 second
      servo1.write(60);
      delay(1000);  // Wait for 1 second
      
      //Going back to the original position
      servo1.write(80);
      delay(1000);  // Wait for 1 second
      servo1.write(100);
      delay(1000);  // Wait for 1 second
      servo1.write(112);
      delay(2000);  // Wait for 2 seconds
      servo4.write(45);
      delay(1000);  // Wait for 1 second
      
      //Releasing of Gripper
      servo3.write(0);
      delay(1000);  // Wait for 1 second
      servo2.write(50);
      delay(1000);  // Wait for 1 second
      servo2.write(40);
      delay(1000);  // Wait for 1 second

      // SECOND TUPI
      lcd.clear();
      lcd.setCursor(3, 0);
      lcd.print("SECOND TUPI");
      lcd.setCursor(7, 1);
      lcd.print("DONE");
      delay(3000);
      lcd.clear();
    
     // THIRD TUPI EXECUTING
     lcd.clear();
     lcd.setCursor(3, 0);
     lcd.print("THIRD TUPI");
     lcd.setCursor(3, 1);
     lcd.print("EXECUTING!");

      // Default Position
      servo1.write(75);
      delay(1000);  // Wait for 1 second
      servo2.write(0);
      delay(1000);  // Wait for 1 second
      servo3.write(0);
      delay(1000);  // Wait for 1 second
      servo4.write(0);
      delay(1000);  // Wait for 1 second

      //Rotating of servo 5
      servo5.write(0);
      delay(1000);  // Wait for 1 second
      servo5.write(100);
      delay(1000);  // Wait for 1 second
    
      //Gripping of Hook 2
      servo1.write(97);
      delay(2000);  // Wait for 2 seconds
      servo2.write(50);
      delay(2000);  // Wait for 2 seconds
      servo2.write(60);
      delay(2000);  // Wait for 2 seconds
      servo2.write(70);
      delay(2000);  // Wait for 2 seconds
      servo4.write(45);
      delay(2000);  // Wait for 2 seconds
      servo3.write(40);
      delay(2000);  // Wait for 2 seconds

     //Moving Right to Fold
      servo1.write(70);
      delay(1000);  // Wait for 1 second
      servo1.write(50);
      delay(1000);  // Wait for 1 second
      servo1.write(40);
      delay(1000);  // Wait for 1 second

      //Going back to the original position
      servo1.write(50);
      delay(1000);  // Wait for 1 second
      servo1.write(70);
      delay(1000);  // Wait for 1 second
      servo1.write(97);
      delay(2000);  // Wait for 2 seconds
      servo4.write(45);
      delay(1000);  // Wait for 1 second
      
      //Releasing of Gripper
      servo3.write(0);
      delay(1000);  // Wait for 1 second
      servo2.write(50);
      delay(1000);  // Wait for 1 second
      servo2.write(40);
      delay(1000);  // Wait for 1 second

      // THIRD TUPI
      lcd.clear();
      lcd.setCursor(3, 0);
      lcd.print("THIRD TUPI");
      lcd.setCursor(7, 1);
      lcd.print("DONE");
      delay(3000);
      lcd.clear();
  
      // Default Position
      servo1.write(75);
      delay(1000);  // Wait for 1 second
      servo2.write(0);
      delay(1000);  // Wait for 1 second
      servo3.write(0);
      delay(1000);  // Wait for 1 second
      servo4.write(0);
      delay(1000);  // Wait for 1 second
      servo5.write(0);
      delay(1000);  // Wait for 1 second

      // Default
      lcd.clear();

      //Ultrasonic Sensor
      servo2.write(60);
      delay(1000);  // Wait for 1 second
      servo2.write(70);
      delay(1000);  // Wait for 1 second
      servo4.write(150);
      delay(3000);  // Wait for 1 second
      servo4.write(0);
      delay(2000);  // Wait for 1 second
      servo2.write(30);
      delay(2000);  // Wait for 1 second
      servo2.write(0);
      delay(2000);  // Wait for 1 second
      servo1.write(50);
      delay(2000);  // Wait for 1 second
      servo1.write(30);
      delay(2000);  // Wait for 1 second
      servo1.write(10);
      delay(2000);  // Wait for 1 second
      servo1.write(0);
      delay(2000);  // Wait for 1 second

   // Measure distance using ultrasonic sensor
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    long duration = pulseIn(echoPin, HIGH);
    float distance = duration * 0.034 / 2; // Calculate distance in cm

    // Display the distance on the LCD
    lcd.setCursor(0, 1);
    lcd.print("          "); // Clear the previous distance value
    lcd.setCursor(0, 1);
    lcd.print(distance);
    lcd.print(" cm");
    lcd.setCursor(0, 0);
    lcd.print("Distance:");
  
    { // Check if the distance is less than or equal to 450 cm
    if (distance <= 450) {
      // Open the gripper at 90 degrees
      servo4.write(90);
    } else {
      // Close the gripper
      servo4.write(0);
  }
    }
      
      servo2.write(60);
      delay(2000);  // Wait for 2 seconds
      servo2.write(70);
      delay(2000);  // Wait for 2 seconds

       stopLoop = true;
    
     // 
     lcd.clear();
     lcd.setCursor(1, 0);
     lcd.print("CLOTH IS FOLDED!");
     lcd.setCursor(6, 1);
     lcd.print("NEXT!");
     
  }
}