#include <WiFi.h>
#include <PubSubClient.h>
#include <Wire.h>
#include <VL53L0X.h>
#include <ESP32Servo.h>

// ====== WIFI & MQTT CONFIG ======
const char* ssid = "Oskar";
const char* password = "claritabonita";

const char* mqtt_server = "89.116.24.168";
const int mqtt_port = 31160;

const char* mqtt_user = "oskar";
const char* mqtt_password = "";

const char* topic_pub = "xiao/distancia";
// =================================

// ====== VL53L0X CONFIG ======
#define SDA_PIN 5
#define SCL_PIN 6

VL53L0X sensor;

const int DISTANCIA_DETECCION_MM = 1500;
// =============================

// ====== SERVO CONFIG ======
#define SERVO_PIN 2

Servo puertaServo;

const int PUERTA_CERRADA = 0;
const int PUERTA_ABIERTA = 90;

bool puertaAbierta = false;

unsigned long tiempoPuerta = 0;
const unsigned long tiempoAbierta = 5000; // 5 segundos
// ===========================

WiFiClient espClient;
PubSubClient client(espClient);

unsigned long lastPublish = 0;
const unsigned long publishInterval = 1000;

void connectWiFi() {
  Serial.print("Conectando a WiFi: ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);

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

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

void reconnectMQTT() {
  while (!client.connected()) {
    Serial.print("Conectando a MQTT... ");

    if (client.connect("XIAO_ESP32S3_TOF", mqtt_user, mqtt_password)) {
      Serial.println("conectado");
    } else {
      Serial.print("fallo, rc=");
      Serial.print(client.state());
      Serial.println(" reintentando en 5 segundos");
      delay(5000);
    }
  }
}

// ====== ABRIR PUERTA ======
void abrirPuerta() {

  Serial.println("Abriendo compuerta");

  for (int angulo = PUERTA_CERRADA; angulo <= PUERTA_ABIERTA; angulo++) {
    puertaServo.write(angulo);
    delay(10);
  }

  puertaAbierta = true;
  tiempoPuerta = millis();
}

// ====== CERRAR PUERTA ======
void cerrarPuerta() {

  Serial.println("Cerrando compuerta");

  for (int angulo = PUERTA_ABIERTA; angulo >= PUERTA_CERRADA; angulo--) {
    puertaServo.write(angulo);
    delay(10);
  }

  puertaAbierta = false;
}

void setup() {

  Serial.begin(115200);
  delay(1000);

  connectWiFi();

  client.setServer(mqtt_server, mqtt_port);

  // ===== I2C =====
  Wire.begin(SDA_PIN, SCL_PIN);

  sensor.setTimeout(500);

  if (!sensor.init()) {
    Serial.println("ERROR: No se detecta el VL53L0X");
    while (1);
  }

  sensor.startContinuous();

  Serial.println("VL53L0X iniciado correctamente");

  // ===== SERVO =====
  puertaServo.setPeriodHertz(50);

  puertaServo.attach(SERVO_PIN, 500, 2500);

  puertaServo.write(PUERTA_CERRADA);

  Serial.println("Servo iniciado");
}

void loop() {

  if (!client.connected()) {
    reconnectMQTT();
  }

  client.loop();

  int distancia = sensor.readRangeContinuousMillimeters();

  if (sensor.timeoutOccurred()) {
    Serial.println("Timeout del sensor");
    return;
  }

  Serial.print("Distancia: ");
  Serial.print(distancia);
  Serial.println(" mm");

  // ===== DETECCION =====
  if (distancia > 0 && distancia <= DISTANCIA_DETECCION_MM) {

    // MQTT
    if (millis() - lastPublish >= publishInterval) {

      lastPublish = millis();

      String mensaje = "{";
      mensaje += "\"deteccion\":true,";
      mensaje += "\"distancia_mm\":";
      mensaje += distancia;
      mensaje += ",\"distancia_m\":";
      mensaje += String(distancia / 1000.0, 2);
      mensaje += "}";

      client.publish(topic_pub, mensaje.c_str());

      Serial.print("MQTT enviado: ");
      Serial.println(mensaje);
    }

    // ABRIR PUERTA SOLO SI ESTA CERRADA
    if (!puertaAbierta) {
      abrirPuerta();
    }
  }

  // ===== CIERRE AUTOMATICO =====
  if (puertaAbierta && (millis() - tiempoPuerta >= tiempoAbierta)) {
    cerrarPuerta();
  }

  delay(1000);
}