//--------------------------------------------------------------------------------------------------------
// INCLUDES NECESARIOS
#include <Arduino.h>
#include <WiFi.h>
#include <PubSubClient.h>


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

// WiFi Parameters
const char* WIFI_SSID = "Wokwi-GUEST";      //"UPV-PSK";          //Enter the router name
const char* WIFI_PASSWORD = "";             //"giirob-pr2-2023";  //Enter the router password
// MQTT Server Parameters
const char* MQTT_CLIENT_ID = "esp8266-weather-demo";
const char* MQTT_BROKER = "broker.mqttdashboard.com";   // "broker.emqx.io";
const char* MQTT_USER =  "";                //"emqx";
const char* MQTT_PASSWORD =  "";            //"public";
const char* MQTT_TOPIC_COMMANDS = "emqx/Grupo_B3/MQTT/RoboDK/soldadura/commands";  // Publico
const char* MQTT_TOPIC_STATUS = "emqx/Grupo_B3/MQTT/RoboDK/soldadura/status";      // Me suscribo
const int MQTT_PORT = 1883;

// Definimos parametros para conectarnos por MQTT
WiFiClient espClient;
PubSubClient client(espClient);


//--------------------------------------------------------------------------------------------------------
// DEFINICIÓN DE LOS PINES DE ENTRADA/SALIDA Y LOS ESTRUCTURA DE LOS BOTONES

// Definimos la utilización de los pines
#define PIN_BUTTON_EMERGERCIA 22
#define PIN_BUTTON_MARCHA 12
#define PIN_SENSOR 13
#define LED_MARCHA 14

#define PIN_BIT_0 2
#define PIN_BIT_1 0
#define PIN_BIT_2 19
#define PIN_BIT_3 21
#define PIN_BIT_4 23


// la función de configuración se ejecuta una vez cuando presionas restablecer o enciendes la placa
// Estructura del boton
struct Button {
  const uint8_t PIN;
  bool pressed;
};


// Variable para parar el sistema
static bool PARAR = false;
// Declaracion del boton de emergencia
Button Button_Emergencia = {PIN_BUTTON_EMERGERCIA, false};
void IRAM_ATTR isr_Emergencia() {
  Button_Emergencia.pressed = true;
  PARAR=true;
  digitalWrite(LED_MARCHA, LOW);
}


// Variable para saber si se ha pulsador el pulsador de marcha
static bool MARCHA = false;
// Variable para almacenar si ya se ha activado la marcha
static bool marcha_activada = false;
// Variable para que el pulsador no se pueda pulsar varia veces
static bool bloqueo_pulsador = false;
// Declaracion del boton de marcha
Button Button_Marcha = {PIN_BUTTON_MARCHA, false};
void IRAM_ATTR isr_Marcha() {
  Button_Marcha.pressed = true;
  MARCHA=true;
}


// Variable para que el sensor no se pueda pulsar varia veces
static bool bloqueo_sensor = false;
// Declaracion del sensor
Button Sensor = {PIN_SENSOR, false};
void IRAM_ATTR isr_Sensor() {
  Sensor.pressed = true;
}


//--------------------------------------------------------------------------------------------------------
// BIT DE CONTROL DEL SISTEMA

// Variable para controlar por donde se encuentra el sistema de la ESP32
static byte ESPByte = 0b00000000; // Inicializa la variable byte
// Variable para controlar por donde se encuentra el sistema de la estación
static byte sistemaByte = 0b00000000; 


//--------------------------------------------------------------------------------------------------------
// DEFINICIÓN DE FUNCIONES
void callback(char *topic, byte *payload, unsigned int length);  // Callback para cuando recibimos un mensaje
void reconnect();     // Función para la reconexion MQTT
void enviar_mensaje_mqtt(const char* mensaje);   // Función para enviar mensajes por MQTT


//--------------------------------------------------------------------------------------------------------
// SETUP Y LOOP

