#include <WiFi.h>
#include <WiFiClient.h>
#include <PubSubClient.h>
#include "parameters.h"
#include "ultrasonic.h"
#include "dht22.h"
#include <ESP32Servo.h>

const int servoPin = 18;
#define buzzer 2

Servo myServo; // Creates a servo object for controlling the servo motor
int angle = 0;

const char *WIFI_SSID = "Wokwi-GUEST";
const char *WIFI_PWD = "";

const char *MQTT_SERVER = "broker.hivemq.com";  // MQTT server name
const uint16_t MQTT_PORT = 1883; // MQTT port number to use

WiFiClient wifiClient;
PubSubClient mqttClient(wifiClient);
// ---------------------------


// ---------------------------
const char *topicPubSensor = "IOT/PA31/s10206236";
const char *topicPubDist = "IOT/PA31/dist";
const char *topicPubTemp = "IOT/PA31/temp";
const char *topicPubHumi = "IOT/PA31/humi";

// defines variables
unsigned long timeStamp;
long duration;
int distance;
//**************
void ConnectToWiFi()
{
  Serial.print("Connecting to WiFi ");
  Serial.println(WIFI_SSID);

  WiFi.begin(WIFI_SSID, WIFI_PWD, 6);
  while (WiFi.status() != WL_CONNECTED)
  {
    Serial.print(".");
    delay(500);
  }
  Serial.print("\nConnected to ");
  Serial.println(WIFI_SSID);
}
//--------------------------------------------------------
void SetupMqtt()
{
  mqttClient.setServer(MQTT_SERVER, MQTT_PORT);
  // set the callback function
  mqttClient.setCallback(callback);
}

void ConnectToMqtt()
{
  Serial.println("Connecting to MQTT Broker...");
  while (!mqttClient.connected())
  {
    char clientId[100] = "\0";
    sprintf(clientId, "ESP32Client-%04X", random(0xffff));
    Serial.println(clientId);

    if (mqttClient.connect(clientId))
    {
      Serial.println("Connected to MQTT broker.");
      // subscribe to topics
      MQTTSubscribe();
    }
  }
}

void callback(String topic, byte* payload, unsigned int length)
{
  String messageTemp;
  Serial.print("\nMessage received in topic: ");
  Serial.print(topic);
  Serial.print(" length is: ");
  Serial.println(length);

  Serial.print("Data received from broker: ");
  for (int i = 0; i < length; i++)
  {
    Serial.print( (char)payload[i] );
    messageTemp += (char)payload[i];
  }
//The whole code above is just to print all the message recieved (payload)

  int value = messageTemp.toInt();
  if (topic == topicPubDist) {
    if (value <= 350) {
      tone(buzzer, 262, 250);
    }
  }
  else if (topic == topicPubTemp) {
    if (value >= 35) {
      // rotates the servo motor from 0 to 180 degrees
      for(angle = 0; angle <= 180; angle+=15)
      {
        myServo.write(angle); // tells servo to go to position in variable 'angle'
        delay(100);
      }

      // rotates the servo motor from 180 to 0 degrees
      for(angle = 180; angle >= 0; angle-=15)
      {
        myServo.write(angle); // tells servo to go to position in variable 'angle'
        delay(100);
      }
    }
  }
  else if (topic == topicPubHumi) {
    if (value >= 80) {
      // rotates the servo motor from 0 to 180 degrees
      for(angle = 0; angle <= 180; angle+=15)
      {
        myServo.write(angle); // tells servo to go to position in variable 'angle'
        delay(100);
      }

      // rotates the servo motor from 180 to 0 degrees
      for(angle = 180; angle >= 0; angle-=15)
      {
        myServo.write(angle); // tells servo to go to position in variable 'angle'
        delay(100);
      }
    }
  }
  else {
    Serial.printf("\nReceived topic \"%s\" + payload \"%s\" not processed",
                  topic.c_str(), messageTemp.c_str());
  }
}

void MQTTSubscribe() 
{
  Serial.printf("\n--- Subscribe to topics:");
  Serial.printf("\n%s", topicPubSensor);
  mqttClient.subscribe(topicPubSensor);

  Serial.printf("\n%s", topicPubDist);
  mqttClient.subscribe(topicPubDist);

  Serial.printf("\n%s", topicPubTemp);
  mqttClient.subscribe(topicPubTemp);
  
  Serial.printf("\n%s", topicPubHumi);
  mqttClient.subscribe(topicPubHumi);
}

//--------------------------------------------------------
// Publish a topic with its payload
bool publishTopic(const char* topic, const char *str) {
  Serial.printf("\nPublishing topic \"%s\" + payload \"%s\"", topic, str);
  if (mqttClient.publish(topic, str)) {
    Serial.print(" -- Done");
    return true;
  }
  else {
    Serial.print("\nPublishing encounter error");
    return false;
  }
}
//--------------------------------------------------------
void publishUltrasonic() {
  char buf[6];
  distance = readUltrasonic();
  itoa(distance, buf, 10);                  // integer to string in buf
  publishTopic(topicPubDist, buf);
}
//--------------------------------------------------------
void publishTemperature() {
  int temperature;
  char buf[6];
  temperature = readAmbientTemperature();
  itoa(temperature, buf, 10);                  // integer to string in buf
  publishTopic(topicPubTemp, buf);
}
//--------------------------------------------------------
void publishHumidity() {
  int humidity;
  char buf[6];
  humidity = readAmbientHumidity();
  itoa(humidity, buf, 10);                  // integer to string in buf
  publishTopic(topicPubHumi, buf);
}
// ---------------------------

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  timeStamp = millis();
  pinMode(buzzer,OUTPUT);
  myServo.attach(18); // Defines the attached pin of servo motor 

  ConnectToWiFi();
  SetupMqtt();
  setup_ultrasonic();
  setup_dht();
}

void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
  if (!mqttClient.connected())
  {
    ConnectToMqtt();
  }
  mqttClient.loop(); //make sure connection is not broken, auto restart the connection

  if (millis() - timeStamp >= 1000) {
    publishUltrasonic();
    publishTemperature();
    publishHumidity();

    timeStamp = millis();
  }
}