#include <ESP32Servo.h>

#define TRIG_PIN_LID 22
#define ECHO_PIN_LID 23
#define TRIG_PIN_STORAGE 14
#define ECHO_PIN_STORAGE 25
#define MOTOR_PIN 26
#define FULL_LED_PIN 27
#define EMPTY_LED_PIN 13 

#define MAX_DISTANCE 100 // Maximum distance to consider for ultrasonic sensor (in centimeters)
#define FULL_THRESHOLD 5 // Threshold distance to consider the bin full (in centimeters)
#define OPEN_THRESHOLD 50 // Threshold distance to trigger opening the lid (in centimeters)

Servo servo;
bool isBinFull = false;

void setup() {
  Serial.begin(9600);
  pinMode(TRIG_PIN_LID, OUTPUT);
  pinMode(ECHO_PIN_LID, INPUT);
  pinMode(TRIG_PIN_STORAGE, OUTPUT);
  pinMode(ECHO_PIN_STORAGE, INPUT);
  pinMode(MOTOR_PIN, OUTPUT);
  pinMode(FULL_LED_PIN, OUTPUT);
  pinMode(EMPTY_LED_PIN, OUTPUT);
  
  servo.attach(MOTOR_PIN);
  servo.write(90); // Close the lid initially
}

void loop() {
  // Read ultrasonic sensor for lid
  float distanceLid = readDistance(TRIG_PIN_LID, ECHO_PIN_LID);
  
  // Read ultrasonic sensor for storage
  float distanceStorage = readDistance(TRIG_PIN_STORAGE, ECHO_PIN_STORAGE);
  
  // Check if the bin is full
  if (distanceStorage < FULL_THRESHOLD) {
    isBinFull = true;
    digitalWrite(FULL_LED_PIN, HIGH); // Turn on full LED
    digitalWrite(EMPTY_LED_PIN, LOW); // Turn off empty LED
    Serial.println("Bin Full");
  } else {
    isBinFull = false;
    digitalWrite(FULL_LED_PIN, LOW); // Turn off full LED
    digitalWrite(EMPTY_LED_PIN, HIGH); // Turn on empty LED
    Serial.println("Bin Empty");
  }
  
  // Check if the lid should be opened
  if (distanceLid < OPEN_THRESHOLD) {
    openLid();
    Serial.println("Lid Opened");
  } else {
    closeLid();
    Serial.println("Lid Closed");
  }
  
  delay(100); // Delay between readings
}

float readDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  unsigned long duration = pulseIn(echoPin, HIGH);
  float distance = duration * 0.0343 / 2; // Convert duration to distance in centimeters
  
  // Limit the maximum distance to prevent inaccuracies
  if (distance > MAX_DISTANCE) {
    distance = MAX_DISTANCE;
  }
  
  return distance;
}

void openLid() {
  servo.write(180); // Open the lid
}

void closeLid() {
  servo.write(90); // Close the lid
}