#include <DHT.h> // Include the DHT library for interfacing with the DHT sensor
#include <WiFi.h> // Include the WiFi library for connecting to a WiFi network
#include <PubSubClient.h> // Include the PubSubClient library for MQTT communication
#include <TinyGPS++.h> // Include the TinyGPS++ library for parsing GPS data
#include <Wire.h> // Include the Wire library for I2C communication

// Pin connected to the DHT sensor
#define DHTPIN 15 // Define the pin connected to the DHT sensor - ADC (Analog-to-Digital Converter) 
#define DHTTYPE DHT22 // Define the type of DHT sensor

// WiFi credentials
const char* ssid = "Wokwi-GUEST"; // Define the WiFi SSID
const char* password = ""; // Define the WiFi password

// Ubidots credentials
const char* mqtt_server = "industrial.api.ubidots.com"; // Define the Ubidots MQTT server address
const char* mqtt_client_name = "ESP32Client"; // Define the MQTT client name
const char* token = "BBUS-rPZvJD7yVejDLA3bfWFOebUbQ4MTiF"; // Define the Ubidots token
const char* device_label = "smart-shoes"; // Define the device label
const char* variable_label_temp = "temperature"; // Define the temperature variable label
const char* variable_label_humi = "humidity"; // Define the humidity variable label
const char* variable_label_pressure = "pressure"; // Define the pressure variable label
const char* variable_label_latitude = "latitude"; // Define the latitude variable label
const char* variable_label_longitude = "longitude"; // Define the longitude variable label
const char* variable_label_altitude = "altitude"; // Define the altitude variable label
const char* variable_label_speed = "speed"; // Define the speed variable label
const char* variable_label_heart_rate = "heart_rate"; // Define the heart rate variable label
const char* variable_label_gait = "gait"; // Define the gait variable label

// DHT sensor instance
DHT dht(DHTPIN, DHTTYPE); // Create an instance of the DHT sensor

// WiFi and MQTT clients
WiFiClient espClient; // Create a WiFi client
PubSubClient client(espClient); // Create a MQTT client using the WiFi client

// Pressure simulation variables
int pressurePin = 13; // Define the pin connected to the pressure sensor
int potPin = 34; // Define the pin connected to the potentiometer
int pressureValue; // Declare a variable to store pressure value
int potValue; // Declare a variable to store potentiometer value

enum GaitType { STOPPED, WALKING, RUNNING }; // Enumerate the gait types
GaitType currentGait = STOPPED; // Initialize the current gait type to stopped

unsigned long gaitStartTime; // Declare a variable to store the start time of the gait
unsigned long currentTime; // Declare a variable to store the current time
unsigned long walkDuration = 10000; // Define the duration of walking
unsigned long runDuration = 5000; // Define the duration of running

// GPS variables
TinyGPSPlus gps; // Create an instance of the TinyGPS++ library
HardwareSerial serialGPS(1); // Create a hardware serial instance for GPS communication
double latitude = 0.0; // Declare a variable to store latitude
double longitude = 0.0; // Declare a variable to store longitude
double altitude = 0.0; // Declare a variable to store altitude
double speed = 0.0; // Declare a variable to store speed
double prev_latitude = 0.0; // For distance calculation
double prev_longitude = 0.0; // For distance calculation

// Battery 
int batteryLevel = 100; // Initialize the battery level to full
const char* variable_label_battery = "battery"; // Define the battery variable label

// LED pins for indicating status
const int walkingLED = 2; // LED for walking status
const int runningLED = 4; // LED for running status
const int stoppedLED = 5; // LED for stopped status

// Distance initlization
double total_distance = 0.0; // Total distance traveled - in Kilometers
const char* variable_label_distance = "distance";

void setup_wifi() {
  delay(10); // Delay for stability
  Serial.println(); // Print a blank line
  Serial.print("Connecting to "); // Print connection message
  Serial.println(ssid); // Print SSID

  WiFi.begin(ssid, password); // Connect to WiFi network with SSID and password
  while (WiFi.status() != WL_CONNECTED) { // Wait until WiFi connection is established
    delay(500); // Delay for stability
    Serial.print("."); // Print connecting dots
  }
  Serial.println(""); // Print a blank line
  Serial.println("WiFi connected"); // Print WiFi connection status
  Serial.println("IP address: "); // Print IP address label
  Serial.println(WiFi.localIP()); // Print local IP address
}

