#include <Servo.h>
#include <LiquidCrystal_I2C.h> // Library for LCD
LiquidCrystal_I2C lcd(0x27, 16, 2); // I2C address 0x27, 16 column and 2 rows
Servo servo1; // servo objects to control the servo's
Servo servo2;
Servo servo3;

int LED_PIN = 4;
int LED_PIN2 = 12;
int trigPin1 = 2;
int echoPin1 =  3;
int trigPin2 = 5;
int echoPin2 = 6;
int SERVO1_PIN = 13;
int SERVO2_PIN = 7;
int SERVO3_PIN;
int pos = 0;    // variable to store the servo position

const int PERSON_DISTANCE_THRESHOLD = 40; // centimeters
const int TRUSH_DISTANCE_THRESHOLD1 = 50; 
const int TRUSH_DISTANCE_THRESHOLD2 = 80;

float duration_us1, distance_cm1, duration_us2, distance_cm2;

void setup() {

  lcd.init(); // initialize the lcd
  lcd.backlight();
  Serial.begin(9600);

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(LED_PIN2, OUTPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  servo1.attach(SERVO1_PIN);   // attaching the servo on pin 13 to the servo object
  servo1.write(0);
  servo2.attach(SERVO2_PIN);
  servo2.write(0);

  lcd.clear();                 // clearing the display
  lcd.setCursor(0, 0);         // move cursor to   (0, 0)
  lcd.print("Arduino");        // print message at (0, 0)
  lcd.setCursor(0, 1);         // move cursor to   (0, 1)
  lcd.print("Smart Bin System"); // print message at (0, 1)
  delay(1500);                 // display the above for 1.5 seconds

}

void loop() {
  // put your main code here, to run repeatedly:

  //Sensing distance from person
  // generating 10-microsecond pulse to TRIG pin2
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);

  duration_us2 = pulseIn(echoPin2, HIGH); // measuring duration of pulse from ECHO pin2
  distance_cm2 = 0.017 * duration_us2; // calculating the distance in cetimeters

  // printing the value to Serial Monitor
  Serial.print("distance from Person: ");
  Serial.print(distance_cm2);
  Serial.println(" cm");
  delay(200);

  if (distance_cm2 < PERSON_DISTANCE_THRESHOLD){
    lcd.clear();                  
    lcd.setCursor(4, 0);          
    lcd.print("HELLO!");        
    lcd.setCursor(0, 1);          
    lcd.print("BIN OPENING"); 
    delay(1000); 

   for (pos = 0; pos <= 90; pos += 1) { // rotating from 0 degrees to 90 degrees in steps of 1 degree
      servo2.write(pos);                   // telling servo to go to position in variable 'pos'
      delay(100);                          // waits 10ms for the servo to reach the position
    }

    delay(2000); //Waits 2 seconds before bin closes

    for (pos = 90; pos >= 0; pos -= 1) { // rotate from 180 degrees to 0 degrees
      servo2.write(pos);                   // tell servo to go to position in variable 'pos'
      delay(150);                          // waits 10ms for the servo to reach the position
    } 
  }

  // Sensing distance from trash
  // generating 10-microsecond pulse to TRIG pin1
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);

  // measure duration of pulse from ECHO pin1
  duration_us1 = pulseIn(echoPin1, HIGH);
  distance_cm1 = 0.017 * duration_us1;


  // printing the value to Serial Monitor
  Serial.print("distance from Trash: ");
  Serial.print(distance_cm1);
  Serial.println(" cm");
  delay(20);

    
  if((distance_cm1 >= TRUSH_DISTANCE_THRESHOLD1) && (distance_cm1 <= TRUSH_DISTANCE_THRESHOLD2)){
    digitalWrite(LED_PIN2, LOW);

    lcd.clear();                  // clear display
    lcd.setCursor(3, 0);          // move cursor to   (3, 0)
    lcd.print(" BIN IS FULL");        // print message at (3, 0)
    lcd.setCursor(0, 1);          // move cursor to   (0, 1)
    lcd.print("   COMPACTING   "); // print message at (0, 1)
    delay(1000);                  // display the above for one second

    for (pos = 0; pos <= 180; pos += 1) { // rotate from 0 degrees to 180 degrees
      // in steps of 1 degree
      digitalWrite(LED_PIN, HIGH);
      delay(25);
      servo1.write(pos); 
      digitalWrite(LED_PIN, LOW);
      delay(25);                  // tell servo to go to position in variable 'pos'
      delay(25);                         // waits 10ms for the servo to reach the position
    }

    delay (1000);

    for (pos = 180; pos >= 0; pos -= 1) { // rotate from 180 degrees to 0 degrees
      digitalWrite(LED_PIN, HIGH);
      delay(25);
      servo1.write(pos);
      digitalWrite(LED_PIN, LOW);
      delay(25);                   // tell servo to go to position in variable 'pos'
      delay(25);                          // waits 10ms for the servo to reach the position
    }
  }

  if(distance_cm1 < TRUSH_DISTANCE_THRESHOLD1){
    //Bin is full
    digitalWrite(LED_PIN2, LOW);
    digitalWrite(LED_PIN, HIGH); // turn on LED
    lcd.clear();                  // clear display
    lcd.setCursor(0, 0);          // move cursor to   (0, 0)
    lcd.print("  BIN IS FULL!  ");        // print message at (0, 0)
    lcd.setCursor(0, 1);          // move cursor to   (0, 1)
    lcd.print("UNFILL THE BIN"); // print message at (0, 1)
    delay(1000);                  // display the above for one second
    
  }
  else{
    digitalWrite(LED_PIN, LOW);
    digitalWrite(LED_PIN2, HIGH);
    lcd.clear();                  // clear display
    lcd.setCursor(0, 0);          // move cursor to   (3, 0)
    lcd.print("READY TO USE ");        // print message at (3, 0)
    lcd.setCursor(0, 1);          // move cursor to   (0, 1)
    lcd.print("  THANK YOU"); // print message at (0, 1)
    delay(2000);                  // display the above for two seconds
  }
    
}