#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";

// MQTT topics
const char*TANK1_PERCENTAGE = "device/mano/data/watermonitoringsystem/tank/1/percentage";
const char*TANK2_PERCENTAGE = "device/mano/data/watermonitoringsystem/tank/2/percentage";
const char*TANK1_VOLUME = "device/mano/data/watermonitoringsystem/tank/1/volume";
const char*TANK2_VOLUME = "device/mano/data/watermonitoringsystem/tank/2/volume";

WiFiClient espClient;
PubSubClient client(espClient);

//Pin Define
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;

//--Values--
const int tank1sensorHeight = 95;
const int tank2sensorHeight = 95;

const int tank1height = 80;
const int tank2height = 80;

const int tank1gap = tank1sensorHeight-tank1height;
const int tank2gap = tank2sensorHeight-tank2height;

const int tank1radius = 45;
const int tank2radius = 45;

const int tank1triggerlevel = 40;
const int tank2triggerlevel = 40;

const int tank1triggeroff = 80;
const int tank2triggeroff = 80;

constexpr double pi = 3.14159265358979323846;

int waterheight1;
int waterheight2;

int distance1;
int distance2;

long duration1;
long duration2;

int lastDistance1 = -1;
int lastDistance2 = -1;

const float SPEED_OF_SOUND = 0.034;

void setup() {

  Serial.begin(115200);

  //Pin Modes
  pinMode(TRIGGPIN1, OUTPUT);
  pinMode(ECHOPIN1, INPUT);
  pinMode(TRIGGPIN2, OUTPUT);
  pinMode(ECHOPIN2, INPUT);
  pinMode(VALVE1, OUTPUT);
  pinMode(VALVE2, OUTPUT);
  pinMode(PUMP, OUTPUT);

  lcd.init();
  lcd.backlight();
  lcd.clear();
  intro();

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

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

  ULSensor1();
  ULSensor2();

  if (distance1 <= tank1gap) {
    waterheight1 = tank1height;
  }
  if (distance2 <= tank2gap) {
    waterheight2 = tank2height;
  }
  
  lcd.setCursor(0,0);
  lcd.print(distance1);
  lcd.setCursor(0,1);
  lcd.print(distance2);



  lastDistance1 = distance1;
  lastDistance2 = distance2;
}




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

void reconnect() {
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Attempting MQTT");
    lcd.setCursor(0, 1);
    lcd.print(" connection...");

    if (client.connect("espClient")) {
      Serial.println("connected");
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("connected");
    } else {
      Serial.print("failed, rc=");
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("failed!!");
      delay(50);
      Serial.print(client.state());

      Serial.println(" try again in 5 seconds");
      lcd.setCursor(0, 0);
      lcd.print("try again in");
      lcd.setCursor(0, 1);
      lcd.print("5 seconds");
      delay(5000);
    }
  }
}

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

  distance1 = round(distance1 * 100) / 100;
}

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

  distance2 = round(distance2 * 100) / 100;
}

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

void intro() {
  // Print introduction on LCD
  lcd.clear();
  lcd.println("WATER TANK");
  lcd.setCursor(3, 1);
  lcd.println("SYSTEM:2025");
  delay(1500);
  lcd.clear();
  lcd.setCursor(7, 0);
  lcd.println("ALL");
  lcd.setCursor(2, 1);
  lcd.println("RIGHTSRESERVED");
  delay(1000);
  lcd.clear();
  lcd.setCursor(1, 0);
  lcd.println("MANOMOY MAITY");
  delay(750);

}