#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <WiFi.h>            // Biblioteca para ESP32. Se for usar ESP8266, use <ESP8266WiFi.h>
#include <PubSubClient.h>    // Biblioteca para MQTT
#include <ArduinoJson.h>     // Biblioteca para criar JSON

Adafruit_MPU6050 mpu;

// Variáveis para armazenar os ângulos anteriores
float previousPitch = 0.0;
float previousRoll = 0.0;

// Configurações Wi-Fi
const char* ssid = "NC Visitante PW";     // Substitua pelo seu SSID
const char* password = "@Nova1234!"; // Substitua pela sua senha

// Configurações do Broker MQTT
const char* mqttServer = "test.mosquitto.org"; // Broker MQTT
const int mqttPort = 1883;                    // Porta padrão do MQTT
WiFiClient espClient;
PubSubClient client(espClient);                // Cliente MQTT

// Função para conectar ao Wi-Fi
void setupWiFi() {
  Serial.print("Conectando-se à rede Wi-Fi");
  WiFi.begin(ssid, password);
  
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.print(".");
  }
  Serial.println("Conectado!");
  Serial.print("Endereço IP: ");
  Serial.println(WiFi.localIP());
}

// Função para conectar ao broker MQTT
void reconnect() {
  while (!client.connected()) {
    Serial.print("Conectando ao MQTT...");

    // Gerar um clientId único aleatório
    String clientId = "MPU6050Client-" + String(random(1000000, 9999999));

    // Conectar ao broker MQTT usando o clientId dinâmico
    if (client.connect(clientId.c_str())) {
      Serial.println("Conectado ao Broker MQTT");
    } else {
      Serial.print("Falha na conexão, rc=");
      Serial.print(client.state());
      Serial.println(" Tentando novamente em 5 segundos...");
      delay(5000);
    }
  }
}

void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10); // Aguarda a inicialização do Serial

  // Conectar ao Wi-Fi
  setupWiFi();

  // Configurar o cliente MQTT
  client.setServer(mqttServer, mqttPort);

  // Tentar inicializar o sensor MPU6050
  if (!mpu.begin()) {
    Serial.println("Falha ao encontrar o chip MPU6050");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 encontrado!");

  // Setup de detecção de movimento
  mpu.setHighPassFilter(MPU6050_HIGHPASS_0_63_HZ);
  mpu.setMotionDetectionThreshold(1);
  mpu.setMotionDetectionDuration(20);
  mpu.setInterruptPinLatch(true);  
  mpu.setInterruptPinPolarity(true);
  mpu.setMotionInterrupt(true);

  // Calibração inicial
  calibracao();

  // Conectar ao broker MQTT
  reconnect();
}

void loop() {
  // Reconnecta ao MQTT se desconectar
  if (!client.connected()) {
    reconnect();
  }
  client.loop(); // Processa mensagens MQTT

  // Detecta movimento e envia dados via MQTT
  detectar_movimento();
  delay(1000); // Aguarda 1 segundo antes de coletar novos dados
}

void detectar_movimento() {
  if (mpu.getMotionInterruptStatus()) {
    // Obter novos dados do sensor
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    // Calcular pitch e roll
    float pitch = atan(a.acceleration.x / sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180.0 / PI;
    float roll = atan(a.acceleration.y / sqrt(a.acceleration.x * a.acceleration.x + a.acceleration.z * a.acceleration.z)) * 180.0 / PI;

    // Calcular variações
    float pitchVariation = pitch - previousPitch;
    float rollVariation = roll - previousRoll;

    // Imprimir os valores na serial
    Serial.print("Pitch: ");
    Serial.print(pitch);
    Serial.print("°, ");
    Serial.print("Pitch Variation: ");
    Serial.print(pitchVariation);
    Serial.print("°, ");
    Serial.print("Roll: ");
    Serial.print(roll);
    Serial.print("°, ");
    Serial.print("Roll Variation: ");
    Serial.print(rollVariation);
    Serial.print("°, ");

    // Temperatura
    float temperature = temp.temperature;
    Serial.print("Temperature: ");
    Serial.print(temperature);
    Serial.println("°C");

    // Criar o JSON
    StaticJsonDocument<200> doc;
    doc["pitch"] = pitch;
    doc["roll"] = roll;
    doc["temperature"] = temperature;
    doc["pitchVariation"] = pitchVariation;
    doc["rollVariation"] = rollVariation;

    // Serializar o JSON em uma string
    char jsonBuffer[512];
    serializeJson(doc, jsonBuffer);

    // Enviar os dados via MQTT para o tópico "sensor/data"
    client.publish("nova_corrente/data", jsonBuffer);

    // Atualizar os valores anteriores
    previousPitch = pitch;
    previousRoll = roll;
  }
}

void calibracao() {
  // Calibração do sensor no início
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  // Calcular o pitch e roll no momento inicial
  float initialPitch = atan(a.acceleration.x / sqrt(a.acceleration.y * a.acceleration.y + a.acceleration.z * a.acceleration.z)) * 180.0 / PI;
  float initialRoll = atan(a.acceleration.y / sqrt(a.acceleration.x * a.acceleration.x + a.acceleration.z * a.acceleration.z)) * 180.0 / PI;

  // Definir como referência inicial
  previousPitch = initialPitch;
  previousRoll = initialRoll;
}