//Pines corregidos

#include <SPI.h>
#include <ESP32Servo.h>
#include <Adafruit_GFX.h>  
#include <Adafruit_ILI9341.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  = 14;
const uint16_t pinOptoacopladorFinalC    = 37;
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);
// 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     = 90; 
const uint16_t ANGULO_SERVO_FIN_A        = 30;
const uint16_t ANGULO_SERVO_INICIO_B     = 90; 
const uint16_t ANGULO_SERVO_FIN_B        = 30;
const uint16_t ANGULO_SERVO_INICIO_C     = 90; 
const uint16_t ANGULO_SERVO_FIN_C        = 30;

volatile unsigned int TiempoPorGrado;             //Tiempo de espera para controlar la velocidad de giro de los servomotores  

void setup() {

  Serial.begin(115200);
  ConfiguracionModuloA();
  ConfiguracionModuloB();
  ConfiguracionModuloC();
  ConfiguracionPantalla();


  TiempoPorGrado = 9259; //0.1 revoluciones por segundo, PRUEBAS
  //Se pregunta la usuario para empezar el proceso y se espera hasta su confirmación
  /*
  TiempoPorGrado = LeerVelocidadServomotor();
  LeerComandoDeInicio();
  */
}

void loop() {
  
  InicializacionPosicionesModulos();
  
  SujeccionDeClamshell();
  
  ReplieguePlataforma();

  GiroServomotores();

  DesplieguePlataforma();

  RetornoDeTapasDeSujeccion();
  /*
  LeerComandoDeContinuacion();
  */
  while(1){
    delay(1000);
  }
}

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(){
  SPI.begin(pinCLK, pinMISO, pinMOSI);

}

/*
  Se mueve los motores de pasos hasta alcanzar la posición inicial para el funcioamiento.
  Esto se marca con los optoacopladores de la posición inicial.
*/
void InicializacionPosicionesModulos(){
  bool InicioCompletoA = false;
  bool InicioCompletoB = false;
  bool InicioCompletoC = false;

  bool EnPosicionInicialMotoresDePaso;
  //Servomotores
  ServoA.write(ANGULO_SERVO_INICIO_A);
  ServoB.write(ANGULO_SERVO_INICIO_B);
  ServoC.write(ANGULO_SERVO_INICIO_C);
  Serial.println("Incialización: Servomotores en posición incial"); //Faltaría un tiempo de espera a la inicialización de los servomotores
  
  //Motores de paso
  while(!EnPosicionInicialMotoresDePaso){
    EnPosicionInicialMotoresDePaso = InicioCompletoA && InicioCompletoB && InicioCompletoC;
    if(!InicioCompletoA){
      InicioCompletoA = digitalRead(pinOptoacopladorInicialA) == ACTIVO;
      if(InicioCompletoA){
        Serial.println("Incialización: Motor de pasos A en posición incial");
      }else{
        MoverUnPasoMotorDePasos(pinDireccionA,pinStepA,HORARIO);
      }
    }
    if(!InicioCompletoB){
      InicioCompletoB = digitalRead(pinOptoacopladorInicialB) == ACTIVO;
      if(InicioCompletoB){
        Serial.println("Incialización: Motor de pasos B en posición incial");
      }else{
        MoverUnPasoMotorDePasos(pinDireccionB,pinStepB,HORARIO);
      }
    }
    if(!InicioCompletoC){
      InicioCompletoC = digitalRead(pinOptoacopladorInicialC) == ACTIVO;
      if(InicioCompletoC){
        Serial.println("Incialización: Motor de pasos C en posición incial");
      }else{
        MoverUnPasoMotorDePasos(pinDireccionC,pinStepC,HORARIO);
      }
    }
  }
  Serial.println("Incialización: Motores de pasos en posición incial");
}

