/*
  Código final para los módulos. Simulación no disponible porque la
  pantalla física tiene pines diferentes a los de la simulación
*/
#include <SPI.h>
#include <ESP32Servo.h>
#include <Adafruit_GFX.h>  
#include <Adafruit_ILI9341.h>
#include <XPT2046_Touchscreen.h>

// Módulo A
const uint16_t pinServoA                 = 17;
const uint16_t pinEnableA                = 18;
const uint16_t pinDireccionA             = 8;
const uint16_t pinStepA                  = 3;
const uint16_t pinOptoacopladorInicialA  = 9;  
const uint16_t pinOptoacopladorFinalA    = 10;
Servo ServoA;
// Módulo B
const uint16_t pinServoB                 = 36;
const uint16_t pinEnableB                = 18;
const uint16_t pinDireccionB             = 38;
const uint16_t pinStepB                  = 46;
const uint16_t pinOptoacopladorInicialB  = 48;
const uint16_t pinOptoacopladorFinalB    = 35;
Servo ServoB;
// Módulo C
const uint16_t pinServoC                 = 13;
const uint16_t pinEnableC                = 18;
const uint16_t pinDireccionC             = 21;
const uint16_t pinStepC                  = 47;
const uint16_t pinOptoacopladorInicialC  = 37;
const uint16_t pinOptoacopladorFinalC    = 14;
Servo ServoC;
// Módulo D (pantalla)
const uint16_t pinDC                     = 42;
const uint16_t pinCS                     = 1;
const uint16_t pinMOSI                   = 41;
const uint16_t pinCLK                    = 40;
const uint16_t pinMISO                   = 16;
const uint16_t pinRESET                  = 2;
const uint16_t pinTouchCS                = 7; 
const uint16_t pinTouchCLK               = 15; 
const uint16_t pinTouchDIN               = 6; 
const uint16_t pinTouchDO                = 5; 
const uint16_t pinTouchIRQ               = 4; 
Adafruit_ILI9341 tft = Adafruit_ILI9341(pinCS, pinDC, pinRESET);
SPIClass mySPI(HSPI);
XPT2046_Touchscreen touch(pinTouchCS, pinTouchIRQ);
// Sensor Infrarrojo
const uint16_t pinSensorInfrarrojo       = 12;
 
//Definición de constantes
const bool ACTIVO                        = HIGH;
const bool HORARIO                       = HIGH;  //Sentido para abrir las tapas de los módulos A y B
const bool ANTIHORARIO                   = LOW;   //Sentido para cerrar las tapas de los módulos A y B
const uint16_t ANGULO_SERVO_INICIO_A     = 95;
const uint16_t ANGULO_SERVO_FIN_A        = 30;
const uint16_t ANGULO_SERVO_INICIO_B     = 90;
const uint16_t ANGULO_SERVO_FIN_B        = 150;
const uint16_t ANGULO_SERVO_INICIO_C     = 76;
const uint16_t ANGULO_SERVO_FIN_C        = 180;
 
//Estados del sistema
const uint16_t REPOSO                    = -1;
const uint16_t INICIALIZACION            = 1;
const uint16_t SUJECION                  = 2;
const uint16_t REPLIEGUE_PLATAFORMA      = 3;
const uint16_t GIRO_SERVOMOTORES         = 4;
const uint16_t DESPLIEGUE_PLATAFORMA     = 5;

//Estados de la pantalla
const uint16_t MENU_DE_PAUSA                      = 1;
const uint16_t MENU_VELOCIDAD                     = 2;
const uint16_t MENSAJE_SEPARACION_CLAMSHELL       = 3;
const uint16_t MENU_EMERGENCIA                    = 4;
const uint16_t MENSAJE_MODIFICANDO_VELOCIDAD      = 5;
const uint16_t MENU_DE_DATOS                      = 6;
const uint16_t MENU_DURANTE_LA_SEPARACION         = 7;
const uint16_t MENSAJE_PAUSANDO_MOVIMIENTO        = 8;
const uint16_t MENU_PARA_INICIALIZACION           = 9;
const uint16_t MENU_DE_MONITOREO                  = 10;
const uint16_t MENU_ESPERANDO_INICIO_MOVIMIENTO   = 11;
const uint16_t CALIBRANDO                         = 12;

//Variables para la inicialización de módulos
bool bandera_A;
bool EnPosicionInicialMotoresDePaso;
bool InicioCompletoA;
bool InicioCompletoB;
bool InicioCompletoC;

//Variables para la sujeción de clamshells
bool bandera_B;
bool EnPosicionFinalSujecion;
bool EnPosicionFinalSujecionA;
bool EnPosicionFinalSujecionB;

//Variables para el repliegue de la plataforma
bool bandera_C;
bool EnPosicionFinalRepliegue;

//Variables para el giro de servomotores
bool bandera_D;
bool SentidoAntihorarioA;  
bool SentidoAntihorarioB;  
bool SentidoAntihorarioC;  
uint16_t AnguloActualA;    
uint16_t AnguloActualB;    
uint16_t AnguloActualC;    
bool GiroCompleto;          
bool GiroCompletoA;        
bool GiroCompletoB;        
bool GiroCompletoC;  

//Variables para el despliegue de la plataforma
bool EnPosicionInicial;
bool bandera_E;

//Variables para el control de velocidad de los motores
unsigned int TiempoPorGrado;            //Tiempo de espera para controlar la velocidad de giro de los servomotores  
unsigned int TiempoPorPasoModulosAyB;   //Tiempo entre pasos del motor de pasos A y B en microsegundos
unsigned int TiempoPorPasoModuloC;      //Tiempo entre pasos del motor de pasos C en microsegundos

