#include <Arduino.h>
#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 VALVE1 = 13;
const int VALVE2 = 12;
const int PUMP = 14;
const float SPEED_OF_SOUND = 0.034 / 2;
// Define Values
const int tank1sensorHeight = 95;  // Height of the sensor in cm for Tank 1
const int tank2sensorHeight = 164; // Height of the sensor in cm for Tank 2
const int tank1height = 80;        // In cm for Tank 1
const int tank2height = 150;       // In cm for Tank 2
const int tank1radius = 45;        // In cm for Tank 1
const int tank2radius = 50;        // In cm for Tank 2
const int tank1triggerlevel = 87;  // Trigger level for Pump to start for Tank1
const int tank2triggerlevel = 149; // Trigger level for Pump to start for Tank2
const int tank1triggeroff = 15;
const int tank2triggeroff = 14;
const double pi = 3.1415926535897932384626433832795; // Value of PI
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
long duration1;
long duration2;
int distance1;
int distance2;

void setup() {
  Serial.begin(115200); //Initialize Serial Communiucation
  lcd.init(); // Initialize the LCD
  lcd.backlight();// Turn on the backlight
  intro(); //Introductary Screen
  // Set pin modes
  pinMode(TRIGGPIN1, OUTPUT);
  pinMode(ECHOPIN1, INPUT);
  pinMode(TRIGGPIN2, OUTPUT);
  pinMode(ECHOPIN2, INPUT);
  pinMode(VALVE1, OUTPUT);
  pinMode(VALVE2, OUTPUT);
  pinMode(PUMP, OUTPUT);

  lcd.clear(); // Clear LCD
}
void loop() {
  ULSensor1();
  ULSensor2();
  int waterheight1;
  int waterheight2;
  // Calculate water height for Tank 1
  if (distance1 <= 2) {
    waterheight1 = tank1height; // Set to maximum possible water height
  } else if (distance1 >= tank1sensorHeight) {
    waterheight1 = 0; // Set to minimum water height (tank empty)
  } else {
    waterheight1 = tank1sensorHeight - distance1; // Calculate water height from sensor
    if (waterheight1 > tank1height) {
      waterheight1 = tank1height; // Cap water height at tank height
    } else if (waterheight1 < 0) {
      waterheight1 = 0; // Ensure water height doesn't become negative
    }
  }
  // Calculate water height for Tank 2
  if (distance2 <= 2) {
    waterheight2 = tank2height; // Set to maximum possible water height
  } else if (distance2 >= tank2sensorHeight) {
    waterheight2 = 0; // Set to minimum water height (tank empty)
  } else {
    waterheight2 = tank2sensorHeight - distance2; // Calculate water height from sensor
    if (waterheight2 > tank2height) {
      waterheight2 = tank2height; // Cap water height at tank height
    } else if (waterheight2 < 0) {
      waterheight2 = 0; // Ensure water height doesn't become negative
    }
  }
  // Calculate tank fill percentages
  float tankFilledPercentage1 = (waterheight1 / (float)tank1height) * 100;
  float tankFilledPercentage2 = (waterheight2 / (float)tank2height) * 100;

  // Check if there's a change in the sensor readings
  if (distance1 != lastDistance1 || distance2 != lastDistance2) {
    // Print tank data via Serial
    Serial.println("------------------------------------");
    Serial.print("Tank 1 - Distance: ");
    Serial.print(distance1);
    Serial.println(" cm");
    Serial.print("Tank 1 - Water Height: ");
    Serial.print(waterheight1);
    Serial.println(" cm");
    Serial.print("Tank 1 - Tank Filled Percentage: ");
    Serial.print(tankFilledPercentage1);
    Serial.println("%");
    Serial.println("------------------------------------");
    Serial.print("Tank 2 - Distance: ");
    Serial.print(distance2);
    Serial.println(" cm");
    Serial.print("Tank 2 - Water Height: ");
    Serial.print(waterheight2);
    Serial.println(" cm");
    Serial.print("Tank 2 - Tank Filled Percentage: ");
    Serial.print(tankFilledPercentage2);
    Serial.println("%");
    Serial.println("------------------------------------");

    // Update LCD display
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Tank 1: ");
    lcd.print(tankFilledPercentage1);
    lcd.print("%");
    lcd.setCursor(0, 1);
    lcd.print("Tank 2: ");
    lcd.print(tankFilledPercentage2);
    lcd.print("%");

    // Update VALVE status
    if (distance1 >= tank1triggerlevel) {
      digitalWriteFast(VALVE1, HIGH);  // Turn on VALVE when tank is less than or equal to 10% full
    } else if (distance1 <= tank1triggeroff) {
      digitalWriteFast(VALVE1, LOW);   // Turn off VALVE when tank is 100% full
    }
    if (distance2 >= tank2triggerlevel) {
      digitalWriteFast(VALVE2, HIGH);  // Turn on VALVE when tank is less than or equal to 10% full
    } else if (distance2 <= tank2triggeroff) {
      digitalWriteFast(VALVE2, LOW);   // Turn off VALVE when tank is 100% full
    }
    // Update PUMP status
    if (digitalRead(VALVE1) || digitalRead(VALVE2)) {
      digitalWriteFast(PUMP, HIGH);  // Turn on PUMP when either tank is less than or equal to 10% full
    } else {
      digitalWriteFast(PUMP, LOW);   // Turn off PUMP when tank is 100% full
    }
    // Update last Distance values
    lastDistance1 = distance1;
    lastDistance2 = distance2;
  }
  delay(500);
}
void intro(){
  // Print introduction on LCD
  lcd.clear();
  lcd.println("WATER MONITORING");
  lcd.setCursor(3, 1);
  lcd.println("SYSTEM:1.0");
  delay(1500);
  lcd.clear();
  lcd.setCursor(7,0);
  lcd.println("ALL");
  lcd.setCursor(1,1);
  lcd.println("RIGHTSRESERVED");
  delay(750);
  lcd.clear();
  lcd.setCursor(1,0);
  lcd.println("MANOMOY MAITY");
  delay(750);
}
void ULSensor1() {
  digitalWriteFast(TRIGGPIN1, LOW);
  delayMicroseconds(2);
  digitalWriteFast(TRIGGPIN1, HIGH);
  delayMicroseconds(10);
  digitalWriteFast(TRIGGPIN1, LOW);
  duration1 = pulseIn(ECHOPIN1, HIGH);
  distance1 = duration1 * SPEED_OF_SOUND + 1;
}
void ULSensor2() {
  digitalWriteFast(TRIGGPIN2, LOW);
  delayMicroseconds(2);
  digitalWriteFast(TRIGGPIN2, HIGH);
  delayMicroseconds(10);
  digitalWriteFast(TRIGGPIN2, LOW);
  duration2 = pulseIn(ECHOPIN2, HIGH);
  distance2 = duration2 * SPEED_OF_SOUND + 1;
}