#include<digitalWriteFast.h>
#include <LiquidCrystal_I2C.h>
// Set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C lcd(0x27, 16, 2);
// Define Pins
const int TRIGGPIN1 = 4;
const int TRIGGPIN2 = 19;
const int ECHOPIN1 = 5;
const int ECHOPIN2 = 18;
const int LED1 = 13;
const int LED2 = 12;
const float SPEED_OF_SOUND = 0.034 / 2;
long duration1;
long duration2;
int distance1;
int distance2;
int lastDistance1 = -1; // Initialize with a value that's outside the normal range of distances
int lastDistance2 = -1; // Initialize with a value that's outside the normal range of distances

void setup() {
  Serial.begin(115200);
  // Initialize the LCD
  lcd.init();
  // Turn on the backlight
  lcd.backlight();
  intro();
  pinMode(TRIGGPIN1, OUTPUT);
  pinMode(ECHOPIN1, INPUT);
  pinMode(TRIGGPIN2, OUTPUT);
  pinMode(ECHOPIN2, INPUT);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  Serial.println("Test Sequence Initiating");
  digitalWriteFast(LED1, HIGH);
  delay(500);
  digitalWriteFast(LED2, HIGH);
  delay(500);
  digitalWriteFast(LED1, LOW);
  delay(500);
  digitalWriteFast(LED2, LOW);
  delay(500);
  lcd.clear();
}

void loop() {
  ULSensor1();
  ULSensor2();
  // Prints the distance on the Serial Monitor
  if (distance1 != lastDistance1 || distance2 != lastDistance2) {
    distance();
    value();
    Serial.print("Distance1: ");
    Serial.print(distance1);
    Serial.println(" cm");
    Serial.print("Distance2: ");
    Serial.print(distance2);
    Serial.println(" cm\n");
    lastDistance1 = distance1;
    lastDistance2 = distance2;
  }
  delay(1000);
  digitalWriteFast(LED1, distance1 < 100);
  digitalWriteFast(LED2, distance2 < 100);
}

void ULSensor1() {
  digitalWriteFast(TRIGGPIN1, LOW);
  delayMicroseconds(2);
  digitalWriteFast(TRIGGPIN1, HIGH);
  delayMicroseconds(10);
  digitalWriteFast(TRIGGPIN1, LOW);
  duration1 = pulseIn(ECHOPIN1, HIGH);
  distance1 = duration1 * SPEED_OF_SOUND;
}

void ULSensor2() {
  digitalWriteFast(TRIGGPIN2, LOW);
  delayMicroseconds(2);
  digitalWriteFast(TRIGGPIN2, HIGH);
  delayMicroseconds(10);
  digitalWriteFast(TRIGGPIN2, LOW);
  duration2 = pulseIn(ECHOPIN2, HIGH);
  distance2 = duration2 * SPEED_OF_SOUND;
}

void intro() {
  //Introductary Screen
  lcd.println("WATER MONITORING");
  lcd.setCursor(0, 1);
  lcd.println("SYSTEM:1.0");
}

void distance() {
  //Distance Mesurement screen
  lcd.setCursor(0, 0);
  lcd.print("Distance 1:   cm");
  lcd.setCursor(0, 1);
  lcd.print("Distance 2:   cm");

}
void value() {
  lcd.setCursor(11, 0);
  lcd.print(distance1);
  lcd.setCursor(11, 1);
  lcd.print(distance2);
}