#include <WiFi.h>
#include "Adafruit_MQTT.h"
#include "Adafruit_MQTT_Client.h"
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <MPU6050_light.h>

#define SSID "Wokwi-GUEST"
#define PASS ""

#define SERVIDOR_MQTT   "io.adafruit.com"
#define PUERTO          1883
#define USUARIO         "DAVID_ALONSO"
#define KEY             "aio_bWoY19Y07BvyzuxNNdc3Mx8BIxUB"

const int AD0pin = 15;


// Create objects. Each sensor is on the "Wire" I2C bus.
//MPU6050 mpu[NUM_MPUS] = 
float x_raw, y_raw, z_raw, t_raw = 0;
unsigned long tempo = 0;
int contador = 0;
//


#define MPU6050_ADDRESS 0x68
Adafruit_MPU6050 mpu;


//}
void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);  // SDA en pin 21, SCL en pin 22 del
  Serial.println("Iniciando...");
    while (!Serial) {
    delay(10);
  }
  if (!mpu.begin(MPU6050_ADDRESS)) {
    Serial.println("No se pudo encontrar el MPU-6050. Verifica las conexiones.");
    while (1);
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.println("MPU-6050 inicializado correctamente.");
  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  // Connect to WiFi access point.
  Serial.print("Conectando a ");
  Serial.println(SSID);

  WiFi.begin(SSID, PASS);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
    if(contador > 10){
      ESP.restart();
    }
    contador++;
  }
  Serial.println(); Serial.println("Conectado a WIFI");
  Serial.println("IP address: "); Serial.println(WiFi.localIP());
  tempo = millis() + 60000;
}
//void loop() {
  //sensors_event_t a, g, temp;
 // mpu.getEvent(&a, &g, &temp);
 // Serial.print("Acelerómetro (m/s^2): ");
 // Serial.print(a.acceleration.x);
 // Serial.print(", ");
 // Serial.print(a.acceleration.y);
 // Serial.print(", ");
 // Serial.println(a.acceleration.z);
 // Serial.print("Giroscopio (grados/s): ");
 // Serial.print(g.gyro.x);
  //Serial.print(", ");
 // Serial.print(g.gyro.y);
  //Serial.print(", ");
 // Serial.println(g.gyro.z);
  //Serial.println(temp.temperature);
 // delay(1000);
//}

//
WiFiClient client;


Adafruit_MQTT_Client mqtt(&client, SERVIDOR_MQTT, PUERTO, USUARIO, USUARIO, KEY);

Adafruit_MQTT_Publish x = Adafruit_MQTT_Publish(&mqtt, USUARIO "/feeds/x");
Adafruit_MQTT_Publish y = Adafruit_MQTT_Publish(&mqtt, USUARIO "/feeds/y");
Adafruit_MQTT_Publish z = Adafruit_MQTT_Publish(&mqtt, USUARIO "/feeds/z");
Adafruit_MQTT_Publish t = Adafruit_MQTT_Publish(&mqtt, USUARIO "/feeds/t");

void MQTT_connect() {

  int8_t ret;
  // Stop if already connected.
  if (mqtt.connected()) {
    return;
  }
  Serial.print("Conectando al servidor MQTT... ");
  uint8_t retries = 0;
  while ((ret = mqtt.connect()) != 0) {
    Serial.println(mqtt.connectErrorString(ret));
    Serial.println("Reintentando conexion en 10 segundo...");
    mqtt.disconnect();
    delay(10000);
    retries++;
    Serial.printf("Intentos %s\n", retries);
    if(retries > 6){
      ESP.restart();
    }
  }
  Serial.println("MQTT Connected!");
}



void loop() {
  MQTT_connect();
  mqtt.processPackets(10000);

  if (! mqtt.ping()) {
    mqtt.disconnect();
  }

  if (millis() - tempo > 1000) {
    tempo = millis();
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    // x_raw = a.acceleration.x / 9.8066;
    // y_raw = a.acceleration.y / 9.8066;
    // z_raw = a.acceleration.z / 9.8066;
    // t_raw = temp.temperature;
    x_raw = a.acceleration.x;
    y_raw = a.acceleration.y;
    z_raw = a.acceleration.z;
    t_raw = temp.temperature;
    char str_x[10], str_y[10], str_z[10], str_t[10];
    sprintf(str_x, "%.3f", x_raw);
    sprintf(str_y, "%.3f", y_raw);
    sprintf(str_z, "%.3f", z_raw);
    sprintf(str_t, "%.3f", t_raw);
    x.publish(x_raw);
    y.publish(y_raw);
    z.publish(z_raw);
    t.publish(t_raw);
    Serial.println(x_raw);
    Serial.println(y_raw);
    Serial.println(z_raw);
    Serial.println(t_raw);
    Serial.println("-----------------------------");
  }
}