#include <Wire.h>
#include <SoftwareSerial.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h> // Library for LCD

LiquidCrystal_I2C lcd(0x27, 20, 4); // I2C address 0x27, 16 column and 2 rows
Servo servo1; // servo objects to control the servo's
Servo servo2;
// Initialize the GSM module's software serial port
SoftwareSerial gsmSerial(10, 11); //rx tx

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 = 8; 
const int TRUSH_DISTANCE_THRESHOLD2 = 19;

float duration_us1, distance_cm1, duration_us2, distance_cm2;

bool full = false;

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);

  // Set up the software serial communication at 9600 baud
  gsmSerial.begin(9600);
  // Wait for the GSM module to become ready
  delay(1000);

  lcd.clear();                 // clearing the display
  lcd.setCursor(1, 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("INTELLIGENT WASTE"); // print message at (0, 1)
  lcd.setCursor(0, 2);
  lcd.print("MANAGEMENT SYSTEM");
  lcd.setCursor(0, 3);
  lcd.print("KEN & OLIVIA");
  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");

  if (distance_cm2 < PERSON_DISTANCE_THRESHOLD){
    if (!full){
      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(10);                          // waits 10ms for the servo to reach the position
      }

      delay(4000); //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(10);                          // waits 10ms for the servo to reach the position
      }
    }

    else {
      servo1 .write(0);
      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
      
    }

  }

  // 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
    full = true;
    servo1 .write(0);
    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
    
    // Send an SMS message
    sendSMS("+265996352102", "Hello, Bin 1 Is Full! go unfill it");
    // Wait for 10 seconds before sending another message
    delay(10000);
    
  }
  else{
    full = false;
    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
  }
    
}

// Function to send an SMS message
void sendSMS(String phoneNumber, String message) {
  // Set the GSM module to text mode
  gsmSerial.println("AT+CMGF=1\r");
  delay(100);
  // Set the phone number of the recipient
  gsmSerial.print("AT+CMGS=\"+265996352102\"\r");
  gsmSerial.print(phoneNumber);
  gsmSerial.println("\"");
  delay(100);
  // Set the message text
  gsmSerial.print("Hello Bin1 is full! Go and empty the bin");
  delay(100);
  // Send the message
  gsmSerial.write(26);
  delay(100);
}