#include <Arduino.h>
#include<digitalWriteFast.h>
#include<LiquidCrystal_I2C.h>
#include<WiFi.h>
#include<PubSubClient.h>

// Set the LCD address to 0x27 for a 16 chars and 2 line display
LiquidCrystal_I2C lcd(0x27, 16, 2);

// WiFi Credentials:
const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqtt_server = "test.mosquitto.org";

WiFiClient espClient;
PubSubClient client(espClient);

// Connecting to WiFi
void setupWiFi() {
  delay(1000);
  Serial.print("Connecting to ");
  lcd.setCursor(0, 0);
  lcd.print("Connecting to ");
  Serial.println(ssid);
  lcd.setCursor(0, 1);
  lcd.print(ssid);
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    spinner();
    Serial.print(".");
    delay(500);
  }
  Serial.println("\nConnected to WiFi");
}

// Reconnect to MQTT server
void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");

    // Attempt to connect
    if (client.connect("espClient")) {
      Serial.println("connected");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

// 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
int tankheight1 = 80;     // In cm for Tank 1
int tankheight2 = 80;     // In cm for Tank 2
int sensorHeight1 = 95;   // Height of the sensor in cm for Tank 1
int sensorHeight2 = 95;   // Height of the sensor in cm for Tank 2
int tankradius1 = 45;     // In cm for Tank 1
int tankradius2 = 45;     // In cm for Tank 2
double pi = 3.1415926535897932384626433832795; // Value of PI

void setup() {
  Serial.begin(115200);
  // Initialize the LCD
  lcd.init();

  // Turn on the backlight
  lcd.backlight();

  // Print introduction on LCD
  intro();

  // Set pin modes
  pinMode(TRIGGPIN1, OUTPUT);
  pinMode(ECHOPIN1, INPUT);
  pinMode(TRIGGPIN2, OUTPUT);
  pinMode(ECHOPIN2, INPUT);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);

  // Test sequence
  /*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); */

  // Clear LCD
  lcd.clear();

  setupWiFi();
  client.setServer(mqtt_server, 1883);
}

void loop() {
  if (!client.connected()) {
    reconnect();
  }
  client.loop();

  ULSensor1();
  ULSensor2();
  int waterheight1;
  int waterheight2;

  // Calculate water height for Tank 1
  if (distance1 <= 2) {
    waterheight1 = tankheight1; // Set to maximum possible water height
  } else if (distance1 >= sensorHeight1) {
    waterheight1 = 0; // Set to minimum water height (tank empty)
  } else {
    waterheight1 = sensorHeight1 - distance1; // Calculate water height from sensor
    if (waterheight1 > tankheight1) {
      waterheight1 = tankheight1; // 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 = tankheight2; // Set to maximum possible water height
  } else if (distance2 >= sensorHeight2) { // Adjusted for Tank 2
    waterheight2 = 0; // Set to minimum water height (tank empty)
  } else {
    waterheight2 = sensorHeight2 - distance2; // Calculate water height from sensor
    if (waterheight2 > tankheight2) {
      waterheight2 = tankheight2; // 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)tankheight1) * 100;
  float tankFilledPercentage2 = (waterheight2 / (float)tankheight2) * 100;

  // Check if there's a change in the sensor readings
  if (distance1 != lastDistance1 || distance2 != lastDistance2) {
    // Print tank data via Serial
    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("%");
    //
    if(tankFilledPercentage1==100){
      int tankcursor1=14;
      else if(tankFilledPercentage1<100 && tankFilledPercentage1>0){
        int tankcursor1=13;
        else(tankFilledPercentage1==0){
          int tankcursor1=12;
        }
      }
    }
    // Update LCD display
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Tank 1: ");
    lcd.setCursor(8, 0);
    lcd.print(tankFilledPercentage1);
    lcd.setCursor(tankcursor1, 0);
    lcd.print("%");

    lcd.setCursor(0, 1);
    lcd.print("Tank 2: ");
    lcd.setCursor(8, 1);
    lcd.print(tankFilledPercentage2);
    lcd.setCursor(14, 1);
    lcd.print("%");

    // Update LED status
    // Update LED status
    if (distance1 >= 87) {
      digitalWriteFast(LED1, HIGH);  // Turn on LED when tank is less than or equal to 10% full
    } else if (distance1 <= 16) {
      digitalWriteFast(LED1, LOW);   // Turn off LED when tank is 100% full
    }

    if (distance2 >= 87) {
      digitalWriteFast(LED2, HIGH);  // Turn on LED when tank is less than or equal to 10% full
    } else if (distance2 <= 16) {
      digitalWriteFast(LED2, LOW);   // Turn off LED when tank is 100% full
    }
    // Update last distance values
    lastDistance1 = distance1;
    lastDistance2 = distance2;
  }

  // Delay before next loop iteration
  delay(500);
}

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;
}

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

void volume(int waterheight, float tankFilledPercentage, int tankNumber) {
  // Calculate volume of water in tank
  if (waterheight == 0) {
    Serial.print("Tank ");
    Serial.print(tankNumber);
    Serial.println(" is empty"); // Print a message indicating the tank is empty
  } else {
    float tankvolume = pi * (tankNumber == 1 ? tankradius1 : tankradius2) * (tankNumber == 1 ? tankradius1 : tankradius2) * waterheight;
    float litre = tankvolume * 0.001;
    // Print volume
    Serial.print("Volume of Tank ");
    Serial.print(tankNumber);
    Serial.print(" is: ");
    Serial.print(litre);
    Serial.println(" L");

    // Print tank filled percentage
    Serial.print("Tank ");
    Serial.print(tankNumber);
    Serial.print(" filled percentage: ");
    Serial.print(tankFilledPercentage);
    Serial.println("%");
  }
}

void spinner() {
  static int8_t counter = 0;
  const char* glyphs = "\xa1\xa5\xdb";
  lcd.setCursor(15, 0);
  lcd.print(glyphs[counter++]);
  if (counter == strlen(glyphs)) {
    counter = 0;
  }
}