#include <SPI.h>
#include <ESP32Servo.h>
#include <Adafruit_GFX.h>  
#include <Adafruit_ILI9341.h>

#include <math.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                        = true;
const bool HORARIO                       = true;  //Sentido para abrir las tapas de los módulos A y B
const bool ANTIHORARIO                   = false; //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_ESPERANDO_MOVIMIENTO    = 1;
const uint16_t MENU_VELOCIDAD               = 2;
const uint16_t MENU_EMERGENCIA              = 4;
const uint16_t MENU_DE_DATOS                = 6;
const uint16_t MENU_DURANTE_LA_SEPARACION   = 7;
const uint16_t MENU_PARA_INICIALIZACION     = 9;
const uint16_t MENU_DE_MONITOREO            = 10;

volatile unsigned int TiempoPorGrado;       //Tiempo de espera para controlar la velocidad de giro de los servomotores  
uint16_t EstadoModulos = REPOSO;            //Estado actual de la operación

//Variables para controlar la impresión de mensajes
bool MensajesInicializacion[6];


//
// Variables de estado 

const uint16_t pinSCL = 0; 
const uint16_t pinSDA = 45;

int pantallaActual = 9;
bool dibujarPantalla = true;
int pantallaAnterior;
int centro_x, centro_y, radio;
bool PantallaIniciada = false; //Condicion para pantallas que tienen touch y tiempo para cambiar de pantalla
unsigned long TiempoAnterior = millis();
unsigned long interval;

bool RegresoDeOpcionDeDatos=false;
//

void setup() {
 
  Serial.begin(115200);
  ConfiguracionModuloA();
  ConfiguracionModuloB();
  ConfiguracionModuloC();
  ConfiguracionPantalla();
 
  TiempoPorGrado = 25000/(27*0.125); //0.1 revoluciones por segundo, PRUEBAS
  //Se pregunta al usuario para empezar el proceso y se espera hasta su confirmación
  /*
  TiempoPorGrado = LeerVelocidadServomotor();
  LeerComandoDeInicio();
  */
}

bool limpieza[5] = {false, false, false, false, false};
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;
  }

  //Led infrarojo
}
 
void ConfiguracionModuloA(){
  pinMode(pinEnableA,OUTPUT);
  pinMode(pinDireccionA,OUTPUT);
  pinMode(pinStepA,OUTPUT);
  pinMode(pinOptoacopladorInicialA,INPUT_PULLDOWN);
  pinMode(pinOptoacopladorFinalA,INPUT_PULLDOWN);
  ServoA.attach(pinServoA);
 
  digitalWrite(pinEnableA,LOW);
}
 
void ConfiguracionModuloB(){
  pinMode(pinEnableB,OUTPUT);
  pinMode(pinDireccionB,OUTPUT);
  pinMode(pinStepB,OUTPUT);
  pinMode(pinOptoacopladorInicialB,INPUT_PULLDOWN);
  pinMode(pinOptoacopladorFinalB,INPUT_PULLDOWN);
  ServoB.attach(pinServoB);
 
  digitalWrite(pinEnableB,LOW);
}
 
void ConfiguracionModuloC(){
  pinMode(pinEnableC,OUTPUT);
  pinMode(pinDireccionC,OUTPUT);
  pinMode(pinStepC,OUTPUT);
  pinMode(pinOptoacopladorInicialC,INPUT_PULLDOWN);
  pinMode(pinOptoacopladorFinalC,INPUT_PULLDOWN);
  ServoC.attach(pinServoC);
 
  digitalWrite(pinEnableC,LOW);
}
 
void ConfiguracionPantalla(){
  // Iniciar bus SPI 
  SPI.begin(pinCLK, pinMISO , pinMOSI);
  //mySPI.begin(pinTouchCLK, pinTouchDO, pinTouchDIN, pinTouchCS);
  //touch.begin(mySPI);
  //touch.setRotation(0);

  // 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);
}

/*
  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.
*/
bool bandera_A[6] = {false, false, false, false, false,false};
bool EnPosicionInicialMotoresDePaso;
bool InicioCompletoA;
bool InicioCompletoB;
bool InicioCompletoC;

