//Módulo listo para implementación individual

#include<ESP32Servo.h>                   //Librería para el control del servomotor
#include<math.h>                         //Librería para el cálculo de la velocidad del servomotor

Servo myservo;                            //Objeto para controlar el servomotor

#define pinA4988_DIR 11                   //Pin para cambiar la dirección del motor de pasos
#define pinA4988_STEP 10                  //Pin para generar el movimiento del motor de pasos
#define pinA4988_ENABLE 12                //Pin para activar el controlador A4988 que controla el motor de pasos
#define pinOptoacopladorInicio 8          //Pin para recibir la señal del optoacoplador en la posición inicial
#define pinOptoacopladorFin 7             //Pin para recibir la señal del optoacoplador en la posición final
#define pinServomotor 13                  //Pin para controlar el servomotor

#define ACTIVO HIGH                       //Constante para definir el estado del optoacoplador
#define HORARIO HIGH                      //Constante para la dirección del motor de pasos
#define ANTIHORARIO LOW                   //Constante para la dirección del motor de pasos

#define ANGULO_SERVOMOTOR_FINAL 30        //Constante para el ángulo final del sevomotor luego del giro
#define ANGULO_SERVOMOTOR_INICIO 90       //Constante para el ángulo inicial del servomotor

uint8_t MovimientoMotorDePasos = 0;       //Determina si se debe mover el motor de pasos
bool SentidoMotorDePasos = HORARIO;       //Sentido de giro del motro motro de pasos
uint8_t MoverServomotor = 0;              //Detemina si se debe mover el servomotor
uint8_t ProcesoCompletado = 0;            //Determina si se terminó de separar un clamshell
uint8_t FinServomotor = 0;                //Determina si se terminó de girar el servomotor
uint8_t ServomotorListoParaRotar = 0;     //Determina si ya se puede rotar el servomotor, plataforma del módulo C ya replegada

uint8_t Inicio;                           //Determina si el programa está corriendo por primera vez
bool BotonDeEmergencia;                   //Determina si se presionó el botón de emergencia (Comando por consola)
bool PlataformaReplegada;                 //Determina si se confirmó el repliegue de la plataforma del módulo C

uint8_t Inicializacion;                   //Determina si el programa está inicializando los motores
uint8_t Stop;                             //Determina si el programa está deteniendo los motores

bool InicializacionCompletada;            //Estado del módulo, los motores están llegaron a la posición de inicial
bool PosicionalInicial;                   //Estado del módulo, el motor de pasos puede empezar a cerrar las tapas
bool PosicionFinalLlegada;                //Estado del módulo, el motor de pasos acaba de cerrar completamente las tapas
bool PosicionFinalSalida;                 //Estado del módulo, el servomotor terminó de girar, se puede proceder abrir las tapas
bool PoscionalIncialFinProceso;           //Estado del módulo, se regresó al inicio y se terminó se separar una clamshell
bool IniciarMovimientoServomotor;         //Estado del módulo, se puede girar el servomotor luego de confimar la posición de la plataforma

unsigned int ContadorDeltaAngulo;         //Cuenta cuántos grados ha girado el servomotor en total
unsigned int RevolucionesPorSegundo ;     //Guarda la velocidad del servomotor
unsigned int DeltaDeTiempoPorGrado;       //Determina cuánto tiempo se espera entre movimiento grado a grado del servomotor
unsigned int AnguloServomotor;            //Determina el ángulo al que debe girar el servomotor

void setup() {
  //Inicialización puerto serial
  Serial.begin(115200);     

  //Definición de pines
  pinMode(pinA4988_DIR,OUTPUT);
  pinMode(pinA4988_STEP,OUTPUT);
  pinMode(pinA4988_ENABLE,OUTPUT);
  pinMode(pinOptoacopladorInicio,INPUT_PULLDOWN);
  pinMode(pinOptoacopladorFin,INPUT_PULLDOWN);

  //Activación del motor
  digitalWrite(pinA4988_ENABLE,LOW); 

  //Configuración del servomotor
  myservo.attach(pinServomotor);

  //Inicialización del programa
  InicializarVariables();
  Serial.println("Ingrese la velocidad del servomotor");
  DeltaDeTiempoPorGrado = LeerVelocidadServomotor();
}

