#include <WiFi.h>
#include <WiFiClient.h>
#include <PubSubClient.h>
#include <ESP32Servo.h>

const int redLEDPin = 23;
const int yellowLEDPin = 19;
const int greenLEDPin = 18;
const int servoPin = 25;

int LEDStateR = LOW;
unsigned long lastRedBlinkTime = 0;
const unsigned long redBlinkIntervalInit = 2000;
int redBlinkInterval = redBlinkIntervalInit;

unsigned long lastGreenBlinkTime = 0;
int greenBlinkInterval = 2000;

int analogPin = A5;
unsigned long previousMillisAnalog = 0;
const unsigned long intervalAnalog = 500;

const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqttBroker = "broker.hivemq.com";
const char* clientName = "SmartTrashCanTracker";
const int mqttPort = 1883;

WiFiClient espClient;
PubSubClient client(espClient);

const char* subscribeTopic = "smart_trash_can/GREEN_LED";
const char* publishTopic = "smart_trash_can/status";
const char* publishGarbageLevelTopic = "IoT/LT50/3861590p";

const int garbageThreshold = 500;
const int TRASH_FULL_THRESHOLD = 80;
const int TRASH_OVERFILLED_THRESHOLD = 100;

Servo trashServo;
int servoAngle = 0;
const int SERVO_OPEN_ANGLE = 90;
const int SERVO_CLOSE_ANGLE = 0;

void setup_wifi() {
    Serial.println("Connecting to WiFi...");
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) {
        delay(1000);
        Serial.print(".");
    }
    Serial.println("\nWiFi connected!");
    Serial.println("IP address: " + WiFi.localIP().toString());
}

void connectMQTT() {
    Serial.println("Connecting to MQTT...");
    while (!client.connected()) {
        if (client.connect(clientName)) {
            Serial.println("MQTT connected!");
            client.subscribe(subscribeTopic);
        } else {
            Serial.println("MQTT connection failed, retrying...");
            delay(2000);
        }
    }
}

void callback(char* topic, byte* payload, unsigned int length) {
    String message;
    Serial.print("Received message on topic: ");
    Serial.println(topic);
    Serial.print("Message length: ");
    Serial.println(length);
    Serial.print("Message content: ");
    for (unsigned int i = 0; i < length; i++) {
        message += (char)payload[i];
        Serial.print((char)payload[i]);
    }
    Serial.println();

    if (String(topic) == subscribeTopic) {
        if (message == "Blink") {
            greenBlinkInterval = 2000;
        } else if (message == "Off") {
            digitalWrite(greenLEDPin, HIGH);
        } else if (message == "On") {
            digitalWrite(greenLEDPin, LOW);
        }
    }
}

String checkTrashStatus() {
    int fillLevel = random(0, 101);
    if (fillLevel >= TRASH_OVERFILLED_THRESHOLD) {
        return "Overfilled";
    } else if (fillLevel >= TRASH_FULL_THRESHOLD) {
        return "Full";
    } else {
        return "Not Full";
    }
}

void publishTrashStatus() {
    String status = checkTrashStatus();
    if (client.publish(publishTopic, status.c_str())) {
        if (status == "Not Full") {
            redBlinkInterval = 2000;
            greenBlinkInterval = 2000;
            trashServo.write(SERVO_OPEN_ANGLE);
        } else if (status == "Full") {
            redBlinkInterval = 1000;
            greenBlinkInterval = 1000;
            trashServo.write(SERVO_CLOSE_ANGLE);
        } else if (status == "Overfilled") {
            redBlinkInterval = 500;
            greenBlinkInterval = 500;
            trashServo.write(SERVO_CLOSE_ANGLE);
        }
    } else {
        Serial.println("Failed to publish status!");
    }
}

void blinkRedLED() {
    if (millis() - lastRedBlinkTime >= redBlinkInterval) {
        digitalWrite(redLEDPin, !digitalRead(redLEDPin));
        lastRedBlinkTime = millis();
    }
}

void blinkGreenLED() {
    if (millis() - lastGreenBlinkTime >= greenBlinkInterval) {
        digitalWrite(greenLEDPin, !digitalRead(greenLEDPin));
        lastGreenBlinkTime = millis();
    }
}

void readAndPublishGarbageLevel() {
    unsigned long currentMillis = millis();
    if (currentMillis - previousMillisAnalog >= intervalAnalog) {
        previousMillisAnalog = currentMillis;
        int analogValue = analogRead(analogPin);
        String payload = String(analogValue);
        client.publish(publishGarbageLevelTopic, payload.c_str());
        if (analogValue > garbageThreshold) {
            digitalWrite(yellowLEDPin, HIGH);
        } else {
            digitalWrite(yellowLEDPin, LOW);
        }
    }
}

void setup_MQTT() {
    client.setServer(mqttBroker, mqttPort);
    client.setCallback(callback);
}

void setup() {
    Serial.begin(115200);
    setup_wifi();
    setup_MQTT();
    pinMode(redLEDPin, OUTPUT);
    pinMode(yellowLEDPin, OUTPUT);
    pinMode(greenLEDPin, OUTPUT);
    trashServo.attach(servoPin);
    trashServo.write(servoAngle);
    connectMQTT();
}

void loop() {
    if (!client.connected()) {
        connectMQTT();
    }
    client.loop();
    static unsigned long lastPublishTime = 0;
    const unsigned long PUBLISH_INTERVAL = 5000;
    if (millis() - lastPublishTime >= PUBLISH_INTERVAL) {
        publishTrashStatus();
        lastPublishTime = millis();
    }
    blinkRedLED();
    blinkGreenLED();
    readAndPublishGarbageLevel();
}