#include <ESP32Servo.h>

// Pin Definitions
#define ULTRASONIC_TRIG 5      // person detection
#define ULTRASONIC_ECHO 18
#define BIN_ULTRASONIC_TRIG 23 // inside bin garbage level
#define BIN_ULTRASONIC_ECHO 22
#define SERVO_PIN 19
#define LED_GREEN 26
#define LED_RED 25


Servo lidServo;

// Thresholds
#define PERSON_DISTANCE 30   // cm
#define FULL_BIN_PERCENT 90  // 90% full

// Bin Fill Variables
float binHeight = 20.0;
int binFillPercent;

// Lid state
bool lidIsOpen = false;

void setServo(int angle) {
  lidServo.write(angle); 
  delay(200); // stabilize
}

void setup() {
  Serial.begin(115200);

  lidServo.setPeriodHertz(50);
  lidServo.attach(SERVO_PIN, 1000, 2000);
  setServo(0); // lid closed

  pinMode(ULTRASONIC_TRIG, OUTPUT);
  pinMode(ULTRASONIC_ECHO, INPUT);
  pinMode(BIN_ULTRASONIC_TRIG, OUTPUT);
  pinMode(BIN_ULTRASONIC_ECHO, INPUT);

  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_RED, LOW);

  Serial.println("Smart Bin Initialized");
}

void loop() {
  float distanceToPerson = readUltrasonic(ULTRASONIC_TRIG, ULTRASONIC_ECHO);
  float wasteDistance = readUltrasonic(BIN_ULTRASONIC_TRIG, BIN_ULTRASONIC_ECHO);

  binFillPercent = map(wasteDistance, 0, binHeight, 100, 0);
  binFillPercent = constrain(binFillPercent, 0, 100);

  Serial.print("Person Distance: "); Serial.print(distanceToPerson); Serial.println(" cm");
  Serial.print("Bin Fill: "); Serial.print(binFillPercent); Serial.println("%");

  if (distanceToPerson <= PERSON_DISTANCE) {
    // Person is close
    if (binFillPercent >= FULL_BIN_PERCENT) {
      // Bin full → red LED, lid closed
      digitalWrite(LED_RED, HIGH);
      digitalWrite(LED_GREEN, LOW);
      if (lidIsOpen) {
        setServo(0);
        lidIsOpen = false;
      }
      Serial.println("Bin full → Lid closed, Red LED ON");
    } else {
      // Bin not full → green LED
      digitalWrite(LED_GREEN, HIGH);
      digitalWrite(LED_RED, LOW);

      // Open lid if not already open
      if (!lidIsOpen) {
        setServo(90); // open lid
        lidIsOpen = true;
        Serial.println("Bin not full → Lid opened, Green LED ON");
      }
      // Keep lid open as long as person is nearby
    }
  } else {
    // Person gone → close lid immediately
    if (lidIsOpen) {
      setServo(0);
      lidIsOpen = false;
      Serial.println("Person gone → Lid closed");
    }
    digitalWrite(LED_GREEN, LOW);
    digitalWrite(LED_RED, LOW);
  }

  delay(300);
}

float readUltrasonic(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000);
  float distance = duration * 0.034 / 2;
  if (distance == 0) distance = 100;
  return distance;
}