//Botol Besar
const int triggerPin_1 = 5;
const int echoPin_1 = 6;
unsigned int duration_1;
unsigned int distance_1;

//Botol Kecil
const int triggerPin_2 = 7;
const int echoPin_2 = 8;
unsigned int duration_2;
unsigned int distance_2;

//Full1
const int triggerPin_3 = 9;
const int echoPin_3 = 10;
unsigned int duration_3;
unsigned int distance_3;

//Full2
const int triggerPin_4 = 11;
const int echoPin_4 = 12;
unsigned int duration_4;
unsigned int distance_4;

// #include <HCSR04.h>
// HCSR04 hcBotolBesar(5,6); //Mendefinisikan pin D5 sebagai Trig dan D6 sebagai Echo
// HCSR04 hcBotolKecil(7,8);
// HCSR04 hcFull1(9,10);
// HCSR04 hcFull2(11,12);
int JarakBotolKecil;
int JarakBotolBesar;
int JarakFull1;
int JarakFull2;

#include <Servo.h>
Servo servo100;
Servo servo200;
Servo servoBotolBesar;
Servo servoBotolKecil;

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27,16,2);

#define irBotolBesar 2
#define irBotolKecil 3
#define PushButton 4
int HasilPushButton;
int HasilBotolBesar;
int HasilBotolKecil;
int i, j;

int JumlahBotolBesar;
int JumlahBotolKecil;

void setup() {
  Serial.begin(115200);
  servo100.attach(A0);
  servo200.attach(A1);
  servoBotolBesar.attach(A2);
  servoBotolKecil.attach(A3);
  pinMode(irBotolBesar,INPUT);
  pinMode(irBotolKecil,INPUT);
  pinMode(PushButton,INPUT_PULLUP);

  pinMode(triggerPin_1, OUTPUT);
  pinMode(echoPin_1, INPUT);
  pinMode(triggerPin_2, OUTPUT);
  pinMode(echoPin_2, INPUT);
  pinMode(triggerPin_3, OUTPUT);
  pinMode(echoPin_3, INPUT);
  pinMode(triggerPin_4, OUTPUT);
  pinMode(echoPin_4, INPUT);

  lcd.init();
  lcd.backlight();
  lcd.setCursor(0,0);
  lcd.print("    Masukkan   ");
  lcd.setCursor(0,1);
  lcd.print("     Botol     ");
  
}

void loop() {
  digitalWrite(triggerPin_1, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin_1, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin_1, LOW);
  duration_1 = pulseIn(echoPin_1, HIGH);
  distance_1 = (duration_1 / 2) / 28.5;
  JarakBotolBesar = distance_1;

  digitalWrite(triggerPin_2, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin_2, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin_2, LOW);
  duration_2 = pulseIn(echoPin_2, HIGH);
  distance_2 = (duration_2 / 2) / 28.5;
  JarakBotolKecil = distance_2;

  digitalWrite(triggerPin_3, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin_3, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin_3, LOW);
  duration_3 = pulseIn(echoPin_3, HIGH);
  distance_3 = (duration_3 / 2) / 28.5;
  JarakFull1 = distance_3;

  digitalWrite(triggerPin_4, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin_4, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin_4, LOW);
  duration_4 = pulseIn(echoPin_4, HIGH);
  distance_4 = (duration_4 / 2) / 28.5;
  JarakFull2 = distance_4;

  //Kapasitas Penuh
  if (JarakFull1 < 5 && JarakFull2 < 5){
    lcd.setCursor(0,0);
    lcd.print("   Kapasitas   ");
    lcd.setCursor(0,1);
    lcd.print("     Penuh     ");
  }
  
  // Kapasitas Masih Tersedia
  else{
    HasilBotolBesar= digitalRead(irBotolBesar);
    HasilBotolKecil= digitalRead(irBotolKecil);
    //HasilPushButton= digitalRead(PushButton);

    //Code Untuk Botol Besar
    if(HasilBotolBesar==0 && JarakBotolBesar <= 5){
      JumlahBotolBesar++; 
      lcd.clear(); 
      lcd.setCursor(0,0);
      lcd.print(("Botol Besar: ")+String(JumlahBotolBesar));
      lcd.setCursor(0,1);
      lcd.print(("Botol Kecil: ")+String(JumlahBotolKecil));
      servoBotolBesar.write(180);
      delay(1000);
      servoBotolBesar.write(90);
    }

    //Code Untuk Botol Kecil
    if(HasilBotolKecil==0 && JarakBotolKecil <= 5){
      JumlahBotolKecil++;
      lcd.clear(); 
      lcd.setCursor(0,0);
      lcd.print(("Botol Besar: ")+String(JumlahBotolBesar));
      lcd.setCursor(0,1);
      lcd.print(("Botol Kecil: ")+String(JumlahBotolKecil));
      servoBotolKecil.write(180);
      delay(1000);
      servoBotolKecil.write(90);
    }

    //Membaca kondisi input dari pin
    static bool buttonState = false;
    bool state = digitalRead(PushButton);

    //Harga Botol
    int HargaBB = 200*JumlahBotolBesar;
    int HargaBK = 100*JumlahBotolKecil;
    int Total = HargaBB + HargaBK;

    //Jika push button ditekan sekali, ubah kondisi
    if (state == LOW && buttonState == false) {
      buttonState = true;
      Serial.println("Button ON");

      // Gerak Servo Untuk Uang 200
      for (int i=0; i < JumlahBotolBesar; i++){
        servo200.write(180);
        delay(1000);
        servo200.write(90);
        delay(1000);
      }
      lcd.setCursor(0,0);
      lcd.print(("Harga BB: Rp")+String(HargaBB));

      // Gerak Servo Untuk Uang 100
      for (int j=0; j < JumlahBotolKecil; j++){
        servo100.write(180);
        delay(1000);
        servo100.write(90);
        delay(1000);
      }
      lcd.setCursor(0,1);
      lcd.print(("Harga BK: Rp")+String(HargaBK));

      delay(2000);
      lcd.clear(); 
      lcd.setCursor(0,0);
      lcd.print("      Total");
      lcd.setCursor(0,1);
      lcd.print(("      Rp")+String(Total));

      delay(2000);
      lcd.clear(); 
      lcd.setCursor(0,0);
      lcd.print(" Silahkan Ambil");
      lcd.setCursor(0,1);
      lcd.print("      Uang");

      delay(2000);
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("   Tekan Untuk");
      lcd.setCursor(0,1);
      lcd.print("     Mereset");
    }

    //Tombol Reset
    else if (state == LOW && buttonState == true) {
      buttonState = false;
      Serial.println("Button OFF");
      JumlahBotolBesar = 0;
      JumlahBotolKecil = 0;
      lcd.clear(); 
      lcd.setCursor(0,0);
      lcd.print(("Botol Besar: ")+String(JumlahBotolBesar));
      lcd.setCursor(0,1);
      lcd.print(("Botol Kecil: ")+String(JumlahBotolKecil));
    }
  
    delay(50);  // memberikan waktu jeda
  }
  
  // Serial.println("JarakBotolBesar(cm): "+String(JarakBotolBesar));
  // Serial.println("JarakBotolKecil(cm): "+String(JarakBotolKecil));
  // Serial.println("JarakFull1(cm): "+String(JarakFull1));
  // Serial.println("JarakFull2(cm): "+String(JarakFull2));
  // Serial.println("HasilBotolBesar(cm): "+String(HasilBotolBesar));
  // Serial.println("HasilBotolKecil(cm): "+String(HasilBotolKecil));
  // Serial.println("HasilPushButton: "+String(HasilPushButton));
  // delay(20);
}