/*Smart Water Pump*/

#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 20, 4);

#define TRIGGER_PIN 3
#define ECHO_PIN 2
#define truck 9 //truck datang atau pergi 
#define indicated 10 //tindikator ketinggian air
#define water_pump 11 //pompa air
#define full 12 //indikator full

byte mode;
long duration;
int prevDistance,distance;
bool jalan,hole1,hole2;
byte tampil = HIGH;

void kedip(bool logika1, bool logika2, int jeda){
  digitalWrite(indicated,logika1);
  delay(jeda);
  digitalWrite(indicated,logika2);
  delay(jeda);
}
//---------------------Truck datang dan baca sensor -----------------------
void baca(){
  //--Mengirimkan sinyal ultrasonik
  digitalWrite(TRIGGER_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);
  
  // Menerima echo dan menghitung jarak
  duration = pulseIn(ECHO_PIN, HIGH);
  distance = duration * 0.034 / 2;
  delay(100);
  prevDistance = distance;
  Serial.println(distance);
}

void readFirst(){
  baca();

  if(tampil == HIGH){
    lcd.clear();
    lcd.setCursor(0,0); lcd.print("Status :"); 
    lcd.setCursor(8,0); lcd.print("No Truck");
    lcd.setCursor(0,1); lcd.print("Jarak  :"); 
    lcd.setCursor(10,1); lcd.print(distance);
    lcd.setCursor(14,1); lcd.print("cm"); 
  }
  
//--logic to SELECT truck A or truck B
  if(!jalan){
    if (distance > 100) {
      mode = 0;
      digitalWrite(truck, LOW);
      digitalWrite(indicated, LOW);
      digitalWrite(water_pump, LOW);
      digitalWrite(full, LOW);

      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("No Truck");
      lcd.setCursor(0,1); lcd.print("Jarak  :");  
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm"); 
      tampil = LOW;
    } 
    else if (distance > 50 && distance < 60) {
      digitalWrite(truck, HIGH);
      mode = 1;
      jalan = true;
    } 
    else if (distance > 40 && distance < 50) {
      digitalWrite(truck, HIGH);
      mode = 2;
      jalan = true;
    }
   }

//--logic to function truck A or truck B
   else if((jalan) && (mode == 1)){
    truckA();

    lcd.clear();
    lcd.setCursor(0,0); lcd.print("Status :"); 
    lcd.setCursor(8,0); lcd.print("Truck A");
    lcd.setCursor(0,1); lcd.print("Jarak  :");  
    lcd.setCursor(10,1); lcd.print(distance);
    lcd.setCursor(14,1); lcd.print("cm"); 
    tampil = LOW;
   }
   
   else if((jalan) && (mode == 2)){
    truckB();

    lcd.clear();
    lcd.setCursor(0,0); lcd.print("Status :"); 
    lcd.setCursor(8,0); lcd.print("Truck B");
    lcd.setCursor(0,1); lcd.print("Jarak  :");  
    lcd.setCursor(10,1); lcd.print(distance);
    lcd.setCursor(14,1); lcd.print("cm"); 
    tampil = LOW;
   } 
}

//---------------------Set Position Parkir-----------------------

