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

const int trigPin = 4;
const int echoPin = 15;
const int PIN_LED = 2;

//define sound speed in cm/uS
#define SOUND_SPEED 0.034
#define CM_TO_INCH  0.393701

/* Configuration of Topic to MQTT */
#define ID_MQTT "damdam_mqtt_ultraupper" // Device ID for connect to MQTT Broker

#define TOPIC_SUBSCRIBE_LED    "damdam/led"
#define TOPIC_PUBLISH_DISTANCE "damdam/ultraupper"

#define PUBLISH_DELAY 2000   // Delay time for publish data to MQTT Broker
unsigned long publishUpdate;

const char *SSID = "Wokwi-GUEST"; // SSID / nome da rede WI-FI que deseja se conectar
const char *PASSWORD = "";        // Senha da rede WI-FI que deseja se conectar

// URL for MQTT broker
const char *BROKER_MQTT = "broker.hivemq.com";
int BROKER_PORT = 1883; // Port of Broker MQTT

float distance;
static char strDistance[10] = {0};

// Variable for Wifi Client Object
WiFiClient espClient;         
PubSubClient MQTT(espClient);


/* Initial WI-FI  */
void initWiFi(void)
{
  delay(10);
  Serial.println("------Connect to WI-FI------");
  Serial.print("Connecting to : ");
  Serial.println(SSID);

  reconnectWiFi();
}

/* Initial MQTT  */
void initMQTT(void)
{
  MQTT.setServer(BROKER_MQTT, BROKER_PORT); 
  MQTT.setCallback(callbackMQTT);
}

/* Function Callback: Receive Message from another MQTT Client publish message pass through the MQTT Broker */
void callbackMQTT(char *topic, byte *payload, unsigned int length)
{
  String msg;

  // Obtem a string do payload recebido
  for (int i = 0; i < length; i++) {
    char c = (char)payload[i];
    msg += c;
  }

  Serial.printf("Receive string via MQTT: %s on topico: %s\n", msg, topic);

  /* Toma ação dependendo da string recebida */
  if (msg.equals("on")) {
    digitalWrite(PIN_LED, HIGH);
    Serial.println("LED status ON");
  }

  if (msg.equals("off")) {
    digitalWrite(PIN_LED, LOW);
    Serial.println("LED status OFF");
  }
}

/* FUnction reconnect MQTT Broker */
void reconnectMQTT(void)
{
  while (!MQTT.connected()) {
    Serial.print("* Connecting MQTT Broker: ");
    Serial.println(BROKER_MQTT);
    if (MQTT.connect(ID_MQTT)) {
      Serial.println("Connecting the MQTT Broker success!");
      MQTT.subscribe(TOPIC_SUBSCRIBE_LED);
    } else {
      Serial.println("Fail to connect then reconnect MQTT Broker again in 2 seconds.");
      delay(2000);
    }
  }
}

/* Function checking connected to WiFI and MQTT Broker */
void checkWiFIAndMQTT(void)
{
  if (!MQTT.connected())
    reconnectMQTT(); 
  reconnectWiFi(); 
}

void reconnectWiFi(void)
{
  if (WiFi.status() == WL_CONNECTED)
    return;

  WiFi.begin(SSID, PASSWORD); 

  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    Serial.print(".");
  }

  Serial.println();
  Serial.print("Connected WiFi success. ");
  Serial.println(SSID);
  Serial.print("IP Address: ");
  Serial.println(WiFi.localIP());
}

float getDistance(){

  long  duration;
  float distanceCm;

  // Clears the trigPin
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    // Sets the trigPin on HIGH state for 10 micro seconds
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    
    // Reads the echoPin, returns the sound wave travel time in microseconds
    duration = pulseIn(echoPin, HIGH);
    
    // Calculate the distance
    distanceCm =duration * SOUND_SPEED/2;

    return(distanceCm);
}

void setup() {
  Serial.begin(115200); // Starts the serial communication

  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED, LOW);

  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input

  initWiFi();
  initMQTT();
}

void loop() {

   /* Repeat every 2 seconds (PUBLISH_DALAY)*/

  if ((millis() - publishUpdate) >= PUBLISH_DELAY) {
    publishUpdate = millis();
    checkWiFIAndMQTT();

    distance = getDistance();   

    // Change format of the Data as String for send it via MQTT protocol
    sprintf(strDistance, "%.2f", distance);

    // Send data string to MQTT Broker
    MQTT.publish(TOPIC_PUBLISH_DISTANCE, strDistance);

    // Prints the distance in the Serial Monitor
    Serial.print("Distance (cm): ");
    Serial.println(distance);

  }
}