/*
  Se calibró los ángulos de los servomotores.
  Módulo A:
    Ángulo inicial: 90 -> 95
  Módulo B:
    Ángulo inicial: 90 -> 85
  Módulo C:
    Ángulo inicial: 0 -> 76
    Ángulo final: 120 -> 180
*/

#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     = 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 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


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

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

  switch (EstadoModulos){
    case INICIALIZACION:
      if (!limpieza[0]){
        LimpiarPantalla();
        limpieza[0] = true;
        limpieza[4] = false;
      }
      InicializacionPosicionesModulos();
      break;
    case SUJECION:
      if (!limpieza[1]){
        LimpiarPantalla();
        limpieza[1] = true;
        limpieza[0] = false;
      }
      SujecionDeClamshell();
      break;
    case REPLIEGUE_PLATAFORMA:
      if (!limpieza[2]){
        LimpiarPantalla();
        limpieza[2] = true;
        limpieza[1] = false;
      }
      ReplieguePlataforma();
      break;
    case GIRO_SERVOMOTORES:
      if (!limpieza[3]){
        LimpiarPantalla();
        limpieza[3] = true;
        limpieza[2] = false;
      }
      GiroServomotores();
      break;
    case DESPLIEGUE_PLATAFORMA:
      if (!limpieza[4]){
        LimpiarPantalla();
        limpieza[4] = true;
        limpieza[3] = false;
      }
      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(){
  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 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);
}