// ------------ Don't mind the code encapsulated here ------------
#include <WiFi.h>
#include "PubSubClient.h"

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

String stMac;
char mac[50];
char clientId[50];

WiFiClient espClient;
PubSubClient client(espClient);
// ------------ Don't mind the code encapsulated here ------------

// ------------ Change as needed below:
const char *mqttServer = "mqtt.flespi.io";                                                  // set mqtt broker, dont change
const char *mqtt_user = "njnM0qzk8XibJBFKBomq1mCCzE54YpubwHNnVavTVBVu2q5tCfHrp7TOBNGp21qp"; // change your token in flespi
const char *mqtt_password = "";                                                          // change with your corresponding password below the token in flespi
int port = 1883;                                                                            // set mqtt port, dont change

// change here your lastnames
const char *topicFromNodered = "Pulvera/Bucol/fromNodered";
const char *topicFromWokwi = "Pulvera/Bucol/fromWokwi";

const int trigPin = 33; // Ultrasonic pins
const int echoPin = 32; // Ultrasonic pins

float duration_us, dist_now, dist_prev, dist_diff; // Ultrasonic variables
float threshold = 1; // for noise filtering

int switchVal, prevVal;

void setup()
{
    Serial.begin(115200);
    randomSeed(analogRead(0));

    delay(10);
    Serial.println();
    Serial.print("Connecting to ");
    Serial.println(ssid);

    wifiConnect();

    Serial.println("");
    Serial.println("WiFi connected");
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());
    Serial.println(WiFi.macAddress());
    stMac = WiFi.macAddress();
    stMac.replace(":", "_");
    Serial.println(stMac);

    client.setServer(mqttServer, port);
    client.setCallback(callback); // Listen to the incoming MQTT messages - change here the topic

    pinMode(trigPin, OUTPUT); // Ultrasonic initialization
    pinMode(echoPin, INPUT);  // Ultrasonic initialization
}

// Connect to the virtual Wokwi wifi generated to create a VPN
void wifiConnect()
{
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED)
    {
        delay(500);
        Serial.print(".");
    }
}

void mqttReconnect()
{
    while (!client.connected())
    {
        Serial.print("Attempting MQTT connection...");
        long r = random(1000);
        sprintf(clientId, "clientId-%ld", r);
        if (client.connect(clientId, mqtt_user, mqtt_password))
        {
            Serial.print(clientId);
            Serial.println(" connected");
            client.subscribe(topicFromNodered);
        }
        else
        {
            Serial.print("failed, rc=");
            Serial.print(client.state());
            Serial.println(" try again in 5 seconds");
            delay(5000);
        }
    }
}

// What to do with the message (in our case, we want to turn control the Wokwi LED)
void callback(char *topic, byte *message, unsigned int length)
{
    Serial.print("Message arrived on topic: ");
    Serial.print(topic);
    Serial.print(". Message: ");
    String stMessage;

    for (int i = 0; i < length; i++)
    {
        Serial.print((char)message[i]);
        stMessage += (char)message[i];
    }
    Serial.println();
}

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

    // Ultrasonic publish data
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    duration_us = pulseIn(echoPin, HIGH);
    dist_now = 0.017 * duration_us;
    dist_diff = abs(dist_now - dist_prev);

    if(dist_diff > threshold )
    {
      Serial.print("distance: ");
      Serial.print(dist_now);
      Serial.println(" cm");

      String data = String(dist_now, 0);
      const char *sensorData = data.c_str();

      client.publish(topicFromWokwi, sensorData);

      dist_prev = dist_now;
    }

  delay(500);
}