void reconnect() {
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection..."); // Print MQTT connection attempt message
    if (client.connect(mqtt_client_name, token, "")) { // Attempt to connect to MQTT broker
      Serial.println("connected"); // Print connection success message
    } else {
      Serial.print("failed, rc="); // Print connection failure message
      Serial.print(client.state()); // Print MQTT client state
      Serial.println(" try again in 5 seconds"); // Print reconnection attempt message
      delay(5000); // Delay for reconnection attempt
    }
  }
}

void setup() {
  Serial.begin(115200); // Start serial communication at 115200 baud rate
  setup_wifi(); // Setup WiFi connection
  client.setServer(mqtt_server, 1883); // Set MQTT server and port
  dht.begin(); // Initialize DHT sensor
  pinMode(pressurePin, OUTPUT); // Initialize pressure sensor pin as output
  gaitStartTime = millis(); // Record start time for pressure simulation
  
  // GPS initialization
  serialGPS.begin(9600, SERIAL_8N1, 16, 17); // Initialize GPS serial communication

  // Initialize LED pins
  pinMode(walkingLED, OUTPUT);
  pinMode(runningLED, OUTPUT);
  pinMode(stoppedLED, OUTPUT);

  Serial.println(F("ESP32 - GPS module Simulation")); // Print ESP32 and GPS module simulation message
}

// Haversine formula to calcluate distance
double haversine(double lat1, double lon1, double lat2, double lon2) {
    // Earth radius in kilometers
    const double R = 6371.0;

    // Convert degrees to radians
    lat1 = radians(lat1);
    lon1 = radians(lat1);
    lat2 = radians(lat2);
    lon2 = radians(lon2);

    // Differences
    double dlat = lat2 - lat1;
    double dlon = lon2 - lon1;

    // Haversine formula
    double a = pow(sin(dlat / 2), 2) + cos(lat1) * cos(lat2) * pow(sin(dlon / 2), 2);
    double c = 2 * atan2(sqrt(a), sqrt(1 - a));

    // Distance in kilometers
    double distance = R * c;

    return distance;
}

void displayGPSData() {
  // Print latitude and longitude
  if (gps.location.isValid()) { // Check if GPS location
    Serial.print(F("Latitude: ")); // Print latitude label
    Serial.println(gps.location.lat(), 6); // Print latitude with 6 decimal places

    Serial.print(F("Longitude: ")); // Print longitude label
    Serial.println(gps.location.lng(), 6); // Print longitude with 6 decimal places

    // Publish latitude and longitude to Ubidots
    char payload_lat[50]; // Declare a character array for latitude payload
    sprintf(payload_lat, "{\"%s\": %.6f}", variable_label_latitude, gps.location.lat()); // Format latitude payload
    client.publish("/v1.6/devices/smart-shoes", payload_lat); // Publish latitude payload to Ubidots

    char payload_lon[50]; // Declare a character array for longitude payload
    sprintf(payload_lon, "{\"%s\": %.6f}", variable_label_longitude, gps.location.lng()); // Format longitude payload
    client.publish("/v1.6/devices/smart-shoes", payload_lon); // Publish longitude payload to Ubidots
  } else {
    Serial.println(F("Location: INVALID")); // Print invalid location message
  }

  // Print altitude
  if (gps.altitude.isValid()) { // Check if altitude is valid
    Serial.print(F("Altitude: ")); // Print altitude label
    Serial.println(gps.altitude.meters()); // Print altitude in meters

    // Publish altitude to Ubidots
    char payload_alt[50]; // Declare a character array for altitude payload
    sprintf(payload_alt, "{\"%s\": %.2f}", variable_label_altitude, gps.altitude.meters()); // Format altitude payload
    client.publish("/v1.6/devices/smart-shoes", payload_alt); // Publish altitude payload to Ubidots
  } else {
    Serial.println(F("Altitude: INVALID")); // Print invalid altitude message
  }

  // Print speed
  if (gps.speed.isValid()) { // Check if speed is valid
    Serial.print(F("Speed: ")); // Print speed label
    Serial.println(gps.speed.kmph()); // Print speed in kilometers per hour

    // Publish speed to Ubidots
    char payload_speed[50]; // Declare a character array for speed payload
    sprintf(payload_speed, "{\"%s\": %.2f}", variable_label_speed, gps.speed.kmph()); // Format speed payload
    client.publish("/v1.6/devices/smart-shoes", payload_speed); // Publish speed payload to Ubidots
  } else {
    Serial.println(F("Speed: INVALID")); // Print invalid speed message
  }

  if (gps.location.isValid()) {
    double distance = haversine(prev_latitude, prev_longitude, gps.location.lat(), gps.location.lng());

    // update distance, and previous values
    total_distance += distance;
    prev_latitude = gps.location.lat();
    prev_longitude = gps.location.lng();

    // Print total distance
    Serial.print(F("Total Distance: "));
    Serial.println(total_distance);

    // publish distance
    char payload_distance[50];
    sprintf(payload_distance, "{\"%s\": %.2f}", variable_label_distance, total_distance);
    client.publish("/v1.6/devices/smart-shoes", payload_distance);
  }

}