void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  // Inicializacion del LED
  pinMode(LED_MARCHA, OUTPUT);
  pinMode(PIN_BIT_0, OUTPUT);
  pinMode(PIN_BIT_1, OUTPUT);
  pinMode(PIN_BIT_2, OUTPUT);
  pinMode(PIN_BIT_3, OUTPUT);
  pinMode(PIN_BIT_4, OUTPUT);


  // Inicializacion del pulsador de emergencia
  pinMode(Button_Emergencia.PIN, INPUT);  // INPUT_PULLUP
  attachInterrupt(Button_Emergencia.PIN, isr_Emergencia, FALLING);
  // Inicializacion del pulsador de marcha
  pinMode(Button_Marcha.PIN, INPUT);      // INPUT_PULLUP
  attachInterrupt(Button_Marcha.PIN, isr_Marcha, FALLING);
  // Inicializacion del sensor
  pinMode(Sensor.PIN, INPUT);             // INPUT_PULLUP
  attachInterrupt(Sensor.PIN, isr_Sensor, FALLING);

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


  // Connecting to a WiFi network
  Serial.println("-----------------------");
  Serial.print("Connecting to WiFi");
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  while (WiFi.status() != WL_CONNECTED && !PARAR) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("Connected!");
  Serial.println("-----------------------");

  //connecting to a mqtt broker
  client.setServer(MQTT_BROKER, MQTT_PORT);


  // Callback para cuando recibimos un mensaje 
  client.setCallback(callback);
  // Conexion
  reconnect(); // Función para conectarnos al servidor MQTT

  if (client.connected()) {
    // Publish and subscribe
    client.publish(MQTT_TOPIC_COMMANDS, "ESP32 Conectado!");
    client.subscribe(MQTT_TOPIC_STATUS);
  }


  // Creación de tareas
  xTaskCreate(Control_PulMarcha,"Control_PulMarcha",10000,NULL,2,NULL);
  xTaskCreate(Control_Sistema,"Control_Sistema",10000,NULL,2,NULL);
}


void loop() {
  Serial.println("Inicio Main");
  Serial.println("-----------------------");

  while (!PARAR)
  {
    // Manejar los mensajes MQTT
    if (client.connected()) {
      client.loop();
    } else {
      reconnect(); // Intentar reconectar al servidor MQTT
    }

    delay(100); // Pequeña pausa para evitar un bucle demasiado rápido
  }
  Serial.println("CERRANDO");
  delay(3000);
  Serial.println("FIN Main");
  exit(0);
}


//--------------------------------------------------------------------------------------------------------
// FUNCIONES DESARROLLADAS

// Callback para cuando recibimos un mensaje
void callback(char *topic, byte *payload, unsigned int length) {
    String mensaje = ""; // Inicializar una cadena vacía para almacenar el mensaje recibido

    Serial.print("Message arrived in topic: ");
    Serial.println(topic);
    Serial.print("Message:");
    for (int i = 0; i < length; i++) {
        Serial.print((char) payload[i]);
        mensaje += (char) payload[i]; // Concatenar cada carácter recibido al final de la cadena
    }
    Serial.println();
    Serial.println("-----------------------");

    // Comprobar el mensaje recibido

    // Comprovamos si el sistema esta en marcha
    if (!marcha_activada && mensaje.equals("Comando de Marcha recibido")) {
        ESPByte = 0b00000001; // Arrancamos el sistema
        marcha_activada = true; // Establecer la bandera a verdadero
        MARCHA =  false;  // Establecer la bandera a falso
    } else if(!marcha_activada && MARCHA) {
        marcha_activada = false; // Establecer la bandera a falso
        MARCHA =  false;  // Establecer la bandera a falso
        Serial.println("Mensaje no reconocido");
        digitalWrite(LED_MARCHA, LOW);
        digitalWrite(PIN_BIT_0, LOW);
        Serial.println("-----------------------");
    }

    // Comprovamos si el mensaje probiene de la estacion de RoboDK
    if (mensaje.equals("Programa Estación RoboDK Ejecutado") && marcha_activada) {
      bloqueo_sensor = false;
      
      ESPByte = (ESPByte << 1);  // Desplazamos el bit de control
    } 
}


// Función para la reconexion MQTT
void reconnect()
{
  Serial.println("Inicio Función reconnect");
  Serial.println("-----------------------");

  while (!PARAR && !client.connected())
  {
    if (!client.connected()) {
      Serial.print("Attempting MQTT connection...");
      if (client.connect(MQTT_CLIENT_ID, MQTT_USER, MQTT_PASSWORD)) {
        Serial.println("connected");
        
        // Nos suscribimos despues de realizar la conexión
        client.subscribe(MQTT_TOPIC_STATUS);
      } else {
        Serial.print("failed, rc=");
        Serial.print(client.state());
        Serial.println(" try again in 5 seconds");
        delay(5000);
      }
    }

    delay(100);
  }
}


// Función para enviar mensajes por MQTT
void enviar_mensaje_mqtt(const char* mensaje) {
  if (client.connected()) {
    client.publish(MQTT_TOPIC_COMMANDS, mensaje);
  } else {
    Serial.println("Error: No se pudo enviar el mensaje MQTT, el cliente no está conectado.");
  }
}


