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

// Set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Define pins for HC-SR04
const int trigPin = 19;
const int echoPin = 18;

// Define pins for Servos
const int servo1Pin = 26;
const int servo2Pin = 25;

// Create Servo objects
Servo servo1;
Servo servo2;

void setup() {
  // Initialize the I2C communication
  Wire.begin(21, 22);

  // Initialize the LCD
  lcd.init();
  
  // Turn on the backlight
  lcd.backlight();

  // Print a message to the LCD
  lcd.setCursor(0, 0); // Set the cursor to column 0, line 0
  lcd.print("Smart Dustbin");

  // Initialize serial communication for debugging
  Serial.begin(115200);

  // Initialize HC-SR04 pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Attach the servo on the respective pins
  servo1.attach(servo1Pin);
  servo2.attach(servo2Pin);

  // Initialize servos to their starting positions
  servo1.write(0); // Closed position
  servo2.write(180); // Open position
}

void loop() {
  // Variable to store the duration and distance
  long duration;
  int distance;

  // Clear the trigPin by setting it LOW
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  // Set the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Read the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);

  // Calculate the distance
  distance = duration * 0.034 / 2;

  // Print the distance on the LCD
  lcd.setCursor(0, 1); // Set the cursor to column 0, line 1
  lcd.print("Distance: ");
  lcd.print(distance);
  lcd.print(" cm   "); // Extra spaces to clear previous characters

  // Print the distance on the Serial Monitor
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  // Control servos based on the distance
  if (distance < 50) {
    servo1.write(90); // Open servo1
    servo2.write(0);  // Close servo2
  } else {
    servo1.write(0);  // Close servo1
    servo2.write(180); // Open servo2
  }

  // Wait for 500 milliseconds before next loop
  delay(500);
}