#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <WiFi.h>
#include <PubSubClient.h>

// OLED display parameters
#define SCREEN_WIDTH 128  
#define SCREEN_HEIGHT 64  
#define OLED_RESET -1     

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Sensor and device pins
const int trigPin = 18;
const int echoPin = 19;
const int ledFullPin = 2;      // LED for full fuel level
const int ledEmptyPin = 4;     // LED for empty fuel level
const int ledNormalPin = 5;    // LED for normal fuel level

// MQTT parameters
#define MQTT_CLIENT_ID "clientidweather"
#define MQTT_BROKER    "mqtt-dashboard.com"
#define MQTT_TOPIC     "FUEL" 

const char* ssid = "Wokwi-GUEST";         
const char* password = ""; 

WiFiClient espClient;
PubSubClient client(espClient);

// Global variable for tank height and thresholds
float tankHeight = 100;      // Tank height (cm)
const float EMPTY_THRESHOLD = 10; // Empty fuel level threshold (cm)
const float FULL_THRESHOLD = 90;   // Full fuel level threshold (cm)

// WiFi connection function
void connectWiFi() {
    Serial.print("Connecting to WiFi...");
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }
    Serial.println(" Connected!");
}

// MQTT reconnect function
void reconnectMQTT() {
    while (!client.connected()) {
        Serial.print("Connecting to MQTT...");
        if (client.connect(MQTT_CLIENT_ID)) {
            Serial.println(" Connected!");
        } else {
            Serial.print(" failed, rc=");
            Serial.print(client.state());
            Serial.println(" try again in 5 seconds");
            delay(5000);
        }
    }
}

// Function to send data to MQTT
void sendToMQTT(float waterLevel, const char* status) {
    String payload = "{\"waterLevel\":" + String(waterLevel) + ", \"status\":\"" + String(status) + "\"}"; 
    client.publish(MQTT_TOPIC, payload.c_str());
}

void setup() {
    Serial.begin(115200);  

    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);       
    pinMode(ledFullPin, OUTPUT);   // Full LED pin setup
    pinMode(ledEmptyPin, OUTPUT);  // Empty LED pin setup
    pinMode(ledNormalPin, OUTPUT); // Normal LED pin setup

    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        Serial.println(F("OLED display not found!"));
        while (true);
    }
    display.clearDisplay();
    display.display();

    // Connect to WiFi and MQTT
    connectWiFi();
    client.setServer(MQTT_BROKER, 1883);
}

void readDistanceAndControlPump(float &distanceCm) {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    long duration = pulseIn(echoPin, HIGH);
    distanceCm = duration * 0.034 / 2;
    Serial.print("Water level: ");
    float waterLevel = tankHeight - distanceCm; // Calculate water level
    if(waterLevel < 0 || waterLevel > tankHeight)
    {waterLevel = 0;}
    Serial.print(waterLevel);
    Serial.println(" cm");
}

// Function to display data on OLED
void displayData(float distanceCm) {
    float waterLevel = tankHeight - distanceCm; // Calculate water level
        if(waterLevel < 0 || waterLevel > tankHeight)
          {waterLevel = 0;}

    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.print("Water Level: ");
    display.print(waterLevel);
    display.println(" cm");

    // Check and display status
    String status;
    if ((tankHeight-distanceCm) > FULL_THRESHOLD) {
        status = "DAY";
        digitalWrite(ledFullPin, HIGH);  // Turn on full LED
        digitalWrite(ledEmptyPin, LOW);  // Turn off empty LED
        digitalWrite(ledNormalPin, LOW); // Turn off normal LED
    } else if ((tankHeight-distanceCm) < EMPTY_THRESHOLD) {
        status = "HET";
        digitalWrite(ledFullPin, LOW);   // Turn off full LED
        digitalWrite(ledEmptyPin, HIGH); // Turn on empty LED
        digitalWrite(ledNormalPin, LOW); // Turn off normal LED
    } else {
        status = "TRUNG_BINH";
        digitalWrite(ledFullPin, LOW);   // Turn off full LED
        digitalWrite(ledEmptyPin, LOW);  // Turn off empty LED
        digitalWrite(ledNormalPin, HIGH);// Turn on normal LED
    }

    display.println("Status: " + status);
    display.display();

    // Send status to MQTT
    sendToMQTT(waterLevel, status.c_str());
}

// Main loop
void loop() {
    float distanceCm;

    // Read distance and control pump
    readDistanceAndControlPump(distanceCm);

    // Display data on OLED
    displayData(distanceCm);

    // Check MQTT connection
    if (!client.connected()) {
        reconnectMQTT();
    }
    client.loop(); // Call MQTT loop

    delay(2000); // Read and send data every 2 seconds
}