#include <WiFi.h>
#include <PubSubClient.h>
#include <ArduinoJson.h>  //include new library for json



#define MQTT_interval 1000 * 10  // Interval in milliseconds (1000 ms = 1 second)
static uint32_t MQTT_previousMillis = 0;

//Basic Safety Message (MQTT)
typedef struct message {
  uint32_t Vehicle_ID;   //Vehicle unique number (5 chasais number)
  double Long;           //Sender Longitude
  double Lat;            //Sender Latitude
  uint16_t Speed;        //Vehicle Speed
  float Heading;         //Vehicle's Direction in degree
  char Vehicle_Type[5];  //Vehicle Type (car,bus...)
  char Event[3];         //Additional sensor data if needed (AC, EB)
  bool Flag = false;     //flag if wanted
} message;

message MQTT_Msg, MQTT_Msg2;

// WiFi
const char *ssid = "Wokwi-GUEST";         // Enter your WiFi name
const char *password = "";  // Enter WiFi password

// MQTT Broker
const char *mqtt_broker = "broker.emqx.io";
const char *topic1 = "ST9";
const char *mqtt_username = "emqx";
const char *mqtt_password = "public";
const int mqtt_port = 1883;

WiFiClient espClient;
PubSubClient client(espClient);

//prototypes
void MQTT_Send(const char *topic, message msg);
void callback(char *topic, byte *payload, unsigned int length);


void setup() {

  uint8_t Car = 1;

  if (Car == 1) {
    MQTT_Msg.Vehicle_ID = 12345;
    MQTT_Msg.Long = 29.92264;
    MQTT_Msg.Lat = 31.56702;
    MQTT_Msg.Speed = 100;
    MQTT_Msg.Heading = 90;
    memcpy(MQTT_Msg.Vehicle_Type, "Car", sizeof("Car"));
    memcpy(MQTT_Msg.Event, "AC", sizeof("AC"));
    MQTT_Msg.Flag = true;
  } else if (Car == 2) {
    MQTT_Msg.Vehicle_ID = 56789;
    MQTT_Msg.Long = 29.92307;
    MQTT_Msg.Lat = 31.57018;
    MQTT_Msg.Speed = 120;
    MQTT_Msg.Heading = 90;
    memcpy(MQTT_Msg.Vehicle_Type, "Car", sizeof("Car"));
    memcpy(MQTT_Msg.Event, "AC", sizeof("AC"));
    MQTT_Msg.Flag = true;
  }
  // Set software serial baud to 115200;
  Serial.begin(115200);
  // Connecting to a WiFi network
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.println("Connecting to WiFi..");
  }
  Serial.println("Connected to the Wi-Fi network");
  //connecting to a mqtt broker
  client.setServer(mqtt_broker, mqtt_port);
  client.setCallback(callback);
  while (!client.connected()) {
    String client_id = "esp32-client-";
    client_id += String(WiFi.macAddress());
    Serial.printf("The client %s connects to the public MQTT broker\n", client_id.c_str());
    if (client.connect(client_id.c_str(), mqtt_username, mqtt_password)) {
      Serial.println("Public EMQX MQTT broker connected");
    } else {
      Serial.print("failed with state ");
      Serial.print(client.state());
      delay(2000);
    }
  }
  //Topics to Subscribe 
  client.subscribe(topic1);
}


void loop() {
  client.loop();
  //Serial.println("p");
  unsigned long MQTT_currentMillis = millis();
  if (MQTT_currentMillis - MQTT_previousMillis >= MQTT_interval) {
    MQTT_previousMillis = MQTT_currentMillis;
    Serial.println("H");
    MQTT_Send(topic1, MQTT_Msg);
  }
}