void InicializacionPosicionesModulos(){

  if (!bandera_A[0]){
    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);
    /*
    tft.fillScreen(ILI9341_BLACK);
    tft.setTextColor(ILI9341_GREEN);
    tft.setTextSize(2);
    tft.setCursor(0, 0);
    tft.println("Incialización: Servomotores en posición incial"); //Faltaría un tiempo de espera a la inicialización de los servomotores
    */
    bandera_A[0] = true;
  }
  //Motores de paso
  if (!EnPosicionInicialMotoresDePaso){
    EnPosicionInicialMotoresDePaso = InicioCompletoA && InicioCompletoB && InicioCompletoC;
    if(!InicioCompletoA){
      InicioCompletoA = digitalRead(pinOptoacopladorInicialA) == ACTIVO;
      if(InicioCompletoA){
        //tft.println("Incialización: Motor de pasos A en posición incial");  
      }else{
        MoverUnPasoMotorDePasos(pinDireccionA,pinStepA,HORARIO,10);
      }
    }
    if(!InicioCompletoB){
      InicioCompletoB = digitalRead(pinOptoacopladorInicialB) == ACTIVO;
      if(InicioCompletoB){
        //tft.println("Incialización: Motor de pasos B en posición incial");
      }else{
        MoverUnPasoMotorDePasos(pinDireccionB,pinStepB,HORARIO,10);
      }
    }
    if(!InicioCompletoC){
      InicioCompletoC = digitalRead(pinOptoacopladorInicialC) == ACTIVO;
      if(InicioCompletoC){
        //tft.println("Incialización: Motor de pasos C en posición incial");
      }else{
        MoverUnPasoMotorDePasos(pinDireccionC,pinStepC,ANTIHORARIO,20);
      }
    }
  }else if(!bandera_A[4]){
    //tft.println("Incialización: Motores de pasos en posición incial");
    bandera_A[4] = true;
    for (int i = 0; i < 5; i++) {
      bandera_A[i] = 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.
*/
bool bandera_B[5] = {false, false, false, false,false};
bool EnPosicionFinal;
bool EnPosicionFinalA;
bool EnPosicionFinalB;

void SujecionDeClamshell(){
 
  if (!bandera_B[0]) {
    EnPosicionFinal  = false;
    EnPosicionFinalA = false;
    EnPosicionFinalB = false;
    //tft.println("Sujección: Cerrando tapas");
    bandera_B[0] = true;
  }
   
  if(!EnPosicionFinal){
    EnPosicionFinal = EnPosicionFinalA && EnPosicionFinalB;
    if(!EnPosicionFinalA){
      EnPosicionFinalA = digitalRead(pinOptoacopladorFinalA) == ACTIVO;
      if(EnPosicionFinalA){
        //tft.println("Sujección: Motor de pasos A en posición para el giro de los servomotores");   
      }else{
        MoverUnPasoMotorDePasos(pinDireccionA,pinStepA,ANTIHORARIO,10);
      }
    }
    if(!EnPosicionFinalB){
      EnPosicionFinalB = digitalRead(pinOptoacopladorFinalB) == ACTIVO;
      if(EnPosicionFinalB){
        //tft.println("Sujección: Motor de pasos B en posición para el giro de los servomotores");
      }else{
        MoverUnPasoMotorDePasos(pinDireccionB,pinStepB,ANTIHORARIO,10);
      }
    }
  }else if(!bandera_B[3]){
    //tft.println("Sujección: Módulos A y B en posición para el giro de los servomotores.");
    EstadoModulos = REPLIEGUE_PLATAFORMA;
    bandera_B[3] = true;
    for (int i = 0; i < 4; i++) {
      bandera_B[i] = false;
    }
  }
}
/*
  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.
*/
bool bandera_C = false;
void ReplieguePlataforma(){

  if (!bandera_C){
    EnPosicionFinal = false;
    //tft.println("Repliegue: Iniciando repliegue de plataforma");
    bandera_C = true;
  }
 
  if(!EnPosicionFinal){
    EnPosicionFinal = digitalRead(pinOptoacopladorFinalC) == ACTIVO;
    if(EnPosicionFinal){
      //tft.println("Repliegue: Plataforma replegada lista para el giro de los servomotores");
      bandera_C = false;
      EstadoModulos = GIRO_SERVOMOTORES;
    }else{
      MoverUnPasoMotorDePasos(pinDireccionC,pinStepC,HORARIO,20);
    }
  }
}

/*
  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.
*/
bool SentidoAntihorarioA;  
bool SentidoAntihorarioB;  
bool SentidoAntihorarioC;  
uint16_t AnguloActualA;    
uint16_t AnguloActualB;    
uint16_t AnguloActualC;    
bool GiroCompleto;          
bool GiroCompletoA;        
bool GiroCompletoB;        
bool GiroCompletoC;  

bool bandera_D[5] = {false, false, false, false, false};
void GiroServomotores(){
  if (!bandera_D[0]){
    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[0] = 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;
        //tft.println("Giro servomotores: Giro del servomotor A completado");
      }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;
       //tft.println("Giro servomotores: Giro del servomotor B completado");
      }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+=2;
      }else if(!SentidoAntihorarioC && AnguloActualC == ANGULO_SERVO_INICIO_C){
        GiroCompletoC = true;
        //tft.println("Giro servomotores: Giro del servomotor C completado");
      }else{ //Solo se presenta 4 estados para el giro de un servomotor
        AnguloActualC-=2;
      }
    }
    ServoA.write(AnguloActualA);
    ServoB.write(AnguloActualB);
    ServoC.write(AnguloActualC);
    delayMicroseconds(TiempoPorGrado);
  }else{
    EstadoModulos = DESPLIEGUE_PLATAFORMA;
    //tft.println("Giro servomotores: Giro de servomotores completado");
    for (int i = 0; i < 4; i++) {
      bandera_D[i] = false;
    }
  }
}
/*
  Se despliega la plataforma del módulo C para sostener la pila de clamshells
  para el siguiente ciclo de seperación
*/
bool EnPosicionInicial;
bool bandera_E[5] = {false, false, false, false, false};
void DesplieguePlataforma(){
 
  if (!bandera_E[0]){
    EnPosicionInicial = false;
    //tft.println("Repliegue: Iniciando repliegue de plataforma");
    bandera_E[0]=true;
  }

  if(!EnPosicionInicial){
    EnPosicionInicial = digitalRead(pinOptoacopladorInicialC) == ACTIVO;
    if(EnPosicionInicial){
      //tft.println("Repliegue: Plataforma desplegada completamente");
      for (int i = 0; i < 2; i++) {
        bandera_E[i] = false;
      }
      EstadoModulos = INICIALIZACION;
    }else{
      MoverUnPasoMotorDePasos(pinDireccionC,pinStepC,ANTIHORARIO,20);
    }
  }
}

