#include <Servo.h> // include servo library

//instanstiate trig and echo pin for sensor 1
int trigPin = 10;
int echoPin = 13;

//instanstiate trig2 and echo2 pin for sensor 2
int trigPin2 = 11;
int echoPin2 = 12;

//instanstiate LED pins
int LED_R = 4;
int LED_G = 5;
int LED_B = 6;

//instanstiate ESP pin
int ESPin = 2;

//instanstiate servo 1 and servo 2
Servo servo1;
Servo servo2;


float duration;
float distance;

void setup() {
  //instanstiate Serial monitor
  Serial.begin(9600);

  //set trig pins to OUTPUT
  //set echo pinds to INPUT
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin, INPUT);

  //set ESP pin to OUTPUT and initialize at LOW
  pinMode(ESPin, OUTPUT);
  digitalWrite(ESPin, LOW);

  //set LED pins to OUTPUT and initialize blue to HIGH
  pinMode(LED_R, OUTPUT);
  pinMode(LED_G, OUTPUT);
  pinMode(LED_B, OUTPUT);
  digitalWrite(LED_B, HIGH);

  //attach servo1 and servo2 and initialize at 0
  servo1.attach(9);
  servo2.attach(8);
  servo1.write(0);
  servo2.write(0);
  }

void loop() {
  //make outside sensor send out a pulse constantly
  digitalWrite(trigPin,LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034/2;
  
  if (distance >= 400 || distance <= 2){
    Serial.println("Out of range");
  }
  
  else{  
    Serial.println("Distance: ");
    Serial.println(distance);
    
      if (distance < 15 and distance >2){
        Serial.println("Opening Trash Can");

        //turn servos (opening the trashcan)
        servo1.write(180);
        servo2.write(135);

        //delay to throw away item
        delay(5000);

        //turn servos back (closing the trashcan
        servo1.write(0);
        servo2.write(0);

        //delay to wait for the lid to close
        delay(2000);
        
        //make inner sensor send out a pulse while lid is closed
        digitalWrite(trigPin2,LOW);
        delayMicroseconds(2);
        digitalWrite(trigPin2, HIGH);
        delayMicroseconds(10);
        digitalWrite(trigPin2, LOW);

        duration = pulseIn(echoPin2, HIGH);
        distance = duration * 0.034/2;
        
          //conditional for if trash level is high --> make LED red and send text
          if (distance < 6 and distance > 2){
            Serial.println("Time to take out the trash");
            digitalWrite(LED_B, LOW);
            digitalWrite(LED_G, LOW);
            digitalWrite(LED_R, HIGH);
            digitalWrite(ESPin, HIGH);
            delay(2500);
            digitalWrite(ESPin, LOW);
            }

          //conditional for if trash level is mid --> make LED green
          else if (distance <17  and distance > 6) {
            digitalWrite(LED_B, LOW);
            digitalWrite(LED_R, LOW);
            digitalWrite(LED_G, HIGH);
          }

          //conditional for if trash level is low --> make LED blue
          else if (distance > 17) {
            digitalWrite(LED_B, HIGH);
            digitalWrite(LED_R, LOW);
            digitalWrite(LED_G, LOW);
          }
            
        }
  }  
         
  delay(500);
  
}