//Variables para controlar el estado de los módulos
uint16_t EstadoModulos;            //Estado actual de la operación
uint16_t EstadoAnteriorModulos;
unsigned int AntiguoContador;
unsigned int Contador;

//Variables para controlar la impresión de mensajes
bool MensajesInicializacion[6];
bool MensajesSujecion[5];
bool MensajesRepliegue[3];
bool MensajesDespliegue[3];
bool MensajesGiroServomotores[5];

//Variables de estado de la pantalla
const uint16_t pinSCL = 0;
const uint16_t pinSDA = 45;
int pantallaActual ;
bool dibujarPantalla ;
int pantallaAnterior;
int centro_x, centro_y, radio;
unsigned long TiempoAnterior;
unsigned long interval;
bool EsVelRapido;
bool hayToque;
int x;
int y;
int ContadorPuntos=0;
bool InicializarArreglosParaImpresionDeMensajes;

void setup() {
  ConfiguracionModuloA();
  ConfiguracionModuloB();
  ConfiguracionModuloC();
  ConfiguracionSensorInfrarrojo();
  ConfiguracionPantalla();
  InicializacionVariables();
}
/*
  Se analiza los estados de los actuadores (Motores de paso y servomotores) y de la pantalla para realizar acciones según el estado correspondiente
*/

void loop() {
  ControlarPantalla();
  switch (EstadoModulos){
    case INICIALIZACION:
      InicializacionPosicionesModulos();
      break;
    case SUJECION:
      SujecionDeClamshell();
      break;
    case REPLIEGUE_PLATAFORMA:
      ReplieguePlataforma();
      break;
    case GIRO_SERVOMOTORES:
      GiroServomotores();
      break;
    case DESPLIEGUE_PLATAFORMA:
      DesplieguePlataforma();
      break;
    default:
      break;
  }
}
 
void ConfiguracionModuloA(){
  pinMode(pinEnableA,OUTPUT);
  pinMode(pinDireccionA,OUTPUT);
  pinMode(pinStepA,OUTPUT);
  pinMode(pinOptoacopladorInicialA,INPUT_PULLDOWN);
  pinMode(pinOptoacopladorFinalA,INPUT_PULLDOWN);
}
 
void ConfiguracionModuloB(){
  pinMode(pinEnableB,OUTPUT);
  pinMode(pinDireccionB,OUTPUT);
  pinMode(pinStepB,OUTPUT);
  pinMode(pinOptoacopladorInicialB,INPUT_PULLDOWN);
  pinMode(pinOptoacopladorFinalB,INPUT_PULLDOWN);
}
 
void ConfiguracionModuloC(){
  pinMode(pinEnableC,OUTPUT);
  pinMode(pinDireccionC,OUTPUT);
  pinMode(pinStepC,OUTPUT);
  pinMode(pinOptoacopladorInicialC,INPUT_PULLDOWN);
  pinMode(pinOptoacopladorFinalC,INPUT_PULLDOWN);
}

void ConfiguracionSensorInfrarrojo(){
  pinMode(pinSensorInfrarrojo,INPUT_PULLUP);
}