/*
  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(){

  bool hayToque = false; //No guardar valores anteriores 
  int16_t 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_ESPERANDO_MOVIMIENTO:
      if (dibujarPantalla) {
        dibujarMenuPrincipal();
        dibujarPantalla = false;
        x = -5, y = -5;
      }
      pantallaAnterior = 1;
      RegresoDeOpcionDeDatos = false;

      if (hayToque) procesarToqueMenuPrincipal(x, y);
      break;

    case MENU_VELOCIDAD:
      if (dibujarPantalla) {
        dibujarMenuVelocidad();
        dibujarPantalla = false;
        x = -5, y = -5;
      }
      if (hayToque) procesarToqueMenuVelocidad(x, y);
      break;

    case 3: //Mensaje intermedio luego de iniciar el movimiento
      if (dibujarPantalla && RegresoDeOpcionDeDatos == false) {
        dibujarPantallaSeparandoClamshells();
        dibujarPantalla = false;
        x = -5, y = -5;
      }
      pantallaAnterior = 3;
      if (hayToque) procesarToqueSeparandoClamshells(x, y);
      if (!PantallaIniciada && !RegresoDeOpcionDeDatos == false) {
        TiempoAnterior = millis();  // Solo se guarda una vez
        PantallaIniciada = true;
      }

      if (PasoTiempo(2000) == true || !RegresoDeOpcionDeDatos == false) {
        pantallaActual = 7;
        dibujarPantalla = true;
        PantallaIniciada = false;
      }
      break;

    case MENU_EMERGENCIA: 
      tft.fillScreen(ILI9341_BLACK);
      tft.setTextColor(ILI9341_RED);
      tft.setTextSize(2);
      tft.setCursor(40, 100);
      tft.println("Sistema detenido");
      pantallaActual = 9;
      delay(5000);
      dibujarPantalla = true;
      break;

    case 5: //Mensaje intermedio, modificandoVelocidad
      if (dibujarPantalla) {
        dibujarModificandoVelocidad();
        dibujarPantalla = false;
        x = -5, y = -5;
      }
      if (hayToque) procesarModificandoVelocidad(x, y);
      //----------------
      if (!PantallaIniciada) {
        TiempoAnterior = millis();  // Solo se guarda una vez
        PantallaIniciada = true;
      }
      if (PasoTiempo(2000) == true) {
        pantallaActual = 2;
        dibujarPantalla = true;
        PantallaIniciada = false;  // Reinicias la bandera si vas a otra pantalla
      }
      break;

    case MENU_DE_DATOS: 
      if (dibujarPantalla) {
        dibujarMostrarDatos();
        dibujarPantalla = false;
        x = -5, y = -5;
      }
      if (hayToque) procesarToqueMostrarDatos(x, y);
      break;

    case MENU_DURANTE_LA_SEPARACION:
      if (dibujarPantalla) {
        dibujarMenuPrincipal_MientrasSepara();
        dibujarPantalla = false;
        x = -5, y = -5;
      }
      if (hayToque) procesarToqueMenuPrincipal_MientrasSepara(x, y);
      break;

    case 8: //Mensaje intermedio, pantalla Inicio Antes
      if (dibujarPantalla) {
        tft.fillScreen(ILI9341_BLACK);
        tft.setTextColor(ILI9341_GREEN);
        tft.setTextSize(2);
        tft.setCursor(25, 100);
        tft.println("Pausando movimiento");
        dibujarBotonDeEmergencia();
        delay(2000);
        pantallaActual = 1;
      }
      break;

    case MENU_PARA_INICIALIZACION:
      if (dibujarPantalla) {
        dibujarInicio();
        dibujarPantalla = false;
        x = -5, y = -5;
      }
      if (hayToque) procesarToqueInicio(x, y);
      break;

    case MENU_DE_MONITOREO:
      if (dibujarPantalla) {
        dibujarPantallaDeMonitoreo();
        dibujarPantalla = false;
        x = -5, y = -5;
        //Inicialización de arreglos
        EstadoModulos = INICIALIZACION;
        for(int i = 0; i<=5; i++){
          MensajesInicializacion[i] = true;
        }
        
      }

      ImprimirMensajes();

      if (hayToque) procesarToqueMenuMonitoreo(x, y);
      break;
    }

}


void ImprimirMensajes(){
  /*
  tft.setTextSize(2);
  tft.setCursor(90,10);
  tft.println("Incializacion");
  tft.setCursor(0,30);
  tft.setTextSize(2);
  tft.println("Servomotores incializados");
  tft.println("Stepper A incializado");
  tft.println("Stepper B incializado");
  tft.println("Stepper C incializado");
  tft.println("Motores incializados");
  */

  switch(EstadoModulos){
    case INICIALIZACION:
      if(MensajesInicializacion[0]){
        tft.setTextSize(2);
        tft.setCursor(90,10);
        tft.println("Incializacion\n");
        MensajesInicializacion[0] = false;
      }
      if(bandera_A[0] && MensajesInicializacion[1]){
        tft.println("Servomotores incializados");
        MensajesInicializacion[1] = false;
      }
      if(InicioCompletoA && MensajesInicializacion[2]){
        tft.println("Stepper A incializado");
        MensajesInicializacion[2] = false;
      }
      if(InicioCompletoB && MensajesInicializacion[3]){
        tft.println("Stepper B incializado");
        MensajesInicializacion[3] = false;
      }
      if(InicioCompletoC && MensajesInicializacion[4]){
        tft.println("Stepper C incializado");
        MensajesInicializacion[4] = false;
      }
      if(EnPosicionInicialMotoresDePaso && MensajesInicializacion[5]){
        tft.println("Motores incializados");
        MensajesInicializacion[5] = false;
      }
      break;
    case SUJECION:
      break;
    case REPLIEGUE_PLATAFORMA:
      break;
    case GIRO_SERVOMOTORES:
      break;
    case DESPLIEGUE_PLATAFORMA:
      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 = 1;
    dibujarPantalla = true;
  }
}
//---------------------------------------