//--------------------------------------------------------------------------------------------------------
// TAREAS

// Tarea que controla si el pulsador de marcha a sido activado
void Control_PulMarcha( void * parameter )
{
  Serial.println("Inicio Tarea Control_PulMarcha");
  Serial.println("-----------------------");

  while (!PARAR)
  {
    if (Button_Marcha.pressed && !bloqueo_pulsador) {
      Button_Marcha.pressed = false;
      bloqueo_pulsador = true;
      delay(100);

      if(MARCHA && !marcha_activada)
      {
        Serial.printf("El botón de Marcha ha sido pulsado\n");
        Serial.println("-----------------------");
        Serial.println("Encendido del LED");
        digitalWrite(LED_MARCHA, HIGH);
        digitalWrite(PIN_BIT_0, HIGH); // Encendemos bit de control
        Serial.println("-----------------------");
        enviar_mensaje_mqtt("0b00000001");
        delay(1000);
      } else {
        Button_Marcha.pressed = false;
        MARCHA = false;  // Establecer la bandera a falso
      }
    } else if(bloqueo_pulsador && !MARCHA) {
      Button_Marcha.pressed = false;
      bloqueo_pulsador = false;
    }

    delay(100);
  }  

  // Enviamos mensaje para apagar la estación
  enviar_mensaje_mqtt("PARAR");
  Serial.println("Mensaje para PARAR la estación enviado");

  delay(100);
  // Apagamos el led de control 
  digitalWrite(LED_MARCHA, LOW);

  Serial.println("Finalizando Tarea Control_PulMarcha");
  vTaskDelete( NULL );
}


// Tarea para controlar por donde se encuentra el sistema
void Control_Sistema( void * parameter )
{
  Serial.println("Inicio Tarea Control_Sistema");
  Serial.println("-----------------------");

  while (!PARAR)
  {
  
    if (ESPByte != sistemaByte && Sensor.pressed && !bloqueo_sensor)
    {
      Sensor.pressed = false;
      bloqueo_sensor = true;
      
      String mensaje = "0b";

      // Busca el bit activo
      for (int i = 0; i < 8; i++) 
      {
        //if (ESPByte & (1 << i)) 
        if (ESPByte & (1 << (7 - i)))
        {
          sistemaByte = ESPByte;

          mensaje += "1";
          for (int j = (i+1); j < 8; j++)
          {
            mensaje += "0";
          } 


          // Apagamos bits de control 
          digitalWrite(PIN_BIT_0, LOW);
          digitalWrite(PIN_BIT_1, LOW);
          digitalWrite(PIN_BIT_2, LOW);
          digitalWrite(PIN_BIT_3, LOW);
          digitalWrite(PIN_BIT_4, LOW);

          // Encendemos bit de control 
          if(i == 6)
          {
            digitalWrite(PIN_BIT_1, HIGH); // Encendemos bit de control
          } 
          else if(i == 5)
          {
            digitalWrite(PIN_BIT_2, HIGH); // Encendemos bit de control
          } 
          else if(i == 4)
          {
            digitalWrite(PIN_BIT_3, HIGH); // Encendemos bit de control
          } 
          else if(i == 3)
          {
            digitalWrite(PIN_BIT_4, HIGH); // Encendemos bit de control
          }

          Serial.printf("El bit %d está activo.\n", 7 - i);
          enviar_mensaje_mqtt(mensaje.c_str());
          
          break; // Sal del bucle una vez que encuentres el bit activo
        }
        mensaje += "0";
      }
    } 
    else if(Sensor.pressed && bloqueo_sensor)
    {
      Sensor.pressed = false;
    }
    else if(ESPByte == 0b00100000)
    {    
      Serial.println("El bit 5 está activo.");
      ESPByte = 0b00000000;      // Paramos el sistema
      sistemaByte = 0b00000000; 
      marcha_activada = false;  // Establecer la bandera a falso
      Serial.println("La estacón de soldadura ha termninado");
      digitalWrite(LED_MARCHA, LOW);
      digitalWrite(PIN_BIT_4, LOW);
    }
    
    delay(100);
  } 

  delay(100);
  // Apagamos bits de control 
  digitalWrite(PIN_BIT_0, LOW);
  digitalWrite(PIN_BIT_1, LOW);
  digitalWrite(PIN_BIT_2, LOW);
  digitalWrite(PIN_BIT_3, LOW);
  digitalWrite(PIN_BIT_4, LOW);

  Serial.println("Finalizando Tarea Control_Sistema");
  vTaskDelete( NULL );
}