#include <Servo.h>
#include <NewPing.h>

#define PIN_TRIGGER_ULTRASONIK_1 3
#define PIN_ECHO_ULTRASONIK_1 4
#define MAX_DISTANCE_ULTRASONIK_1 200
#define PIN_TRIGGER_ULTRASONIK_2 5
#define PIN_ECHO_ULTRASONIK_2 6
#define MAX_DISTANCE_ULTRASONIK_2 30
#define BATTERY_PIN A0
#define REFERENCE_VOLTAGE 5.0

NewPing deteksiObjek(PIN_TRIGGER_ULTRASONIK_1, PIN_ECHO_ULTRASONIK_1, MAX_DISTANCE_ULTRASONIK_1);
NewPing deteksiSampah(PIN_TRIGGER_ULTRASONIK_2, PIN_ECHO_ULTRASONIK_2, MAX_DISTANCE_ULTRASONIK_2);
Servo myservo;
bool servoTerbuka = false;
bool peringatanPenuh = false;
enum Status { Kosong, TidakPenuh, Penuh, Terbuka };
Status status = Kosong;

void setup() {
  Serial.begin(115200);
  myservo.attach(9);
  pinMode(LED_BUILTIN, OUTPUT);
}

void loop() {
  delay(500);
  myservo.write(0);
  Serial.print("ROBOBIN TONG SAMPAH CERDAS OTOMATIS\n");

  Serial.print("- Jarak Objek\t: ");
  int jarakObjek = deteksiObjek.ping_cm();
  Serial.print(jarakObjek);
  Serial.println("cm");

  Serial.print("- Jarak Sampah\t: ");
  int jarakSampah = deteksiSampah.ping_cm();
  Serial.print(jarakSampah);
  Serial.println("cm");

  int batteryRawValue = analogRead(BATTERY_PIN);
  float batteryVoltage = (batteryRawValue / 1023.0) * REFERENCE_VOLTAGE;
  int batteryPercentage = (batteryVoltage / 5.0) * 100;

  int kapasitas;
  if (jarakSampah == 0) {
    kapasitas = 0;
    status = Kosong;
    Serial.print("- Kapasitas\t: ");
    Serial.print(kapasitas);
    Serial.println("% Terpakai");

    Serial.print("- Daya Baterai\t: ");
    Serial.print(batteryPercentage);
    Serial.println("% Tersisa");
    
    Serial.print("- Status\t: ");
    Serial.println("ROBOBIN Kosong");
  } else {
    kapasitas = 100 - ((jarakSampah * 100) / MAX_DISTANCE_ULTRASONIK_2);
    Serial.print("- Kapasitas\t: ");
    Serial.print(kapasitas);
    Serial.println("% Terpakai");

    Serial.print("- Daya Baterai\t: ");
    Serial.print(batteryPercentage);
    Serial.println("% Tersisa");

    if (jarakSampah > 0 && jarakSampah <= 10) {
      if (!servoTerbuka) {
        status = Penuh;
        Serial.print("- Status\t: ");
        Serial.println("ROBOBIN Sudah Penuh");
        digitalWrite(LED_BUILTIN, HIGH);
        delay(250);
        digitalWrite(LED_BUILTIN, LOW);
        peringatanPenuh = true;
      }
    } else {
      status = TidakPenuh;
      Serial.print("- Status\t: ");
      Serial.println("ROBOBIN Belum Penuh");
    }
  }

  if (jarakObjek > 0 && jarakObjek < 50) {
    status = Terbuka;
    Serial.print("- Status\t: ");
    Serial.println("ROBOBIN Terbuka");
    myservo.write(130);
    servoTerbuka = true;
    peringatanPenuh = false;
    while (jarakObjek < 50) {
      jarakObjek = deteksiObjek.ping_cm();
    }
    delay(2000);
    myservo.write(0);
    servoTerbuka = false;
  }

  Serial.println("");
}