#include <WiFi.h>
#include <PubSubClient.h>
//#include "config.h"
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <string>
#include <ArduinoJson.h>

// WiFi Credentials
const char *ssid = "EUSKALTEL_Ajo";            // Replace with your WiFi name
const char *password = "943783646";  // Replace with your WiFi password

// MQTT Broker Settings
const char *mqtt_broker = "g11bcbb1.emqx.cloud";
const char *mqtt_topic = "esp-json-info";

const char *mqtt_username = "Angelito";
const char *mqtt_password = "Angelito_PSW";
const int mqtt_port = 1883;

// Sensor variables
// Declaración del sensor
Adafruit_MPU6050 mpu;
// Aquí declaramos los valores de aceleración, giro y temperatura
sensors_event_t accel, gyro, temp;

WiFiClient espClient;
PubSubClient mqtt_client(espClient);
unsigned long lastMillis = 0;

// Function Declarations
void connectToWiFi();

void connectToMQTT();

void mqttCallback(char *mqtt_topic, byte *payload, unsigned int length);

void send();

void connectToMPU();

void getAcc(double &_Acc_X, double &_Acc_Y, double &_Acc_Z);

double getTemp();

void setup() {
    Serial.begin(115200);
    connectToWiFi();
    mqtt_client.setServer(mqtt_broker, mqtt_port);
    mqtt_client.setKeepAlive(60);
    mqtt_client.setCallback(mqttCallback); // Corrected callback function name
    connectToMQTT();
    connectToMPU();
}

void connectToWiFi() {
    WiFi.begin(ssid, password);
    Serial.print("Connecting to WiFi");
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }
    Serial.println("\nConnected to WiFi!!");
}

void connectToMQTT() {
    while (!mqtt_client.connected()) {
        String client_id = "esp32-client-" + String(WiFi.macAddress());
        Serial.printf("Connecting to MQTT Broker as %s.....\n", client_id.c_str());
        if (mqtt_client.connect(client_id.c_str(), mqtt_username, mqtt_password)) {
            Serial.println("Connected to MQTT broker!!");
            mqtt_client.subscribe(mqtt_topic);
            //mqtt_client.publish(mqtt_topic, "Calentitos"); // Publish message upon successful connection
        } else {
            Serial.print("Failed, rc=");
            Serial.print(mqtt_client.state());
            Serial.println(" try again in 5 seconds");
            delay(5000);
        }
    }
}

void mqttCallback(char *mqtt_topic, byte *payload, unsigned int length) {
    Serial.print("Topic: ");
    Serial.println(mqtt_topic);
    Serial.print("Message: ");
    for (unsigned int i = 0; i < length; i++) {
        Serial.print((char) payload[i]);
    }
    Serial.println("\n-----------------------");
}

String createJSON(){
  // Allocate the JSON document
  JsonDocument doc;
  static unsigned cont = 1;
  // Add values in the document
  doc["ID"] = cont;
  doc["sensor"] = "MPU6050";
  doc["time"] = millis();
  doc["temperature"] = getTemp();
  // ID increment
  cont++;

  double X, Y, Z;
  getAcc(X, Y, Z);

  // Add acceleration array
  // JsonArray data = doc["acceleration"].to<JsonArray>();
  // data.add(X);
  // data.add(Y);
  // data.add(Z);
  doc["accelerationX"] = X;
  doc["accelerationY"] = Y;
  doc["accelerationZ"] = Z;


  //Json Output
  String output;
  //serializeJsonPretty(doc, Serial);
  //serializeJsonPretty(doc, output);
  serializeJson(doc, output);
  return output;

}

void send() {
  mqtt_client.publish(mqtt_topic, createJSON().c_str()); 
  delay(100);
}

void connectToMPU() {
  // Espera para la conexión con el MPU
  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 Ready!");
}

// Obtención de aceleración del MPU
void getAcc(double &_Acc_X, double &_Acc_Y, double &_Acc_Z){
  mpu.getAccelerometerSensor()->getEvent(&accel);
  _Acc_X = accel.acceleration.x;
  _Acc_Y = accel.acceleration.y;
  _Acc_Z = accel.acceleration.z;
}

// Obtención de temperatura del MPU
double getTemp(){
  mpu.getEvent(&accel, &gyro, &temp);
  return temp.temperature;
}

void loop() {
    if (!mqtt_client.connected()) {
        connectToMQTT();
    }

    if (millis() - lastMillis > 5000) {
      lastMillis = millis();
      send();
    }    

    mqtt_client.loop();
}