void ConfiguracionPantalla(){
  // Iniciar bus SPI
  SPI.begin(pinCLK, pinMISO , pinMOSI);
  // Configurar pantalla
  tft.begin(10000000);
  tft.setRotation(0);
  tft.fillScreen(ILI9341_BLACK);
  mySPI.begin(pinTouchCLK, pinTouchDO, pinTouchDIN, pinTouchCS);
  touch.begin(mySPI);
  touch.setRotation(0);
  // Mensaje inicial
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.setCursor(65, 100);
  tft.println("Pantalla lista");
  delay(1000);
  tft.fillScreen(ILI9341_BLACK);
}
void InicializacionVariables(){
  //Módulos
  EstadoModulos = REPOSO;
  EstadoAnteriorModulos = REPOSO;
  TiempoPorGrado = 25000/(9*0.375); //Por defecto 0.125 revoluciones por segundo, velocidad rápida
  TiempoPorPasoModulosAyB = 10;
  TiempoPorPasoModuloC = 20;
  //Activación de motores
  ServoA.attach(pinServoA);
  digitalWrite(pinEnableA,LOW);
  ServoB.attach(pinServoB);
  digitalWrite(pinEnableB,LOW);
  ServoC.attach(pinServoC);
  digitalWrite(pinEnableC,LOW);
  //Pantalla
  pantallaActual = MENU_PARA_INICIALIZACION;
  dibujarPantalla = true;
  Contador = 0;
  AntiguoContador = 0;
  bandera_A = false;
  bandera_B = false;
  bandera_C = false;
  bandera_D = false;
  bandera_E = false;
  EsVelRapido = true;
  InicializarArreglosParaImpresionDeMensajes = true;
}
/*
  Se mueven los motores de pasos hasta alcanzar la posición inicial para el funcionamiento. Esto se marca con los optoacopladores de la posición inicial.
*/
void InicializacionPosicionesModulos(){
  if (!bandera_A){
    EnPosicionInicialMotoresDePaso = false;
    InicioCompletoA = false;
    InicioCompletoB = false;
    InicioCompletoC = false;
    //Servomotores
    ServoA.write(ANGULO_SERVO_INICIO_A);
    ServoB.write(ANGULO_SERVO_INICIO_B);
    ServoC.write(ANGULO_SERVO_INICIO_C);
    bandera_A = true;
  }
  //Motores de paso
  if (!EnPosicionInicialMotoresDePaso){
    EnPosicionInicialMotoresDePaso = InicioCompletoA && InicioCompletoB && InicioCompletoC;
    if(!InicioCompletoA){
      InicioCompletoA = digitalRead(pinOptoacopladorInicialA) == ACTIVO;
      if(!InicioCompletoA){  
        MoverUnPasoMotorDePasos(pinDireccionA,pinStepA,HORARIO,TiempoPorPasoModulosAyB);
      }
    }
    if(!InicioCompletoB){
      InicioCompletoB = digitalRead(pinOptoacopladorInicialB) == ACTIVO;
      if(!InicioCompletoB){
        MoverUnPasoMotorDePasos(pinDireccionB,pinStepB,HORARIO,TiempoPorPasoModulosAyB);
      }
    }
    if(!InicioCompletoC){
      InicioCompletoC = digitalRead(pinOptoacopladorInicialC) == ACTIVO;
      if(!InicioCompletoC){
        MoverUnPasoMotorDePasos(pinDireccionC,pinStepC,ANTIHORARIO,TiempoPorPasoModuloC);
      }
    }
  }else{
    bandera_A = false;
    EnPosicionInicialMotoresDePaso = false;
    InicioCompletoA = false;
    InicioCompletoB = false;
    InicioCompletoC = false;
    EstadoModulos = SUJECION;
  }
}
/*
  Se mueve los motores de pasos para cerrar las tapas de los módulos A y B hasta llegar a la posición final que es marcada por los optoacopladores de la posición final.
*/
void SujecionDeClamshell(){
  if (!bandera_B) {
    EnPosicionFinalSujecion  = false;
    EnPosicionFinalSujecionA = false;
    EnPosicionFinalSujecionB = false;
    bandera_B = true;
  }
  if(!EnPosicionFinalSujecion){
    EnPosicionFinalSujecion = EnPosicionFinalSujecionA && EnPosicionFinalSujecionB;
    if(!EnPosicionFinalSujecionA){
      EnPosicionFinalSujecionA = digitalRead(pinOptoacopladorFinalA) == ACTIVO;
      if(!EnPosicionFinalSujecionA){
        MoverUnPasoMotorDePasos(pinDireccionA,pinStepA,ANTIHORARIO,TiempoPorPasoModulosAyB);
      }
    }
    if(!EnPosicionFinalSujecionB){
      EnPosicionFinalSujecionB = digitalRead(pinOptoacopladorFinalB) == ACTIVO;
      if(!EnPosicionFinalSujecionB){
        MoverUnPasoMotorDePasos(pinDireccionB,pinStepB,ANTIHORARIO,TiempoPorPasoModulosAyB);
      }
    }
  }else{
    bandera_B = false;
    EnPosicionFinalSujecion  = false;
    EnPosicionFinalSujecionA = false;
    EnPosicionFinalSujecionB = false;
    EstadoModulos = REPLIEGUE_PLATAFORMA;
  }
}
/*
  Se mueve el motor de pasos del módulo C para replegar la plataforma y permitir que los clamshells puedan caer con el giro de los servomotores. El giro para la sujeción el horario.
*/
void ReplieguePlataforma(){
  if (!bandera_C){
    EnPosicionFinalRepliegue = false;
    bandera_C = true;
  }
  if(!EnPosicionFinalRepliegue){
    EnPosicionFinalRepliegue = digitalRead(pinOptoacopladorFinalC) == ACTIVO;
    if(!EnPosicionFinalRepliegue){
      MoverUnPasoMotorDePasos(pinDireccionC,pinStepC,HORARIO,TiempoPorPasoModuloC);
    }
  }else{
    bandera_C = false;
    EnPosicionFinalRepliegue = false;
    EstadoModulos = GIRO_SERVOMOTORES;
  }
}
/*
  Se giran los servomotores de los módulos A, B y C para separar una clamshell y dejarla caer. Se control la velociad de los servomotores según la configuración inicial.
*/
void GiroServomotores(){
  if (!bandera_D){
    SentidoAntihorarioA  = true;
    SentidoAntihorarioB  = true;
    SentidoAntihorarioC  = true;
    AnguloActualA    = ANGULO_SERVO_INICIO_A;
    AnguloActualB    = ANGULO_SERVO_INICIO_B;
    AnguloActualC    = ANGULO_SERVO_INICIO_C;
    GiroCompleto         = false;
    GiroCompletoA        = false;
    GiroCompletoB        = false;
    GiroCompletoC        = false;
    bandera_D = true;
  }
  if(!GiroCompleto){
    GiroCompleto = GiroCompletoA && GiroCompletoB && GiroCompletoC;
    //Servomotor módulo A
    if(!GiroCompletoA){
      if(SentidoAntihorarioA && AnguloActualA == ANGULO_SERVO_FIN_A){
        SentidoAntihorarioA = false;
      }else if(SentidoAntihorarioA && AnguloActualA != ANGULO_SERVO_FIN_A){
        AnguloActualA--;
      }else if(!SentidoAntihorarioA && AnguloActualA == ANGULO_SERVO_INICIO_A){
        GiroCompletoA = true;
      }else{ //Solo se presenta 4 estados para el giro de un servomotor
        AnguloActualA++;
      }
    }
    //Servomotor módulo B
    if(!GiroCompletoB){
      if(SentidoAntihorarioB && AnguloActualB == ANGULO_SERVO_FIN_B){
        SentidoAntihorarioB = false;
      }else if(SentidoAntihorarioB && AnguloActualB != ANGULO_SERVO_FIN_B){
        AnguloActualB++;
      }else if(!SentidoAntihorarioB && AnguloActualB == ANGULO_SERVO_INICIO_B){
       GiroCompletoB = true;
      }else{ //Solo se presenta 4 estados para el giro de un servomotor
        AnguloActualB--;
      }
    }
    //Servomotor módulo C
    if(!GiroCompletoC){
      if(SentidoAntihorarioC && AnguloActualC == ANGULO_SERVO_FIN_C){
        SentidoAntihorarioC = false;
      }else if(SentidoAntihorarioC && AnguloActualC != ANGULO_SERVO_FIN_C){
        AnguloActualC++;
      }else if(!SentidoAntihorarioC && AnguloActualC == ANGULO_SERVO_INICIO_C){
        GiroCompletoC = true;
      }else{ //Solo se presenta 4 estados para el giro de un servomotor
        AnguloActualC--;
      }
    }
    ServoA.write(AnguloActualA);
    ServoB.write(AnguloActualB);
    ServoC.write(AnguloActualC);
    delayMicroseconds(TiempoPorGrado);
  }else{
    bandera_D = false;
    GiroCompleto = false;
    GiroCompletoA = false;
    GiroCompletoB = false;
    GiroCompletoC = false;
    EstadoModulos = DESPLIEGUE_PLATAFORMA;
  }
}
/*
  Se despliega la plataforma del módulo C para sostener la pila de clamshells para el siguiente ciclo de seperación
*/
void DesplieguePlataforma(){
  if (!bandera_E){
    EnPosicionInicial = false;
    bandera_E = true;
  }
  if(!EnPosicionInicial){
    EnPosicionInicial = digitalRead(pinOptoacopladorInicialC) == ACTIVO;
    if(!EnPosicionInicial){
      MoverUnPasoMotorDePasos(pinDireccionC,pinStepC,ANTIHORARIO,TiempoPorPasoModuloC);
    }
  }else{
    bandera_E = false;
    EnPosicionInicial = false;
    //Fin del proceso de separación de un clamshell, se actualiza el contador de clamshells separados
    Contador++;
    if(digitalRead(pinSensorInfrarrojo) == LOW){
      EstadoModulos = INICIALIZACION;
    }else{
      EstadoModulos = REPOSO;
      pantallaActual = MENU_ESPERANDO_INICIO_MOVIMIENTO;
    }
  }
}
/*
  Se avanza de paso en paso los motores de paso para poder controlar todos a mismo tiempo.
*/
void MoverUnPasoMotorDePasos(uint16_t pinDireccion,uint16_t pinStep,bool Sentido, uint16_t TiempoPorPaso){
  //Sentido de giro
  digitalWrite(pinDireccion,Sentido);
  //1 paso cada 10 ms
  digitalWrite(pinStep,HIGH);
  delay(TiempoPorPaso/2);
  digitalWrite(pinStep,LOW);
  delay(TiempoPorPaso/2);
}
void LimpiarPantalla(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_GREEN);
  tft.setCursor(0, 0);
}
void ControlarPantalla(){
  hayToque = false; //No guardar valores anteriores
  x = -5, y = -5; //No guardar valores anteriores
  if (touch.touched()){
    TS_Point p = touch.getPoint();
    x = map(p.x, 150, 3700, 0, 320);
    y = map(p.y, 150, 3700, 0, 240);
    hayToque = true;
  }
  switch(pantallaActual) {
    case MENU_PARA_INICIALIZACION:
      if (dibujarPantalla) {
        dibujarInicio();
        dibujarPantalla = false;
        x = -5, y = -5;
      }
      if (hayToque) procesarToqueInicio(x, y);
      break;
    case MENU_ESPERANDO_INICIO_MOVIMIENTO:
      if (dibujarPantalla) {
        dibujarMenuPrinciparaIniciarMovimiento();
        dibujarPantalla = false;
        x = -5, y = -5;
        pantallaAnterior = MENU_ESPERANDO_INICIO_MOVIMIENTO;
      }
      if (hayToque) procesarToqueMenuPrincipalParaIniciarMovimiento(x, y);
      break;
    case MENU_VELOCIDAD:
      if (dibujarPantalla) {
        dibujarMenuVelocidad();
        dibujarPantalla = false;
        x = -5, y = -5;
      }
      if (hayToque) procesarToqueMenuVelocidad(x, y);
      break;
    case MENSAJE_MODIFICANDO_VELOCIDAD: //Mensaje intermedio, modificando velocidad
      if (dibujarPantalla) {
        dibujarModificandoVelocidad();
        dibujarPantalla = false;
        x = -5, y = -5;
        TiempoAnterior = millis();  // Solo se guarda una vez
      }
      if (hayToque) procesarModificandoVelocidad(x, y);
      if (PasoTiempo(3000)==true){
        pantallaActual = MENU_VELOCIDAD;
        dibujarPantalla = true;
      }
      break;
    case MENSAJE_SEPARACION_CLAMSHELL:  //Mensaje intermedio luego de iniciar el movimiento
      if (dibujarPantalla){
        dibujarPantallaSeparandoClamshells();
        dibujarPantalla = false;
        x = -5, y = -5;
        TiempoAnterior = millis(); // Solo se guarda una vez
      }
      if (hayToque) procesarTocoBotonDeEmergencia(x, y);
      if (PasoTiempo(3000)==true) {
        pantallaActual = MENU_DURANTE_LA_SEPARACION;
        dibujarPantalla = true;
      }
      break;
    case MENU_DURANTE_LA_SEPARACION:
      if (dibujarPantalla) {
        dibujarMenuPrincipal_MientrasSepara();
        dibujarPantalla = false;
        x = -5, y = -5;
        pantallaAnterior = MENU_DURANTE_LA_SEPARACION;
      }
      if (hayToque) procesarToqueMenuPrincipal_MientrasSepara(x, y);
      break;
    case MENSAJE_PAUSANDO_MOVIMIENTO: //Mensaje intermedio
      if (dibujarPantalla){
        dibujarPantallaPausandoMovimiento();
        dibujarPantalla = false;
        x = -5, y = -5;
        TiempoAnterior = millis();  // Solo se guarda una vez
      }
      if (hayToque) procesarTocoBotonDeEmergencia(x, y);
      if (PasoTiempo(1500)==true) {
        pantallaActual = MENU_DE_PAUSA;
        dibujarPantalla = true;
      }
      break;
    case MENU_DE_PAUSA:
      if (dibujarPantalla) {
        dibujarMenuPrincipalMientrasEspera();
        dibujarPantalla = false;
        x = -5, y = -5;
        pantallaAnterior = MENU_DE_PAUSA;
      }
      if (hayToque) procesarToqueMenuPrincipal(x, y);
      break;
    case MENU_DE_MONITOREO:
      if (dibujarPantalla) {
        dibujarPantallaDeMonitoreo();
        dibujarPantalla = false;
        x = -5, y = -5;
        InicializarArreglosParaImpresionDeMensajes = true;
      }
      if(InicializarArreglosParaImpresionDeMensajes){
        for(int i = 0; i<=5; i++){
          MensajesInicializacion[i] = true;
          MensajesGiroServomotores[i] = true;
        }
        for(int i = 0; i<=4; i++){
          MensajesSujecion[i] = true;
        }
        for(int i = 0; i<=3; i++){
          MensajesRepliegue[i] = true;
          MensajesDespliegue[i] = true;
        }
        InicializarArreglosParaImpresionDeMensajes = false;
      }
      ImprimirMensajes();
      if (hayToque) procesarToqueMenuMonitoreo(x, y);
      break;
    case MENU_DE_DATOS:
      if (dibujarPantalla) {
        dibujarMostrarDatos();
        dibujarPantalla = false;
        x = -5, y = -5;
      }
      tft.setTextSize(2);
      tft.setCursor(20, 80);
      tft.setTextColor(ILI9341_GREEN);
      tft.print(Contador);
      if(AntiguoContador != Contador){
        tft.setCursor(20, 80);
        tft.setTextColor(ILI9341_BLACK);
        tft.print(Contador-1);
        AntiguoContador = Contador;
      }
      if (hayToque) procesarToqueMostrarDatos(x, y);
      break;
    case MENU_EMERGENCIA:
      if (dibujarPantalla) {
        DesactivarMotores();
        x = -5, y = -5;
        dibujarPantallaEmergencia();
        dibujarPantallaEmergenciaDetenido();
        dibujarPantalla = false;
      }
      if(hayToque) ProcesarToqueEmergencia(x, y);
      break;
    case CALIBRANDO:
      if (dibujarPantalla) {
        dibujarCalibrando();
        dibujarPantalla = false;
        TiempoAnterior = millis();
        x = -5, y = -5;
      }
      if(EnPosicionInicialMotoresDePaso){
        tft.fillScreen(ILI9341_BLACK);
        tft.setTextSize(2);
        tft.setCursor(30, 105);
        tft.println("Sistema Calibrado");
        delay(1000);
        pantallaActual = MENU_ESPERANDO_INICIO_MOVIMIENTO;
        dibujarPantalla = true;
        //Actualizacion para estado de los módulos
        bandera_A = false;
        EnPosicionInicialMotoresDePaso = false;
        InicioCompletoA = false;
        InicioCompletoB = false;
        InicioCompletoC = false;
        EstadoModulos = REPOSO;
      }else{
        tft.setTextColor(ILI9341_GREEN);
        tft.setCursor(180, 105);
        if (PasoTiempo(250)==true && ContadorPuntos!=4){
          delay(10);
          if(ContadorPuntos==0) tft.print(".");
          if(ContadorPuntos==1) tft.print(" .");
          if(ContadorPuntos==2) tft.print("  .");
          ContadorPuntos++;
          TiempoAnterior = millis();
        }
        if(ContadorPuntos==4){
          tft.setTextColor(ILI9341_BLACK);
          tft.setCursor(180, 105);
          tft.print("...");
          ContadorPuntos=0;
          }
          if(hayToque) procesarTocoBotonDeEmergencia(x, y);
        }
      break;
    default:
    break;
  }
}
/*
  Se imprimen los mensajes en la pantalla una vez si está activo el menú de monitero con ayuda de las variables que confirman el fin de un subproceso de los actuadores. Además, se borran los mensajes anteriores para evitar dibujar nuevamente la pantalla
*/