void loop() {
  if (!client.connected()) {
    reconnect(); // Reconnect to MQTT broker if not connected
  }
  client.loop(); // Maintain MQTT connection

  // Read temperature and humidity from DHT sensor
  float temperature = dht.readTemperature(); // Read temperature
  float humidity = dht.readHumidity(); // Read humidity

  // Check if any readings failed
  if (isnan(temperature) || isnan(humidity)) {
    Serial.println(F("Failed to read from DHT sensor!")); // Print DHT sensor failure message
    return;
  }

  // Publish temperature to Ubidots
  char payload_temp[50]; // Declare a character array for temperature payload
  sprintf(payload_temp, "{\"%s\": %.2f}", variable_label_temp, temperature); // Format temperature payload
  client.publish("/v1.6/devices/smart-shoes", payload_temp); // Publish temperature payload to Ubidots

  // Publish humidity to Ubidots
  char payload_humi[50]; // Declare a character array for humidity payload
  sprintf(payload_humi, "{\"%s\": %.2f}", variable_label_humi, humidity); // Format humidity payload
  client.publish("/v1.6/devices/smart-shoes", payload_humi); // Publish humidity payload to Ubidots

  // Simulate gait type based on time intervals
  currentTime = millis(); // Get the current time
  if (currentTime - gaitStartTime < walkDuration) {
    currentGait = WALKING; // Simulate walking state
  } else if (currentTime - gaitStartTime < walkDuration + runDuration) {
    currentGait = RUNNING; // Simulate running state
  } else {
    currentGait = STOPPED; // Simulate stopped state
    gaitStartTime = currentTime; // Reset gait start time for next simulation
  }

  // Display and publish gait type
  char payload_gait[50]; // Declare a character array for gait payload
  switch (currentGait) {
    case WALKING:
      Serial.println(F("Gait: Walking")); // Print walking state
      sprintf(payload_gait, "{\"%s\": \"walking\"}", variable_label_gait); // Format walking payload
      digitalWrite(walkingLED, HIGH);
      digitalWrite(runningLED, LOW);
      digitalWrite(stoppedLED, LOW);
      break;
    case RUNNING:
      Serial.println(F("Gait: Running")); // Print running state
      sprintf(payload_gait, "{\"%s\": \"running\"}", variable_label_gait); // Format running payload
      digitalWrite(walkingLED, LOW);
      digitalWrite(runningLED, HIGH);
      digitalWrite(stoppedLED, LOW);
      break;
    case STOPPED:
      Serial.println(F("Gait: Stopped")); // Print stopped state
      sprintf(payload_gait, "{\"%s\": \"stopped\"}", variable_label_gait); // Format stopped payload
      digitalWrite(walkingLED, LOW);
      digitalWrite(runningLED, LOW);
      digitalWrite(stoppedLED, HIGH);
      break;
  }
  client.publish("/v1.6/devices/smart-shoes", payload_gait); // Publish gait payload to Ubidots

  displayGPSData(); // Display and publish GPS data

  delay(2000); // Delay for 2 seconds before next loop
}