void MQTT_Send(const char *topic, message msg) {
  /*
  char data[sizeof(msg)];
  memcpy(data, &msg, sizeof(msg));
  // Publish and subscribe
  client.publish(topic, (uint8_t *)data, sizeof(data), true);  // Retain message (optional)
  client.subscribe(topic);
  */

  StaticJsonDocument<200> doc;  //create json document
  doc["Vehicle_ID"] = msg.Vehicle_ID;
  doc["Long"] = msg.Long;
  doc["Lat"] = msg.Lat;
  doc["Speed"] = msg.Speed;
  doc["Heading"] = msg.Heading;
  doc["Vehicle_Type"] = msg.Vehicle_Type;
  doc["Event"] = msg.Event;
  doc["Flag"] = msg.Flag;

  char jsonBuffer[200];
  serializeJson(doc, jsonBuffer);  // Serialize JSON to a char array

  // Publish and subscribe
  client.publish(topic, jsonBuffer);
  Serial.printf("Message published at topic: %s\n", topic);

}



void callback(char *topic, byte *payload, unsigned int length) {

  Serial.printf("Message arrived in topic: %s\n", topic);

  StaticJsonDocument<200> doc;
  Serial.println("AA");
  DeserializationError error = deserializeJson(doc, payload);
  Serial.println("BB");

  if (error) {
    Serial.print(F("deserializeJson() failed: "));
    Serial.println(error.f_str());
    return;
  }
  
  Serial.printf("Available heap before parsing: %u\n", ESP.getFreeHeap()); // Check memory

  // Populate MQTT_Msg2 from JSON
  MQTT_Msg2.Vehicle_ID = doc["Vehicle_ID"];
  MQTT_Msg2.Long = doc["Long"];
  MQTT_Msg2.Lat = doc["Lat"];
  MQTT_Msg2.Speed = doc["Speed"];
  MQTT_Msg2.Heading = doc["Heading"];
  strlcpy(MQTT_Msg2.Vehicle_Type, doc["Vehicle_Type"], sizeof(MQTT_Msg2.Vehicle_Type));
  strlcpy(MQTT_Msg2.Event, doc["Event"], sizeof(MQTT_Msg2.Event));
  MQTT_Msg2.Flag = doc["Flag"];

  Serial.println("DD");


  // Print MQTT_Msg2 in your desired format
  Serial.print("Bytes received: ");
  Serial.print(length);

  Serial.print(", Vehicle ID: ");
  Serial.print(MQTT_Msg2.Vehicle_ID);

  Serial.print(", Vehicle Type: ");
  Serial.print(MQTT_Msg2.Vehicle_Type);

  Serial.print(", Event: ");
  Serial.print(MQTT_Msg2.Event);

  Serial.print(", Heading: ");
  Serial.println(MQTT_Msg2.Heading);

  Serial.print("Speed: ");
  Serial.print(MQTT_Msg2.Speed);

  Serial.print(", Longitude: ");
  Serial.print(MQTT_Msg2.Long, 7);

  Serial.print(", Latitude: ");
  Serial.print(MQTT_Msg2.Lat, 7);

  Serial.print(", Flag: ");
  Serial.println(MQTT_Msg2.Flag);
  Serial.println("-----------------------");
  /*

  Serial.print("Message arrived in topic: ");
  Serial.println(topic);
  Serial.print("Message:");
  for (int i = 0; i < length; i++) {
    Serial.print((char)payload[i]);
  }
  Serial.println();
  memcpy(&MQTT_Msg, payload, sizeof(MQTT_Msg));
  Serial.print("Bytes received: ");
  Serial.print(length);

  Serial.print(", Vehicle ID: ");
  Serial.print(MQTT_Msg.Vehicle_ID);

  Serial.print(", Vehicle Type: ");
  Serial.print(MQTT_Msg.Vehicle_Type);

  Serial.print(", Event: ");
  Serial.print(MQTT_Msg.Event);

  Serial.print(", Heading: ");
  Serial.println(MQTT_Msg.Heading);

  Serial.print("Speed: ");
  Serial.print(MQTT_Msg.Speed);

  Serial.print(", Longitude: ");
  Serial.print(MQTT_Msg.Long, 7);

  Serial.print(", Latitude: ");
  Serial.print(MQTT_Msg.Lat, 7);

  Serial.print(", Flag: ");
  Serial.println(MQTT_Msg.Flag);
  Serial.println("-----------------------");
  */
}