void loop() {

  LeerComandos(&BotonDeEmergencia,&PlataformaReplegada);

  if(BotonDeEmergencia){
    Stop = 1;
    MovimientoMotorDePasos = 0;
    MoverServomotor = 0;
    Serial.println("Deteniendo el proceso");
  }

  if(Inicio){  //Se mueve las tapas al inicio
    myservo.write(ANGULO_SERVOMOTOR_INICIO);

    MovimientoMotorDePasos = 1;
    SentidoMotorDePasos = HORARIO;
    
    Inicio = 0;
    Inicializacion = 1;

    Serial.println("Proceso: Inicializando las tapas.");
  }
    
  InicializacionCompletada = MovimientoMotorDePasos && digitalRead(pinOptoacopladorInicio) == ACTIVO && Inicializacion && !Stop;
  PosicionalInicial = !MovimientoMotorDePasos && digitalRead(pinOptoacopladorInicio) == ACTIVO && !Inicializacion && !Stop;
  PosicionFinalLlegada = MovimientoMotorDePasos && digitalRead(pinOptoacopladorFin) == ACTIVO && !FinServomotor && !Inicializacion && !Stop;
  PosicionFinalSalida = !MovimientoMotorDePasos && digitalRead(pinOptoacopladorFin) == ACTIVO && FinServomotor && !Inicializacion && !Stop;
  PoscionalIncialFinProceso = MovimientoMotorDePasos && digitalRead(pinOptoacopladorInicio) == ACTIVO && FinServomotor && !Inicializacion && !Stop;
  IniciarMovimientoServomotor = ServomotorListoParaRotar && PlataformaReplegada && !Inicializacion && !Stop;


  if(InicializacionCompletada){ //Se llegó a la posición inicial
      Inicializacion = 0;
      MovimientoMotorDePasos = 0;
    Serial.println("Proceso: Se completo la inicialización de las tapas.");

  }else if(PosicionalInicial){
    MovimientoMotorDePasos = 1;
    SentidoMotorDePasos = ANTIHORARIO; //Adelante antihorario
    Serial.println("Proceso: Cerrando las tapas");

  }else if(PosicionFinalLlegada){
    MovimientoMotorDePasos = 0;
    ServomotorListoParaRotar = 1;
    Serial.println("Proceso: Esperando confirmación de repliegue de la plataforma(Ingrese P)");

  }else if(IniciarMovimientoServomotor){
    MoverServomotor = 1;
    ContadorDeltaAngulo = 1;
    ServomotorListoParaRotar = 0;
    
  }else if(PosicionFinalSalida){ //Se debe además analizar si la plataforma ya en posición para abrir las tapas
    MovimientoMotorDePasos = 1;
    SentidoMotorDePasos = HORARIO; //Atras horario
    Serial.println("Proceso: Abriendo las tapas");

  }else if(PoscionalIncialFinProceso){
    MovimientoMotorDePasos = 0;
    ProcesoCompletado = 1;
    Serial.println("Proceso: Se completó abrir las tapas");
  }

  if(MovimientoMotorDePasos){
    MoverUnPasoMotorDePasos(SentidoMotorDePasos);
  }

  if(MoverServomotor){
    if(ContadorDeltaAngulo<=ANGULO_SERVOMOTOR_INICIO-ANGULO_SERVOMOTOR_FINAL){
      MoverServomotorUnGradoAngulo(HORARIO,DeltaDeTiempoPorGrado);
      ContadorDeltaAngulo++;
    }else if(ContadorDeltaAngulo<=2*(ANGULO_SERVOMOTOR_INICIO-ANGULO_SERVOMOTOR_FINAL)){
      MoverServomotorUnGradoAngulo(ANTIHORARIO,DeltaDeTiempoPorGrado);
      ContadorDeltaAngulo++;
    }else{
      Serial.println("Proceso: Se completó girar el servomotor");
      MoverServomotor = 0;
      FinServomotor = 1;  
    }
  }

  if(ProcesoCompletado){
    MensajeProcesoCompletado();
  }
  if(Stop){
    MensajeBotonDeEmergencia();
  }
}

void MoverUnPasoMotorDePasos(bool Sentido){
  digitalWrite(pinA4988_DIR,Sentido); //Se configura el sentido
  //Se mueve 1 paso cada 10 ms, 5 ms en alta y 5 ms en baja
  digitalWrite(pinA4988_STEP,HIGH);
  delay(5);
  digitalWrite(pinA4988_STEP,LOW);
  delay(5);
}

void MensajeProcesoCompletado(){
  Serial.println("Proceso completado");
  Serial.println("¿Continuar con otro ciclo?(Y)");
  while(1){
    delay(500);
    if(Serial.available()>0){
      if(Serial.read() == 'Y'){
        InicializarVariables();
        Inicio = 0; //No se inicializa nuevamente el proceso, ya están en la posición inicial
        break;
      }else{
        LimpiarBuffer();
      }
    }
  }
}

