//--------------------------------------------------------------------------------------------------------
// INCLUDES NECESARIOS

#include <Arduino.h>
#include "Programas.h"
#include <WiFi.h>
#include <PubSubClient.h>



//--------------------------------------------------------------------------------------------------------
// COMUNICACIÓN MQTT

// Configuración WiFi
const char* ssid = "Wokwi-GUEST"; //"UPV-PSK";       // Nombre de la red WiFi.
const char* password = "";        //"giirob-pr2-2023"; // Contraseña de la red WiFi.


// Configuración MQTT
const char* clientId = "Deslizador";            // Identificador único para el cliente MQTT
const char* mqtt_server = "broker.mqttdashboard.com"; //"broker.emqx.io"; // Dirección del servidor MQTT
const char *topicCruce = "Grupo_B_1/cruce/permiso";       // Tópico donde el robot recibe permisos de cruce
const char *topicGloables = "Grupo_B_1/globales/permiso"; // Tópico donde el robot recibe permisos de cruce
const char *topicEstado = "Grupo_B_1/estado/delizador";   // Tópico se publica el estado del deslizador
const int mqtt_port = 1883;                           // Puerto del servidor MQTT


// Creación de clientes WiFi y MQTT
WiFiClient espClient;           // Cliente WiFi para conectar al servidor MQTT
PubSubClient client(espClient); // Cliente MQTT que usa el cliente WiFi para enviar y recibir mensajes



//--------------------------------------------------------------------------------------------------------
// DEFINICIÓN DE LOS PINES DE ENTRADA/SALIDA 

// Configuración de pines para el control del motor y sensor PIR
const int pinENA = 14; // Pin PWM del motor izquierdo.
const int pinIN1 = 26; // Pin para controlar la dirección hacia atrás del motor izquierdo.
const int pinIN2 = 25; // Pin para controlar la dirección hacia adelante del motor izquierdo.
const int pinENB = 27; // Pin PWM del motor derecho.
const int pinIN3 = 33; // Pin para controlar la dirección hacia atrás del motor derecho.
const int pinIN4 = 32; // Pin para controlar la dirección hacia adelante del motor derecho.
const int pinPIR = 34; // Pin conectado al sensor PIR para detectar objetos en el camino.
const int pinTrig = 12; // Pin para la señal Trig del sensor ultrasónico
const int pinEcho = 13; // Pin para la señal Echo del sensor ultrasónico

const int pinLED = 2; // LED para indicar movimiento


//--------------------------------------------------------------------------------------------------------
// CONFIGURACIÓN DEL CONTROL DE VELOCIDAD PWM

// Configuración de PWM para los motores
const int freq = 3000;           // Frecuencia de la señal PWM en Hz.
const int pwmChannelA = 0;       // Canal de PWM para el motor izquierdo.
const int pwmChannelB = 1;       // Canal de PWM para el motor derecho.
const int resolution = 8;        // Resolución de la señal PWM en bits (0-255).
const int velocidadNormal = 200; // Velocidad normal de los motores (ciclo de trabajo del PWM).
const int velocidadGiro = 150;   // Velocidad de giro del deslizador (usada en maniobras).



//--------------------------------------------------------------------------------------------------------
//  VARIABLES GLOBALES

// Variables de estado
bool permisoCruce = false; // Indica si el robot tiene permiso para cruzar el túnel.
bool activarMover = true;  // Bandera para activar el movimiento inicial del robot.

// Variable para almacenar la distancia detectarda por el senosr de ultrasonidos
float distanciaMedida = 0;  // Distancia medida por el sensor ultrasónico

// Variable para almacenar el último mensaje recibido por MQTT
String mensajeRecibido = "";

// Variable global para almacenar el estado anterior del sensor PIR
bool estadoAnteriorPIR = false;



//--------------------------------------------------------------------------------------------------------
// SETUP

// Configuración Inicial
void setup() {
  Serial.begin(115200); // Inicia la comunicación serial.

  // Configuración de los pines como entradas/salidas
  pinMode(pinENA, OUTPUT);
  pinMode(pinIN1, OUTPUT);
  pinMode(pinIN2, OUTPUT);
  pinMode(pinENB, OUTPUT);
  pinMode(pinIN3, OUTPUT);
  pinMode(pinIN4, OUTPUT);
  pinMode(pinPIR, INPUT);
  pinMode(pinTrig, OUTPUT);
  pinMode(pinEcho, INPUT);

  pinMode(pinLED, OUTPUT); // Configuración del pin del LED como salida.


  // Configuración del PWM en los pines para controlar la velocidad de los motores
  ledcAttachChannel(pinENA, freq, resolution, pwmChannelA);
  ledcAttachChannel(pinENB, freq, resolution, pwmChannelB);

  // Si no funciona la función "ledcAttachChannel" utilizaremos:
  /*
  ledcSetup(pwmChannelA, freq, resolution);
  ledcAttachPin(pinENA, pwmChannelA);
  
  ledcSetup(pwmChannelB, freq, resolution;
  ledcAttachPin(pinENB, pwmChannelB);
  */

  // Inicialización segura: todos los motores apagados y en reposo
  detencionInmediata();

  // Conexión a la red WiFi.
  conectarWiFi(ssid, password); 

  // Configuración del servidor MQTT.
  client.setServer(mqtt_server, mqtt_port); // Define el servidor MQTT
  reconnectMQTT(client, clientId);          // Intenta una reconexión inicial al servidor MQTT
  
  // Suscripción al tópico MQTT que gestiona los permisos de cruce
  client.setCallback(dispatcherCallback);  // Usa dispatcherCallback como callback principal
  client.subscribe(topicCruce);            // Se suscribe al tópico donde recibe los permisos de cruce
  client.subscribe(topicGloables);         // Se suscribe al tópico donde recibe los mensajes globales 
}



