#include <ESP32Servo.h>
#include <LiquidCrystal_I2C.h>

#define CLOSE_DEGREE 0
#define OPEN_DEGREE 60
#define DISTANCE1_THRESHOLD 20
#define DISTANCE2_THRESHOLD 20

// Pinout
const int TRIG1_PIN = 2;
const int ECHO1_PIN = 4;
const int TRIG2_PIN = 12;
const int ECHO2_PIN = 14;
const int SERVO_PIN = 18;

// Global Variables
long duration1, distance1;
long duration2, distance2;
int pos = 0;
bool distance_status = false;
bool trash_status = false;
String text1 = "Membuka";
String text2 = "Menutup";
String text3 = "Sampah penuh!";
String text4 = "Sampah kosong";
String line1, line2;

// Class Declaration
Servo servo;
LiquidCrystal_I2C lcd(0x27, 16, 2);

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

  // Ultrasonic Setup
  pinMode(TRIG1_PIN, OUTPUT);
  pinMode(ECHO1_PIN, INPUT);
  pinMode(TRIG2_PIN, OUTPUT);
  pinMode(ECHO2_PIN, INPUT);

  // Servo Setup
  servo.attach(SERVO_PIN, 500, 2400);

  // LCD Setup
  lcd.init();
  lcd.backlight();
  lcd.clear();
  printLCD("Smart Trash Bin","");
  delay(3000);
}

void loop() {
  // Kontrol Tutup Tempat Sampah
  distance1 = getDistance1();
  Serial.println("Distance1 = "+ String(distance1));

  if ((distance1 < DISTANCE1_THRESHOLD) && (distance_status == false)) {
    Serial.println("Tempat sampah membuka");
    distance_status = true;
    line1 = text1;
    printLCD(line1,line2);
    servoOpen();
  }
  else if ((distance1 >= DISTANCE1_THRESHOLD) && (distance_status == true)) {
    Serial.println("Tempat sampah menutup");
    distance_status = false;
    line1 = text2;
    printLCD(line1,line2);
    servoClose();
  }

  // Monitor Isi Tempat Sampah
  distance2 = getDistance2();
  Serial.println("Distance2 = "+ String(distance2));

  if ((distance2 < DISTANCE2_THRESHOLD) && (trash_status == false)) {
    Serial.println("Sampah penuh!");
    trash_status = true;
    line2 = text3;
    printLCD(line1,line2);
  }
  else if ((distance2 >= DISTANCE2_THRESHOLD) && (trash_status == true)) {
    Serial.println("Sampah belum penuh");
    trash_status = false;
    line2 = text4;
    printLCD(line1,line2);
  }

  delay(500);
}

long getDistance1() {
  digitalWrite(TRIG1_PIN,LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG1_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG1_PIN, LOW);
  duration1 = pulseIn(ECHO1_PIN, HIGH);
  return (duration1 * 0.034 / 2) + 1;
}

long getDistance2() {
  digitalWrite(TRIG2_PIN,LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG2_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG2_PIN, LOW);
  duration2 = pulseIn(ECHO2_PIN, HIGH);
  return (duration2 * 0.034 / 2) + 1;
}

void servoOpen() {
  for (pos = CLOSE_DEGREE; pos <= OPEN_DEGREE; pos += 1) {
    servo.write(pos);
    delay(10);
  }
}

void servoClose() {
  for (pos = OPEN_DEGREE; pos >= CLOSE_DEGREE; pos -= 1) {
    servo.write(pos);
    delay(10);
  }
}

void printLCD(String line1, String line2) {
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(line1);
  lcd.setCursor(0,1);
  lcd.print(line2);
}