void truckA(){
//--kapasitas 1.5 kal 
  baca();

//--logic tunggu hingga pas lubang tangki
//--bisa menggunakan delay 2 minute atau menunggu pas lubang selama 
  if(!hole1) {
    if((distance > 50 && distance < 100) && (distance <= prevDistance+5) ) { 
      
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Jeda 10s");
      lcd.setCursor(0,1); lcd.print("Jarak  :");  
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm"); 
      tampil = LOW;
    
      long parkir = millis();
      while (millis() - parkir < 5000) {
        if (distance > 250) {
          return; 
        }
        duration = pulseIn(ECHO_PIN, HIGH);
        distance = duration * 0.034 / 2;
      }
      delay(5000);
      hole1 = true;
      
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Hole A");
      lcd.setCursor(0,1); lcd.print("Jarak  :");  
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm"); 
      tampil = LOW;
    }
  }
  
  if(hole1){
    if (distance < 50) { // full
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Full");
      lcd.setCursor(0,1); lcd.print("Jarak  :"); 
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm"); 
      tampil = LOW;
      
      digitalWrite(water_pump, LOW); //pompa off
      digitalWrite(full, HIGH);
      kedip(0,0,0);
      jalan = false;
      hole1 = false;
    }
    else if (distance > 80 && distance < 100) { // Empty
      kedip(1,0,2000);
      digitalWrite(water_pump, HIGH);
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Empty");
      lcd.setCursor(0,1); lcd.print("Jarak  :");  
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm"); 
      tampil = LOW;
    }
    else if (distance > 60 && distance < 80) { // 2/3 dari 1.5
      kedip(1,0,1000);
      digitalWrite(water_pump, HIGH);
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Low");
      lcd.setCursor(0,1); lcd.print("Jarak  :");  
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm"); 
      tampil = LOW;
    }
    else if (distance > 51 && distance < 60) { // 1/3 dari 1.5
      kedip(1,0,500);
      digitalWrite(water_pump, HIGH);
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Medium");
      lcd.setCursor(0,1); lcd.print("Jarak  :");  
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm"); 
      tampil = LOW;
    }
  }
}

//---------------------Set Position Parkir-----------------------

void truckB(){
//--kapasitas 1.5 kal 
  baca();

//--logic tunggu hingga pas lubang tangki
//--bisa menggunakan delay 2 minute atau menunggu pas lubang selama 
  if(!hole2) {
    if((distance > 40 && distance < 100) && distance <= prevDistance+5 ) { 
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Jeda 10s");
      lcd.setCursor(0,1); lcd.print("Jarak  :"); 
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm"); 
      tampil = LOW;
      
      long parkir = millis();
      while (millis() - parkir < 5000) {
        if (distance > 250) {
          return; 
        }
        duration = pulseIn(ECHO_PIN, HIGH);
        distance = duration * 0.034 / 2;
      }
      delay(5000);
      hole2 = true;
      
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Hole B");
      lcd.setCursor(0,1); lcd.print("Jarak  :"); 
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm");
      tampil = LOW;
    }
  }
  
  if(hole2){
    if (distance < 40) { // full
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Full");
      lcd.setCursor(0,1); lcd.print("Jarak  :");  
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm"); 
      tampil = LOW;
      
      digitalWrite(water_pump, LOW); //pompa off
      digitalWrite(full, HIGH);
      kedip(0,0,0);
      jalan = false;
      hole2 = false;
    }
    else if (distance > 85 && distance < 100) { // empty
      kedip(1,0,2000);
      digitalWrite(water_pump, HIGH);
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Empty");
      lcd.setCursor(0,1); lcd.print("Jarak  :"); 
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm"); 
      tampil = LOW;
    }
    else if (distance > 65 && distance < 85) { // 2/3 dari 1.5
      kedip(1,0,1000);
      digitalWrite(water_pump, HIGH);
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Low");
      lcd.setCursor(0,1); lcd.print("Jarak  :"); 
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm"); 
      tampil = LOW;
    }
    else if (distance > 41 && distance < 65) { // 1/3 dari 1.5
      kedip(1,0,500);
      digitalWrite(water_pump, HIGH);
      lcd.clear();
      lcd.setCursor(0,0); lcd.print("Status :"); 
      lcd.setCursor(8,0); lcd.print("Medium");
      lcd.setCursor(0,1); lcd.print("Jarak  :"); 
      lcd.setCursor(10,1); lcd.print(distance);
      lcd.setCursor(14,1); lcd.print("cm");
      tampil = LOW;
    }
  }
}

void setup() {
  lcd.init();
  lcd.backlight();
  
  lcd.setCursor(4,0); lcd.print("WELCOME");
  lcd.setCursor(0,1); lcd.print("WATERMATIS v1.0");
  delay(1500);
  lcd.clear();
  
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(truck, OUTPUT);
  pinMode(indicated, OUTPUT);
  pinMode(water_pump, OUTPUT);
  pinMode(full, OUTPUT);
  
  digitalWrite(truck, LOW);
  digitalWrite(indicated, LOW);
  digitalWrite(water_pump, LOW);
  digitalWrite(full, LOW);
  Serial.begin(9600);
  jalan = false;
  hole1 = false;
  hole2 = false;
}

void loop() {
  readFirst();
}