void MensajeBotonDeEmergencia(){
  Serial.println("Proceso detenido");
  Serial.println("¿Reiniciar?(Y)");
  while(1){
    delay(500);
    if(Serial.available()>0){
      if(Serial.read() == 'Y'){
        InicializarVariables();
        break;
      }else{
        LimpiarBuffer();
      }
    }
  }
}

void InicializarVariables(){
  ProcesoCompletado = 0;
  Stop = 0;
  Inicio = 1;
  FinServomotor = 0;
  AnguloServomotor = ANGULO_SERVOMOTOR_INICIO;
}

void MoverServomotorUnGradoAngulo(bool Sentido, unsigned int TiempoDeEspera){
  if(Sentido == HORARIO){
    AnguloServomotor--;
    myservo.write(AnguloServomotor);
  }else if(Sentido == ANTIHORARIO){
    AnguloServomotor++;
    myservo.write(AnguloServomotor);
  }
  delayMicroseconds(TiempoDeEspera);
}

int LeerVelocidadServomotor(){
  uint8_t LeyendoParteEntera = 1;
  uint8_t Contador = 0;
  uint8_t Inicio = 1;
  uint8_t CantidadCifras = 0;

  char Caracter; 
  char ArregloParteEntera[8] = {0};
  char ArregloParteDecimal[8] = {0};
  int ParteEntera;
  int ParteDecimal;
  double RevolucionesPorSegundo;

  while(1){
    if(Serial.available()>0){
      Caracter = Serial.read();
      if(Inicio && (Caracter == '\n' || Caracter == '.' || Caracter == ',')){ //Solo se ingresó ENTER, coma o punto al inicio
        Serial.println("Error: Ingrese un número válido");
        LimpiarBuffer();
      }else{
        Inicio = 0;
        if(Caracter == '\n' || CantidadCifras > 7){ //Se terminó la lectura y los caracteres son válidos
          ParteEntera = atoi(ArregloParteEntera);
          ParteDecimal = atoi(ArregloParteDecimal);
          
          if(ParteDecimal != 0){
            RevolucionesPorSegundo = ParteEntera + ParteDecimal/pow(10,((int)log10(ParteDecimal)+1));
          }else{
            RevolucionesPorSegundo = ParteEntera;
          }
        
          Serial.print("RPS: ");
          Serial.println(RevolucionesPorSegundo,2);
          break;
        }else if(48 <= Caracter && Caracter <= 57){
          if(LeyendoParteEntera){
            ArregloParteEntera[Contador] = Caracter;
          }else{
            ArregloParteDecimal[Contador] = Caracter;
          }
          Contador++;
          CantidadCifras++;
        }else if(Caracter == '.' || Caracter == ','){
          LeyendoParteEntera = 0;
          Contador = 0;
        }else{
          Serial.println("Error: Ingrese un número valido");
          Inicio = 1;
          Contador = 0;
          LeyendoParteEntera = 1;

          ArregloParteEntera[Contador] = {0};
          ArregloParteDecimal[Contador] = {0};
          LimpiarBuffer();
        }
      }
    }
    delay(15);
  }
  return (int)(25000/(9*RevolucionesPorSegundo));
}

void LimpiarBuffer(){
  while(Serial.available()>0){
    Serial.read();
  }
}

void LeerComandos(bool *BotonDeEmergencia,bool *PlataformaReplegada){

  char Caracter;

  //Estas variables se mantiene en true solo por un ciclo de la función loop() para evitar activar los comandos en puntos no deseados
  *BotonDeEmergencia = false;
  *PlataformaReplegada = false;

  if(Serial.available()>0){
    Caracter = Serial.read();
    switch(Caracter){
      case 'E':
        Caracter = Serial.read();
        if(Caracter == '\n'){
          *BotonDeEmergencia = true;
        }
        break;
      case 'P':
        Caracter = Serial.read();
        if(Caracter == '\n'){
          *PlataformaReplegada = true;
        }
        break;
      default:
        break;
    }
  }
}
//Todas las conexiones van a solo 1 procesador
//Switch pullup


//Qué señal generan los limit switch
//Cuánto tiempo
//Qué pines son los limit switch


//Se cambió el botón de emergencia por un comando por la consola
//Se simula la recepción de la información sobre la plataforma del módulo C con un comando por la consola
//La velocidad del servomotor debe estar entre 0.1 y 0.25 revoluciones por segundo para garantizar una ejecución segura