void MoverUnPasoMotorDePasos(bool pinDireccion,uint16_t pinStep,bool Sentido){
  //Sentido de giro
  digitalWrite(pinDireccion,Sentido);
  //1 paso cada 10 ms
  digitalWrite(pinStep,HIGH);
  delay(5);
  digitalWrite(pinStep,LOW);
  delay(5);
}

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

  bool EnPosicionFinal  = false;
  bool EnPosicionFinalA = false;
  bool EnPosicionFinalB = false;

  Serial.println("Sujección: Cerrando tapas");
  while(!EnPosicionFinal){
    EnPosicionFinal = EnPosicionFinalA && EnPosicionFinalB;
    if(!EnPosicionFinalA){
      EnPosicionFinalA = digitalRead(pinOptoacopladorFinalA) == ACTIVO;
      if(EnPosicionFinalA){
        Serial.println("Sujección: Motor de pasos A en posición para el giro de los servomotores");
      }else{
        MoverUnPasoMotorDePasos(pinDireccionA,pinStepA,ANTIHORARIO);
      }
    }
    if(!EnPosicionFinalB){
      EnPosicionFinalB = digitalRead(pinOptoacopladorFinalB) == ACTIVO;
      if(EnPosicionFinalB){
        Serial.println("Sujección: Motor de pasos B en posición para el giro de los servomotores");
      }else{
        MoverUnPasoMotorDePasos(pinDireccionB,pinStepB,ANTIHORARIO);
      }
    }
  }
  Serial.println("Sujección: Módulos A y B en posición para el giro de los servomotores.");
}

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

  bool EnPosicionFinal = false;

  Serial.println("Repliegue: Iniciando repliegue de plataforma");
  while(!EnPosicionFinal){
    EnPosicionFinal = digitalRead(pinOptoacopladorFinalC) == ACTIVO;
    if(EnPosicionFinal){
      Serial.println("Repliegue: Plataforma replegada lista para el giro de los servomotores");
    }else{
      MoverUnPasoMotorDePasos(pinDireccionC,pinStepC,HORARIO);
    }
  }
}
/*
  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(){

  bool SentidoAntihorarioA  = true;
  bool SentidoAntihorarioB  = true;
  bool SentidoAntihorarioC  = true;
  uint16_t AnguloActualA    = ANGULO_SERVO_INICIO_A;
  uint16_t AnguloActualB    = ANGULO_SERVO_INICIO_B;
  uint16_t AnguloActualC    = ANGULO_SERVO_INICIO_C;
  bool GiroCompleto         = false;
  bool GiroCompletoA        = false;
  bool GiroCompletoB        = false;
  bool GiroCompletoC        = false;

  while(!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;
       Serial.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;
       Serial.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--;
      }else if(!SentidoAntihorarioC && AnguloActualC == ANGULO_SERVO_INICIO_C){
       GiroCompletoC = true;
       Serial.println("Giro servomotores: Giro del servomotor C completado"); 
      }else{ //Solo se presenta 4 estados para el giro de un servomotor
        AnguloActualC++;
      }
    }
    ServoA.write(AnguloActualA);
    ServoB.write(AnguloActualB);
    ServoC.write(AnguloActualC);
    delayMicroseconds(TiempoPorGrado);
  }
  Serial.println("Giro servomotores: Giro de servomotores completado"); 
}
/*
  Se despliega la plataforma del módulo C para sostener la pila de clamshells
  para el siguiente ciclo de seperación 

*/
void DesplieguePlataforma(){

  bool EnPosicionInicial = false;

  Serial.println("Repliegue: Iniciando repliegue de plataforma");
  while(!EnPosicionInicial){
    EnPosicionInicial = digitalRead(pinOptoacopladorInicialC) == ACTIVO;
    if(EnPosicionInicial){
      Serial.println("Repliegue: Plataforma desplegada completamente");
    }else{
      MoverUnPasoMotorDePasos(pinDireccionC,pinStepC,ANTIHORARIO);
    }
  }
}
/*
  Se abren las tapas de los módulo A y B para dejar caer la pila de clamshells en la
  plataforma del módulo C para iniciar el siguiente ciclo de separación. El giro para
  la apertura el horario.
*/
void RetornoDeTapasDeSujeccion(){
  bool EnPosicionInicial  = false;
  bool EnPosicionInicialA = false;
  bool EnPosicionInicialB = false;

  Serial.println("Retorno: Abriendo tapas");
  while(!EnPosicionInicial){
    EnPosicionInicial = EnPosicionInicialA && EnPosicionInicialB;
    if(!EnPosicionInicialA){
      EnPosicionInicialA = digitalRead(pinOptoacopladorInicialA) == ACTIVO;
      if(EnPosicionInicialA){
        Serial.println("Retorno: Motor de pasos A en posición inicial");
      }else{
        MoverUnPasoMotorDePasos(pinDireccionA,pinStepA,HORARIO);
      }
    }
    if(!EnPosicionInicialB){
      EnPosicionInicialB = digitalRead(pinOptoacopladorInicialB) == ACTIVO;
      if(EnPosicionInicialB){
        Serial.println("Retorno: Motor de pasos B en posición inicial");
      }else{
        MoverUnPasoMotorDePasos(pinDireccionB,pinStepB,HORARIO);
      }
    }
  }
  Serial.println("Retorno: Módulos A y B en posición inicial.");
}


//Los mensajes con Serial.println() debe ser mostrados en la pantalla