/*
  Código final para los módulos con simulación. No es el mismo para
  los módulos pues la pantalla tiene otros pines
*/

#include <SPI.h>
#include <ESP32Servo.h>
#include <Adafruit_GFX.h>  
#include <Adafruit_ILI9341.h>
#include <Adafruit_FT6206.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);
Adafruit_FT6206 touch = Adafruit_FT6206();
// 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     = 85;
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;
uint16_t ContadorPuntos;
bool InicializarArreglosParaImpresionDeMensajes;

void setup() {
  ConfiguracionModuloA();
  ConfiguracionModuloB();
  ConfiguracionModuloC();
  ConfiguracionSensorInfrarrojo();
  ConfiguracionPantalla();
  InicializacionVariables();
}

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(1);
  tft.fillScreen(ILI9341_BLACK);
  Wire.begin(pinSDA, pinSCL);
  if(!touch.begin()) {
    Serial.println("Verifica conexiones");
    while (1);
  }
  // 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.375 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 = p.y;  
    y = 240 - p.x;
    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;
  }
}
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.3 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.375 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;
  }
}