void dibujarMenuPrincipal(){
  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.drawRect(108, 196, 90, 60, ILI9341_YELLOW);
  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.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 = 3; //Pantalla para iniciar el movmiento
    dibujarPantalla = true;
  }
  // Opción Modificar velocidad
  if(DentroCirculo(x, y, 15, 95, 15)==1){
    pantallaActual = 2;   //Pantalla para modificar velocidad
    dibujarPantalla = true;
  }
  //Opción Mostrar Datos
  if(DentroCirculo(x, y, 15, 140, 15)==1){
    pantallaActual = 6;
    dibujarPantalla = true;
  }
  if (DentroCirculo(x, y, 270, 195, 35)==1){
    pantallaActual = 4;
    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();
}
bool EsVelRapido=true;
void procesarToqueMenuVelocidad(int x, int y) {
  if(DentroCirculo(x, y, 15, 80, 15) == 1){   //Velocidad lenta
    pantallaActual = 5;
    dibujarPantalla = true;
    EsVelRapido = false;
  }
  if(DentroCirculo(x, y, 15, 140, 15) == 1 ){ //Velocidad rapida
    pantallaActual = 5;
    dibujarPantalla = true;
    EsVelRapido = true;
  }
  if(DentroCirculo(x,y, 270, 195, 35)==1){  //Emergencia
    pantallaActual = 4;
    dibujarPantalla = true;
  }
  //atrás
  if (x > 0 && x < 50 && y > 210 && y < 235) {
    pantallaActual = 1;
    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 procesarToqueSeparandoClamshells(int x, int y) {
  if (DentroCirculo(x, y, 270, 195, 35)==1){ //Emergencia
    pantallaActual = 4;
    dibujarPantalla = true;
  }
}
//------------------------------------
//---------------------------------------
void dibujarModificandoVelocidad() {
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_BLUE);
  tft.setTextSize(2);
  tft.setCursor(20, 105);
  tft.println("Modificando Velocidad");
  delay(2000);
  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 = 2;
    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 == 1){   //Pantalla Inicial (No hay movimiento)
      pantallaActual = 1;
    } //Pantalla Inicial (Hay movimiento)
    else {
      pantallaActual = 3;
      RegresoDeOpcionDeDatos = true;
    }
    dibujarPantalla = true;
  }
    if(DentroCirculo(x,y, 270, 195, 35)==1){  //Emergencia
    pantallaActual = 4;
    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.drawRect(108, 196, 90, 60, ILI9341_YELLOW);
  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){
  //Opción Pausar sistema
  if(DentroCirculo(x, y, 15, 50, 15)==1){
    pantallaActual = 8;
    dibujarPantalla = true;
  }
  // Opción Monitoreo
  if(DentroCirculo(x, y, 15, 95, 15)==1){
    pantallaActual = 10;
    dibujarPantalla = true;
  }
  //Opción Mostrar Datos
  if(DentroCirculo(x, y, 15, 140, 15)==1){
    pantallaActual = 6;
    dibujarPantalla = true;
  }
  if (DentroCirculo(x, y, 270, 195, 35)==1){  //Emergencia
    pantallaActual = 4;
    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 (pow(distancia_x,2) + pow(distancia_y,2))<= (radio*radio);
}
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 = 4;
    dibujarPantalla = true;
  }
  //atrás
  if (x > 0 && x < 50 && y > 210 && y < 235) {
    pantallaActual = 3; //Pantalla de menu, mientras hay movimiento
    dibujarPantalla = true;
    RegresoDeOpcionDeDatos = true;
  }
}