/*
  Se realizaron pruebas en el laboratorio con los módulos.
  1. Se cambió los pines de los optoacopladores del módulo C para concordar
  con la lógica del código.
    pinOptoacopladorInicialC = 14; -> 37;
    pinOptoacopladorFinalC = 37; -> 14;
  2. Se estableció la dirección del movimiento según el sentido de giro de los motores
  de pasos para todos los módulos. Los módulos A y B tienen el mismo sentido de giro.
   Las tapas de los módulos A y B se cierran con el sentido antihorario y se abren
   con el horario.
   La plataforma del módulo C se despliega con un giro antihorario y se repliega con
   un giro horario. Esto no se pudo comprobar por una desconexión con el pin de dirección,
   pero se asume que es antihorario pues el pin debería quedar en LOW.
  3. Se estableció los ángulos de inicio y fin para el giro de los servomotores. Esto
  implicó cambiar la lógica de control de estos motores.
    Inicio B: 90°, Fin B:150°
    Inicio C: 0°, Fin C: 120°
*/

#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  = 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);
// 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        = 150;
const uint16_t ANGULO_SERVO_INICIO_C     = 0;
const uint16_t ANGULO_SERVO_FIN_C        = 120;

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();
  LimpiarPantalla();
 
  SujeccionDeClamshell();
  LimpiarPantalla();
 
  ReplieguePlataforma();
  LimpiarPantalla();
 
  GiroServomotores();
  LimpiarPantalla();

  DesplieguePlataforma();
  LimpiarPantalla();

  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(){
  int i=0;
  // Iniciar bus SPI
  SPI.begin(pinCLK, pinMISO , pinMOSI);
  // Configurar pantalla
  tft.begin();
  tft.setRotation(0);
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(3);
  tft.print("Cargando");
  while (i<4) {
    delay(50);
    tft.print(".");
    i++;
  }
  delay(250);
  tft.fillScreen(ILI9341_BLUE);  // Color invertido en pantalla en físico
  delay(250);
  tft.fillScreen(ILI9341_WHITE);
  delay(250);
  tft.fillScreen(ILI9341_BLACK);
 
  // Mensaje inicial
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.setCursor(65, 100);
  tft.println("Pantalla lista");
  delay(500);
  tft.fillScreen(ILI9341_BLACK);

}
/*
  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);
  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
 
  //Motores de paso
  while(!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);
      }
    }
    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);
      }
    }
    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);
      }
    }
  }
  tft.println("Incialización: Motores de pasos en posición incial");
}
/*
  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){
  //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;

  tft.println("Sujección: Cerrando tapas");
  while(!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);
      }
    }
    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);
      }
    }
  }
  tft.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 es horario.
*/
void ReplieguePlataforma(){

  bool EnPosicionFinal = false;

  tft.println("Repliegue: Iniciando repliegue de plataforma");
  while(!EnPosicionFinal){
    EnPosicionFinal = digitalRead(pinOptoacopladorFinalC) == ACTIVO;
    if(EnPosicionFinal){
      tft.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;
       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++;
      }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--;
      }
    }
    ServoA.write(AnguloActualA);
    ServoB.write(AnguloActualB);
    ServoC.write(AnguloActualC);
    delayMicroseconds(TiempoPorGrado);
  }
  tft.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;

  tft.println("Repliegue: Iniciando repliegue de plataforma");
  while(!EnPosicionInicial){
    EnPosicionInicial = digitalRead(pinOptoacopladorInicialC) == ACTIVO;
    if(EnPosicionInicial){
      tft.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 es horario.
*/
void RetornoDeTapasDeSujeccion(){
  bool EnPosicionInicial  = false;
  bool EnPosicionInicialA = false;
  bool EnPosicionInicialB = false;

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

void LimpiarPantalla(){
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextColor(ILI9341_GREEN);
  tft.setCursor(0, 0);
}
//Los mensajes con Serial.println() debe ser mostrados en la pantalla