//--------------------------------------------------------------------------------------------------------
// LOOP

// Bucle Principal
void loop() {
  // Verifica si el dispositivo está conectado a la red WiFi
  if (WiFi.status() != WL_CONNECTED) {
    conectarWiFi(ssid, password); // Si no está conectado, intenta reconectarse
  }
  
  // Verifica si el cliente MQTT sigue conectado
  if (!client.connected()) {
    reconnectMQTT(client, clientId); // Si no está conectado, intenta reconectarse
  }
  client.loop(); // Mantiene la conexión MQTT activa (maneja mensajes entrantes y salientes)


  // Activa el movimiento inicial del deslizador si está permitido.
  String mensajeRecibido = suscriptorMQTT();
  if(mensajeRecibido == "Marcha" && activarMover){
    publicadorMQTT(client, topicEstado, "Deslizador en movimiento");
    Serial.println("Deslizador en movimiento");
    Serial.println("");
    
    moverAdelante();       // Inicia el movimiento hacia adelante.
    permisoCruce = false;  // Resetea el estado del permiso de cruce.
    activarMover = false;  // Resetea el estado del permiso para activar el deslizador.
    mensajeRecibido = ""; // Limpia el mensaje recibido

    digitalWrite(pinLED, HIGH);
  }
  

  // Detecta si hay un flanco de subida usando la función "detectarObjeto"
  bool estadoActualPIR = detectarObjeto(pinPIR);
  if (estadoActualPIR && !estadoAnteriorPIR) {
    publicadorMQTT(client, topicEstado, "Túnel detectado. Parada controlada");
    Serial.println("Túnel detectado. Deteniendo deslizador...");
    Serial.println("");
    detencionControlada(); // Detiene el robot gradualmente.

    digitalWrite(pinLED, LOW);

    // Espera permiso para cruzar el túnel o reinicia tras 20 segundos.
    unsigned long startWait = millis(); // Marca de tiempo de inicio.
    while (!permisoCruce && WiFi.status() == WL_CONNECTED) { // Repite hasta recibir permiso o perder la conexión WiFi.
      if (millis() - startWait >= 20000) { // Si pasan más de 20 segundos sin permiso:
        Serial.println("Timeout alcanzado. Reiniciando...");
        ESP.restart(); // Reinicia el ESP32 para restablecer conectividad y estado.
      }
      client.loop(); // Mantiene activa la conexión MQTT y procesa mensajes.
      delay(10);     // Pequeña pausa para no sobrecargar el sistema.
    }

    // Si se recibió el permiso, reanuda el movimiento.
    if (permisoCruce) {
      publicadorMQTT(client, topicEstado, "Permiso otorgado. Reanudando movimiento");
      Serial.println("Permiso otorgado. Reanudando movimiento...");
      Serial.println("");

      moverAdelante();  // Permite que el robot vuelva a moverse.
      permisoCruce = false; // Restablece la variable para futuras detecciones.
      activarMover = true;  // Resetea el estado del permiso para activar el deslizador.

      digitalWrite(pinLED, HIGH);

    } 
    else 
    {
      // Caso opcional: Podría incluir lógica adicional si el bucle terminó por otro motivo.
      Serial.println("Error inesperado o pérdida de conexión WiFi.");
      Serial.println("");
    }
  }


  // Detecta objetos con el sensor ultrasónico en el cruce
  float distancia = medirDistancia();
  if (distancia > 0 && distancia < 5.0 && activarMover) { // Parada de emergencia
    publicadorMQTT(client, topicEstado, "Objeto detectado. Parada de emergencia");
    Serial.println("Objeto detectado en el cruce. Deteniendo de emergencia...");
    Serial.println("");

    detencionInmediata();
    activarMover = false;  // Resetea el estado del permiso para activar el deslizador.

    digitalWrite(pinLED, LOW);
  } 
  else 
  {
    activarMover = true;  // Resetea el estado del permiso para activar el deslizador.
  }


  // Actualiza el estado anterior del sensor PIR
  estadoAnteriorPIR = estadoActualPIR;
  
  // Pausa para evitar saturación del microcontrolador
  delay(100); 
}