void ImprimirMensajes(){
  switch(EstadoModulos){
    case INICIALIZACION:
      if(MensajesInicializacion[0]){
        tft.fillRect(87, 6, 210, 24, ILI9341_BLACK);
        tft.fillRect(0, 39, 315, 118, ILI9341_BLACK);
        tft.fillRect(0, 157, 232, 57, ILI9341_BLACK);
        tft.setTextSize(2);
        tft.setCursor(90,10);
        tft.println("Inicializacion\n");
        MensajesInicializacion[0] = false;
      }
      if(bandera_A && MensajesInicializacion[1]){
        tft.println("Servomotores inicializados");
        MensajesInicializacion[1] = false;
      }
      if(InicioCompletoA && MensajesInicializacion[2]){
        tft.println("Stepper A inicializado");
        MensajesInicializacion[2] = false;
      }
      if(InicioCompletoB && MensajesInicializacion[3]){
        tft.println("Stepper B inicializado");
        MensajesInicializacion[3] = false;
      }
      if(InicioCompletoC && MensajesInicializacion[4]){
        tft.println("Stepper C inicializado");
        MensajesInicializacion[4] = false;
      }
      if(EnPosicionInicialMotoresDePaso && MensajesInicializacion[5]){
        tft.println("Motores inicializados");
        MensajesInicializacion[5] = false;
      }
      break;
    case SUJECION:
      if(MensajesSujecion[0]){
        tft.fillRect(87, 6, 210, 24, ILI9341_BLACK);
        tft.fillRect(0, 39, 315, 118, ILI9341_BLACK);
        tft.fillRect(0, 157, 232, 57, ILI9341_BLACK);
        tft.setTextSize(2);
        tft.setCursor(90,10);
        tft.println("Sujecion\n");
        MensajesSujecion[0] = false;
      }
      if(bandera_B && MensajesSujecion[1]){
        tft.println("Cerrando tapas");
        MensajesSujecion[1] = false;
      }
      if(EnPosicionFinalSujecionA && MensajesSujecion[2]){
        tft.println("Stepper A listo para giro de servomotor");
        MensajesSujecion[2] = false;
      }
      if(EnPosicionFinalSujecionB && MensajesSujecion[3]){
        tft.println("Stepper B listo para giro de servomotor");
        MensajesSujecion[3] = false;
      }
      if(EnPosicionFinalSujecion && MensajesSujecion[4]){
        tft.println("Steppers listos para giro de servomotor");
        MensajesSujecion[4] = false;
      }
      break;
    case REPLIEGUE_PLATAFORMA:
      if(MensajesRepliegue[0]){
        tft.fillRect(87, 6, 210, 24, ILI9341_BLACK);
        tft.fillRect(0, 39, 315, 118, ILI9341_BLACK);
        tft.fillRect(0, 157, 232, 57, ILI9341_BLACK);
        tft.setTextSize(2);
        tft.setCursor(90,10);
        tft.println("Repliegue\n");
        MensajesRepliegue[0] = false;
      }
      if(bandera_C && MensajesRepliegue[1]){
        tft.println("Replegando plataforma");
        MensajesRepliegue[1] = false;
      }
      if(EnPosicionFinalRepliegue && MensajesRepliegue[2]){
        tft.println("Plataforma lista para el giro de los servomotores");
        MensajesRepliegue[2] = false;
      }
      break;
    case GIRO_SERVOMOTORES:
      if(MensajesGiroServomotores[0]){
        tft.fillRect(87, 6, 210, 24, ILI9341_BLACK);
        tft.fillRect(0, 39, 315, 118, ILI9341_BLACK);
        tft.fillRect(0, 157, 232, 57, ILI9341_BLACK);
        tft.setTextSize(2);
        tft.setCursor(90,10);
        tft.println("Giro\n");
        MensajesGiroServomotores[0] = false;
      }
      if(GiroCompletoA && MensajesGiroServomotores[1]){
        tft.println("Giro del servomotor A completado");
        MensajesGiroServomotores[1] = false;
      }
      if(GiroCompletoB && MensajesGiroServomotores[2]){
        tft.println("Giro del servomotor B completado");
        MensajesGiroServomotores[2] = false;
      }
      if(GiroCompletoB && MensajesGiroServomotores[3]){
        tft.println("Giro del servomotor C completado");
        MensajesGiroServomotores[3] = false;
      }
      if(GiroCompleto && MensajesGiroServomotores[4]){
        tft.println("Giro de servomotores completado");
        MensajesGiroServomotores[3] = false;
      }
      break;
    case DESPLIEGUE_PLATAFORMA:
      if(MensajesDespliegue[0]){
        tft.fillRect(87, 6, 210, 24, ILI9341_BLACK);
        tft.fillRect(0, 39, 315, 118, ILI9341_BLACK);
        tft.fillRect(0, 157, 232, 57, ILI9341_BLACK);
        tft.setTextSize(2);
        tft.setCursor(90,10);
        tft.println("Despliegue\n");
        MensajesDespliegue[0] = false;
      }
      if(bandera_E && MensajesDespliegue[1]){
        tft.println("Iniciando despliegue de plataforma");
        MensajesDespliegue[1] = false;
      }
      if(EnPosicionInicial && MensajesDespliegue[2]){
        tft.println("Plataforma desplegada completamente");
        MensajesDespliegue[2] = false;
        InicializarArreglosParaImpresionDeMensajes = true;
      }
      break;
    default:
      break;
  }
}
void dibujarInicio(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(1);
  tft.setCursor(60, 100);
  tft.println("Haz clic en la pantalla");
  tft.setCursor(10, 120);
  tft.print("para colocar los m");
  tft.write(162);
  tft.print("dulos en su posici");
  tft.write(162);
  tft.print("n inicial.");
}
void procesarToqueInicio(int x, int y) {
  if (x > 0 && x < 200 && y > 0 && y < 240) {
    pantallaActual = CALIBRANDO;
    dibujarPantalla = true;
    //Actualización estado de los módulos
    EstadoModulos = INICIALIZACION;
  }
}
void dibujarMenuPrinciparaIniciarMovimiento(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.setCursor(45, 1);
  tft.print("Men");
  tft.write(163);
  tft.println(" de opciones");
  tft.drawFastHLine(0, 20, 300, ILI9341_GREEN);
  tft.fillCircle(15, 50, 15, ILI9341_CYAN);
  tft.setCursor(40, 45);
  tft.println("Iniciar movimiento");
  tft.fillCircle(15, 95, 15, ILI9341_CYAN);
  tft.setCursor(40, 90);
  tft.println("Modificar velocidad");

  tft.setTextColor(ILI9341_BLUE);
  tft.setTextSize(2);
  tft.setCursor(0, 200);
  if (digitalRead(pinSensorInfrarrojo) == LOW){
    tft.println("Hay clamshells");
  }else{
    tft.println("No se detectan");
    tft.println("clamshells");
  }
  dibujarBotonDeEmergencia();
}
void procesarToqueMenuPrincipalParaIniciarMovimiento(int x, int y){
  if(DentroCirculo(x, y, 15, 50, 15)==1){ //Opción Separar clamshells
    if(digitalRead(pinSensorInfrarrojo) == LOW){ //Se detectó clamshells
      pantallaActual = MENSAJE_SEPARACION_CLAMSHELL; //Pantalla para iniciar el movimento
      EstadoModulos = SUJECION;
    }
    dibujarPantalla = true;
    //Conexión con estado de los módulos
  }else if(DentroCirculo(x, y, 15, 95, 15)==1){ // Opción Modificar velocidad
    pantallaActual = MENU_VELOCIDAD;  
    dibujarPantalla = true;
  }else if (DentroCirculo(x, y, 270, 195, 35)==1){
    pantallaActual = MENU_EMERGENCIA;
    dibujarPantalla = true;
  }
}
void dibujarMenuPrincipalMientrasEspera(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.setCursor(45, 1);
  tft.print("Men");
  tft.write(163);
  tft.println(" de opciones");
  tft.drawFastHLine(0, 20, 300, ILI9341_GREEN);
  tft.fillCircle(15, 50, 15, ILI9341_CYAN);
  tft.setCursor(40, 45);
  tft.println("Retornar movimiento");
  tft.fillCircle(15, 95, 15, ILI9341_CYAN);
  tft.setCursor(40, 90);
  tft.println("Modificar velocidad");
  tft.fillCircle(15, 140, 15, ILI9341_CYAN);
  tft.setCursor(40, 135);
  tft.println("Mostrar datos");
  // Botón de emergencia
  dibujarBotonDeEmergencia();
}
void procesarToqueMenuPrincipal(int x, int y){
  //Opción Separar clamshells
  if(DentroCirculo(x, y, 15, 50, 15)==1){
    pantallaActual = MENSAJE_SEPARACION_CLAMSHELL; //Pantalla para continuar el movimento
    dibujarPantalla = true;
    //Se restaura el estado anterior
    EstadoModulos = EstadoAnteriorModulos;
  }else if(DentroCirculo(x, y, 15, 95, 15)==1){ // Opción Modificar velocidad
    pantallaActual = MENU_VELOCIDAD;  
    dibujarPantalla = true;
  }else if(DentroCirculo(x, y, 15, 140, 15)==1){ //Opción Mostrar Datos
    pantallaActual = MENU_DE_DATOS;
    dibujarPantalla = true;
  }else if (DentroCirculo(x, y, 270, 195, 35)==1){
    pantallaActual = MENU_EMERGENCIA;
    dibujarPantalla = true;
  }
}
void dibujarMenuVelocidad() {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setTextSize(2);
  tft.setCursor(0,10);
  tft.println("Indique la velocidad:");
  tft.fillCircle(15, 80, 15, ILI9341_ORANGE);
  tft.setCursor(40, 75);
  tft.print("Modo R");
  tft.write(160);
  tft.print("pido");
  tft.fillCircle(15, 140, 15, ILI9341_ORANGE);
  tft.setCursor(40, 135);
  tft.print("Modo Lento");
  tft.setCursor(13, 210);
  tft.print("atr");
  tft.write(160);
  tft.print("s");
  //Botón de emergencia
  dibujarBotonDeEmergencia();
}
void procesarToqueMenuVelocidad(int x, int y) {
  if(DentroCirculo(x, y, 15, 80, 15) == 1){   //Velocidad lenta
    pantallaActual = 5;
    dibujarPantalla = true;
    EsVelRapido = false;
    //Actualización de variables para control de módulos
    TiempoPorGrado = 25000/(9*0.3); //0.1 revoluciones por segundo
    TiempoPorPasoModulosAyB = 10;
    TiempoPorPasoModuloC = 20;
  }else if(DentroCirculo(x, y, 15, 140, 15) == 1 ){ //Velocidad rápida
    pantallaActual = 5;
    dibujarPantalla = true;
    EsVelRapido = true;
    //Actualización de variables para control de módulos
    TiempoPorGrado = 25000/(9*0.375); //0.125 revoluciones por segundo
    TiempoPorPasoModulosAyB = 15;
    TiempoPorPasoModuloC = 25;
  }else if(DentroCirculo(x,y, 270, 195, 35)==1){  
    pantallaActual = MENU_EMERGENCIA;
    dibujarPantalla = true;
  }else if (x > 0 && x < 50 && y > 210 && y < 235) { //atrás
    if (pantallaAnterior == MENU_DE_PAUSA){   //Pantalla Inicial (No hay movimiento)
      pantallaActual = MENU_DE_PAUSA;
    } //Pantalla Inicial (Hay movimiento)
    else {
      pantallaActual = MENU_ESPERANDO_INICIO_MOVIMIENTO;
    }
    dibujarPantalla = true;
  }
}
void dibujarPantallaSeparandoClamshells() {
    tft.fillScreen(ILI9341_BLACK);
    tft.setTextColor(ILI9341_GREEN);
    tft.setTextSize(2);
    tft.setCursor(25, 100);
    tft.println("Separando clamshells");
    dibujarBotonDeEmergencia();
}
void dibujarModificandoVelocidad() {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_BLUE);
  tft.setTextSize(2);
  tft.setCursor(20, 105);
  tft.println("Modificando Velocidad");
  delay(1000);
  tft.fillScreen(ILI9341_BLACK);
  tft.setCursor(20, 105);
  tft.println("Velocidad Modificada");
}
void procesarModificandoVelocidad(int x, int y) {
  if (x > 0 && x < 200 && y > 10 && y < 240) {
    pantallaActual = MENU_VELOCIDAD;
    dibujarPantalla = true;
  }
}
void dibujarMostrarDatos(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setTextSize(1);
  tft.setCursor(20, 60);
  tft.print("N");
  tft.write(163);
  tft.println("mero de clasmshells separados: ");
  tft.setCursor(20, 100);
  tft.print("Velocidad configurada:");
  if (EsVelRapido){
    tft.setCursor(20, 110);
    tft.print("Velocidad configurada en modo lento");
  }else{
    tft.setCursor(20, 110);
    tft.print("Velocidad configurada en modo rapido");
  }
  tft.setCursor(13, 218);
  tft.print("atr");
  tft.write(160);
  tft.print("s");
  //botón de emergencia
  dibujarBotonDeEmergencia();
}
void procesarToqueMostrarDatos(int x, int y) {
  if (x > 0 && x < 50 && y > 210 && y < 235) {  //Atras
    if (pantallaAnterior == MENU_DE_PAUSA){   //Pantalla Inicial (No hay movimiento)
      pantallaActual = MENU_DE_PAUSA;
    }else{ //Pantalla Inicial (Hay movimiento)
      pantallaActual =  MENU_DURANTE_LA_SEPARACION ;
    }
    dibujarPantalla = true;
  }else if(DentroCirculo(x,y, 270, 195, 35)==1){  //Emergencia
    pantallaActual = MENU_EMERGENCIA;
    dibujarPantalla = true;
  }
}
void dibujarMenuPrincipal_MientrasSepara(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.setCursor(45, 1);
  tft.print("Men");
  tft.write(163);
  tft.println(" de opciones");
  tft.drawFastHLine(0, 20, 300, ILI9341_GREEN);
  tft.fillCircle(15, 50, 15, ILI9341_CYAN);
  tft.setCursor(40, 45);
  tft.println("Pausar movimiento");
  tft.fillCircle(15, 95, 15, ILI9341_CYAN);
  tft.setCursor(40, 135);
  tft.println("Mostrar datos");
  tft.fillCircle(15, 140, 15, ILI9341_CYAN);
  tft.setCursor(40, 90);
  tft.println("Monitoreo");
 
  // Botón de emergencia
  dibujarBotonDeEmergencia();
}
void procesarToqueMenuPrincipal_MientrasSepara(int x, int y){
  if(DentroCirculo(x, y, 15, 50, 15)==1){ //Mensaje de pausa
    pantallaActual = MENSAJE_PAUSANDO_MOVIMIENTO;
    dibujarPantalla = true;
    //Actualización de variable de estado de los módulos
    EstadoAnteriorModulos = EstadoModulos;
    EstadoModulos = REPOSO;
  }else if(DentroCirculo(x, y, 15, 95, 15)==1){ //Botón para menú de monitoreo
    pantallaActual = MENU_DE_MONITOREO;
    dibujarPantalla = true;
  }else if(DentroCirculo(x, y, 15, 140, 15)==1){ //Botón para menú de datos
    pantallaActual = MENU_DE_DATOS;
    dibujarPantalla = true;
  }else if (DentroCirculo(x, y, 270, 195, 35)==1){ //Botón de emergencia
    pantallaActual = MENU_EMERGENCIA;
    dibujarPantalla = true;
  }
}
void dibujarPantallaPausandoMovimiento(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.setCursor(25, 100);
  tft.println("Pausando movimiento");
  dibujarBotonDeEmergencia();
}
void dibujarPantallaDeMonitoreo(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_YELLOW);
  tft.setTextSize(2);
  tft.setCursor(0,10);
  tft.println("Estado:");
  tft.setCursor(13, 218);
  tft.print("atr");
  tft.write(160);
  tft.print("s");
  //botón de emergencia
  dibujarBotonDeEmergencia();
}
void procesarToqueMenuMonitoreo(int x, int y) {
  if(DentroCirculo(x,y, 270, 195, 35)==1){  //Emergencia
    pantallaActual = MENU_EMERGENCIA;
    dibujarPantalla = true;
  }else if (x > 0 && x < 50 && y > 210 && y < 235) { //Atrás
    pantallaActual = MENU_DURANTE_LA_SEPARACION; //Pantalla de menu, mientras hay movimiento
    dibujarPantalla = true;
  }
}
void dibujarBotonDeEmergencia(){
  tft.fillCircle(270, 195, 35, ILI9341_RED);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(1);  // Tamaño pequeño por lo largo del texto
  tft.setCursor(240,190);
  tft.print("EMERGENCIA");
}
bool PasoTiempo(unsigned long interval){
  unsigned long TiempoActual = millis();
  if(TiempoActual - TiempoAnterior >= interval){
    return true;
  }else{
    return false;
  }
}
bool DentroCirculo(int x, int y, int centro_x, int centro_y, int radio) {
  int distancia_x=x-centro_x;
  int distancia_y=y-centro_y;
  return (distancia_x*distancia_x + distancia_y*distancia_y)<= (radio*radio);
}
void DesactivarMotores(){
  ServoA.detach();
  digitalWrite(pinEnableA,HIGH);
  ServoB.detach();
  digitalWrite(pinEnableB,HIGH);
  ServoC.detach();
  digitalWrite(pinEnableC,HIGH);
}
void dibujarPantallaEmergencia(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(2);
  tft.setCursor(40, 100);
  tft.println("Deteniendo Sistema");
  delay(3000);
}
void dibujarPantallaEmergenciaDetenido(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(2);
  tft.setCursor(40, 90);
  tft.println("Sistema Detenido");
  tft.fillRoundRect(30, 130, 220, 40, 10, ILI9341_PINK);
  tft.setTextColor(ILI9341_BLACK);
  tft.setCursor(40, 145);
  tft.println("Reiniciar Sistema");
}
void ProcesarToqueEmergencia(int x, int y){
  if (x > 30 && x < 230 && y > 130 && y < 170) {
    pantallaActual = MENU_PARA_INICIALIZACION;
    dibujarPantalla = true;
    InicializacionVariables();
  }
}
void dibujarCalibrando(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.setCursor(55, 100);
  tft.println("Calibrando");
  dibujarBotonDeEmergencia();
}
void procesarTocoBotonDeEmergencia(int x, int y){
  if (DentroCirculo(x, y, 270, 195, 35)==1){ //Emergencia
    pantallaActual = MENU_EMERGENCIA;
    dibujarPantalla = true;
  }
}