#include <Wire.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;

WiFiClient clienteWiFi;
PubSubClient mqtt(clienteWiFi);

const int ledVerde = 4;
const int ledAmarelo = 5;
const int ledVermelho = 6;

float referenciaX = 0;
float referenciaY = 0;

String situacaoAnterior = "";

const char* topicoMQTT =
  "atlasgeoasses/piloto/marco001/status";

void conectarMQTT()
{
  Serial.print("Conectando ao servidor MQTT");

  while (!mqtt.connected())
  {
    String identificador = "atlas-marco-001-";
    identificador += String(random(0xffff), HEX);

    if (mqtt.connect(identificador.c_str()))
    {
      Serial.println();
      Serial.println("MQTT conectado!");

      mqtt.publish(
        topicoMQTT,
        "MARCO 001 CONECTADO"
      );
    }
    else
    {
      Serial.print(".");
      delay(1000);
    }
  }
}

void setup()
{
  Serial.begin(115200);

  // Conecta ao Wi-Fi virtual do Wokwi
  Serial.print("Conectando ao Wi-Fi");

  WiFi.begin("Wokwi-GUEST", "");

  while (WiFi.status() != WL_CONNECTED)
  {
    delay(500);
    Serial.print(".");
  }

  Serial.println();
  Serial.println("Wi-Fi conectado!");

  Serial.print("Endereco IP: ");
  Serial.println(WiFi.localIP());

  // Configura e conecta ao MQTT
  mqtt.setServer("broker.hivemq.com", 1883);
  conectarMQTT();

  // Comunicação com o MPU6050
  Wire.begin(8, 9);

  // Configuração dos LEDs
  pinMode(ledVerde, OUTPUT);
  pinMode(ledAmarelo, OUTPUT);
  pinMode(ledVermelho, OUTPUT);

  digitalWrite(ledVerde, LOW);
  digitalWrite(ledAmarelo, LOW);
  digitalWrite(ledVermelho, LOW);

  Serial.println("ATLAS GEOASSES");
  Serial.println("Procurando MPU-6050...");

  if (!mpu.begin(0x68, &Wire))
  {
    Serial.println("ERRO: MPU-6050 nao encontrado!");

    while (true)
    {
      delay(1000);
    }
  }

  Serial.println("MPU-6050 encontrado!");

  mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  // Calibração inicial
  Serial.println("Calibrando posicao inicial...");
  delay(2000);

  sensors_event_t aceleracao;
  sensors_event_t giroscopio;
  sensors_event_t temperatura;

  mpu.getEvent(
    &aceleracao,
    &giroscopio,
    &temperatura
  );

  float ax = aceleracao.acceleration.x;
  float ay = aceleracao.acceleration.y;
  float az = aceleracao.acceleration.z;

  referenciaX =
    atan2(
      ay,
      sqrt(ax * ax + az * az)
    ) * 180.0 / PI;

  referenciaY =
    atan2(
      -ax,
      sqrt(ay * ay + az * az)
    ) * 180.0 / PI;

  Serial.println("Calibracao concluida!");
}

void loop()
{
  // Reconecta ao MQTT se necessário
  if (!mqtt.connected())
  {
    conectarMQTT();
  }

  mqtt.loop();

  // Faz a leitura do MPU6050
  sensors_event_t aceleracao;
  sensors_event_t giroscopio;
  sensors_event_t temperatura;

  mpu.getEvent(
    &aceleracao,
    &giroscopio,
    &temperatura
  );

  float ax = aceleracao.acceleration.x;
  float ay = aceleracao.acceleration.y;
  float az = aceleracao.acceleration.z;

  // Calcula os ângulos
  float anguloX =
    atan2(
      ay,
      sqrt(ax * ax + az * az)
    ) * 180.0 / PI;

  float anguloY =
    atan2(
      -ax,
      sqrt(ay * ay + az * az)
    ) * 180.0 / PI;

  float variacaoX = anguloX - referenciaX;
  float variacaoY = anguloY - referenciaY;

  float maiorVariacao =
    max(
      fabs(variacaoX),
      fabs(variacaoY)
    );

  // Desliga todos os LEDs
  digitalWrite(ledVerde, LOW);
  digitalWrite(ledAmarelo, LOW);
  digitalWrite(ledVermelho, LOW);

  String situacaoAtual;

  // Define a situação
  if (maiorVariacao <= 0.50)
  {
    digitalWrite(ledVerde, HIGH);
    situacaoAtual = "NORMAL";
  }
  else if (maiorVariacao <= 3.00)
  {
    digitalWrite(ledAmarelo, HIGH);
    situacaoAtual = "ATENCAO";
  }
  else
  {
    digitalWrite(ledVermelho, HIGH);
    situacaoAtual = "ALERTA";
  }

  // Exibe os dados no Monitor Serial
  Serial.print("X: ");
  Serial.print(variacaoX, 2);

  Serial.print(" graus | Y: ");
  Serial.print(variacaoY, 2);

  Serial.print(" graus | Situacao: ");
  Serial.println(situacaoAtual);

  // Envia ao computador quando a situação mudar
  if (
    situacaoAtual != situacaoAnterior &&
    mqtt.connected()
  )
  {
    char mensagem[150];

    snprintf(
      mensagem,
      sizeof(mensagem),
      "{\"marco\":\"001\",\"x\":%.2f,"
      "\"y\":%.2f,\"situacao\":\"%s\"}",
      variacaoX,
      variacaoY,
      situacaoAtual.c_str()
    );

    mqtt.publish(
      topicoMQTT,
      mensagem
    );

    Serial.print("Enviado ao computador: ");
    Serial.println(mensagem);

    situacaoAnterior = situacaoAtual;
  }

  delay(500);
}