#include<ESP32Servo.h>
#include<math.h>
Servo myservo;

//Módulo listo para simulación en Wokwi

#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
#define pinLimitSwitchInicio 8            //Pin para recibir la señal del sensor de fin de carrera
#define pinLimitSwitchFin 7
#define pinServomotor 13
#define pinBotonDeEmergencia 4

#define ACTIVO HIGH                       //Constante para definir el estado del sensor de fin de carrera
#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        //Ángulo de giro del servomotor para separar los clamshells
#define ANGULO_SERVOMOTOR_INICIO 90       //Ángulo inicial del servomotor
#define TIEMPO_DE_ESPERA_PARA_GIRAR 1000  //Tiempo hasta que el servomotor gire luego de que el motor de pasos frene

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 separa un clamshell
uint8_t FinServomotor = 0;                //Determina si se terminó de girar el servomotor
uint8_t ServomotorListoParaRotar = 0;

uint8_t Inicio;                       //Determina si el programa está comenzando
uint8_t Inicializacion;               //Determina si el programa se está inicializando
uint8_t Stop;

bool InicializacionCompletada;
bool PosicionalInicial;
bool PosicionFinalLlegada;
bool PosicionFinalSalida;
bool PoscionalIncialFinProceso;
bool IniciarMovimientoServomotor;

uint8_t PlataformaReplegada;
unsigned int ContadorDeltaAngulo = 1;
unsigned int RevolucionesPorSegundo = 0.1;
unsigned int DeltaDeTiempoPorGrado; // (int)25000/(27*0.1); En us
unsigned int AnguloServomotor;

void setup() {
  Serial.begin(115200);
  //Definición de pines
  pinMode(pinA4988_DIR,OUTPUT);
  pinMode(pinA4988_STEP,OUTPUT);
  pinMode(pinA4988_ENABLE,OUTPUT);
  
  digitalWrite(pinA4988_ENABLE,LOW); //Se activa el motor

  myservo.attach(pinServomotor);

  pinMode(pinLimitSwitchInicio,INPUT_PULLDOWN);
  pinMode(pinLimitSwitchFin,INPUT_PULLDOWN);
  pinMode(pinBotonDeEmergencia,INPUT_PULLDOWN);

  InicializarVariables();
  
  Serial.println("Ingrese la velocidad del servomotor");
  DeltaDeTiempoPorGrado = LeerVelocidadServomotor();
}

void loop() {

  if(digitalRead(pinBotonDeEmergencia) == ACTIVO){
    delay(15);
    if(digitalRead(pinBotonDeEmergencia) == ACTIVO){ //Para el efecto rebote
      Stop = 1;
      MovimientoMotorDePasos = 0;
      MoverServomotor = 0;
      Serial.println("Deteniendo el proceso");
      while(digitalRead(pinBotonDeEmergencia) == ACTIVO); //Se espera a soltar el botón solo para la simulación 
    }
  }

  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.");
  }
  
  PlataformaReplegada = LeerConfirmacionPlataforma();
  
  InicializacionCompletada = MovimientoMotorDePasos && digitalRead(pinLimitSwitchInicio) == ACTIVO && Inicializacion && !Stop;
  PosicionalInicial = !MovimientoMotorDePasos && digitalRead(pinLimitSwitchInicio) == ACTIVO && !Inicializacion && !Stop;
  PosicionFinalLlegada = MovimientoMotorDePasos && digitalRead(pinLimitSwitchFin) == ACTIVO && !FinServomotor && !Inicializacion && !Stop;
  PosicionFinalSalida = !MovimientoMotorDePasos && digitalRead(pinLimitSwitchFin) == ACTIVO && FinServomotor && !Inicializacion && !Stop;
  PoscionalIncialFinProceso = MovimientoMotorDePasos && digitalRead(pinLimitSwitchInicio) == 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.");
  }
  if(PosicionalInicial){
    MovimientoMotorDePasos = 1;
    SentidoMotorDePasos = ANTIHORARIO; //Adelante antihorario
    //while(digitalRead(pinLimitSwitchInicio) == ACTIVO); //Se espera a soltar los botones, solo para la simulación
    Serial.println("Proceso: Cerrando las tapas");
  }
  //
  if(PosicionFinalLlegada){
    MovimientoMotorDePasos = 0;
    ServomotorListoParaRotar = 1;
    //while(digitalRead(pinLimitSwitchFin) == ACTIVO);
    Serial.println("Proceso: Esperando confirmación de repliegue de la plataforma(Ingrese P)");
  }
  //
  if(IniciarMovimientoServomotor){
    MoverServomotor = 1;
    ContadorDeltaAngulo = 1;
    ServomotorListoParaRotar = 0;
  }
  //
  if(PosicionFinalSalida){ //Se debería además analizar si la plataforma ya en posición para abrir las tapas
    MovimientoMotorDePasos = 1;
    SentidoMotorDePasos = HORARIO; //Atras horario
    //while(digitalRead(pinLimitSwitchFin) == ACTIVO);
    Serial.println("Proceso: Abriendo las tapas");
  }
  if(PoscionalIncialFinProceso){
    MovimientoMotorDePasos = 0;
    ProcesoCompletado = 1;
    FinServomotor = 0;
    //while(digitalRead(pinLimitSwitchInicio) == ACTIVO);
    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 20 ms, 10 ms en alta y 10 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'){
        ProcesoCompletado = 0;
        Stop = 0;
        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);
}

uint8_t LeerConfirmacionPlataforma(){
  char Caracter;

  if(Serial.available()>0){
    Caracter = Serial.read();
    if(Caracter == 'P'){
      return 1;
    }
  }
  return 0;
}

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

  char Caracter; 
  char ArregloParteEntera[8];
  char ArregloParteDecimal[8];
  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)(int)25000/(27*RevolucionesPorSegundo);
}

void LimpiarBuffer(){
  while(Serial.available()>0){
    Serial.read();
  }
}
//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 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