#include <ESP32Servo.h>
#include <AccelStepper.h>
// Pines de los limit switches y señal de seguridad
const int limitSwitch1Pin = 7;  // limit switch de atrás SW1
const int limitSwitch2Pin = 8;  // limit switch de adelante SW2
int safetyActivated = 0;
bool enable = false;
const int enable_sistema = 37;  // Para simular enable_sistema SW3
const int servo_enable = 6;     // SW4
// Pines del A4988 (Control del motor paso a paso)
const int stepPin = 10;  // Pin para el paso (STEP)
const int dirPin = 11;   // Pin para la dirección (DIR)
const int enablePin = 12; // Pin para habilitar el motor (ENABLE)
#define MotorInterfaceType 1 

// Configuracion del ServoMotor
Servo myServo;
int angulo=90 ; //inicializar servo
// Variables de posicionamiento del servo
const int SignalServo = 13;
const uint8_t sw0 = 5;
const uint8_t sw1 = 16;
int reinicio=0; //bandera para reiniciar el sistema del paquete B
int stopServo = 0; //bandera para inicializar el movimiento del servo

//Variables para medir el tiempo de lógica de seguridad


//Configuracion del Motor paso a paso
AccelStepper motor =AccelStepper(MotorInterfaceType, stepPin, dirPin); 


//////// Función de calibración del posicionamiento inicial para motor y servomotor ////////
void posicion_inicial() {
  // Configura servo en una posicion inicial
  Serial.println("se inicio posicionamiento");
  reinicio=0;
  angulo = 90;
  myServo.write(angulo);
  //Calibracion del motor paso a paso 
  //El motor desde cualquier posicion retrocede hasta activar el limit Switch 1 y avanza la distancia mínima hasta desactivarlo
  if(digitalRead(limitSwitch1Pin) == HIGH && digitalRead(limitSwitch2Pin)==HIGH || digitalRead(limitSwitch1Pin) == HIGH && digitalRead(limitSwitch2Pin)==LOW || digitalRead(limitSwitch1Pin) == LOW && digitalRead(limitSwitch2Pin)==HIGH ){
    while (1) {  // Mientras SW1 no está presionado
      motor.setSpeed(100); //retrocede
      motor.runSpeed();
	  if(digitalRead(limitSwitch1Pin)==LOW){
	  break;
	  }
    }
  
    while (1) {  // Llegó a presionar SW1 y avanza hasta justo después de dejar de presionarlo (posicion inicial)
      motor.setSpeed(-100); //avanza
      motor.runSpeed();
	   if(digitalRead(limitSwitch1Pin)==HIGH){
	  break;
	  }
    }
     
  }
  else{
    //CASO DE ERROR DURANTE LA CALIBRACIÓN INICIAL 
	safetyActivated=1;
    digitalWrite(enablePin, HIGH); //motor apagado
	
  }

  digitalWrite(enablePin, HIGH);
  Serial.println("Se termino el posicionamiento....");
}
//////// Función del movimiento del servomotor durante el proceso de separación de clamshells ////////
void servo_motor() {
  Serial.println("Inicio de movimiento del servo");
  for (int angulo = 90; angulo <= 150; angulo++) {
    myServo.write(angulo);
    delay(2);
  }
  delay(1000);
  for (int angulo = 150; angulo >= 90; angulo--) {
    myServo.write(angulo);
    delay(2);
  }
  delay(1000);
  Serial.println("Se terminó el movimiento del servo");
  stopServo = 1;
  angulo = 90;
  myServo.write(angulo);
}
void limpiarBuffer() {
  while (Serial.available() > 0) {
    Serial.read(); // Leer y descartar los datos en el buffer
  }
}

void setup() {
  
  // Configurar pines de los limit switches y señal de seguridad
  pinMode(limitSwitch1Pin, INPUT_PULLUP);
  pinMode(limitSwitch2Pin, INPUT_PULLUP);
  //Pines que funcionan para los sliders en la simulacion
  pinMode(servo_enable, INPUT_PULLUP);
  pinMode(enable_sistema, INPUT_PULLUP);

  //Configuración de velocidad
  motor.setMaxSpeed(100); 

  Serial.begin(9600);
  myServo.attach(SignalServo);
  while (digitalRead(enable_sistema) == HIGH) {
  }
  if (digitalRead(enable_sistema) ==LOW) { // señal ENABLE del sistema en alta para poder realizar la prueba de hardware
    posicion_inicial();  // Empieza siempre aquí
  }
}
void loop() {
    if (digitalRead(enable_sistema) == LOW && safetyActivated==0 && reinicio==0) { //espera que la señal de paquete D y que no entró en error durante calibración
     digitalWrite(enablePin, LOW);//prende el motor nema
	 
     while(1){
      if (Serial.available()> 0){
              String letra=Serial.readStringUntil('\n');
              if(letra.equals("REINICIO")){
                reinicio=1;
                Serial.flush();//LIMPIAMOS BUFFER
                limpiarBuffer();
                break;
                
              }
            }
      motor.setSpeed(-100); //avanzando
      motor.runSpeed();
      if(digitalRead(limitSwitch2Pin) == LOW){
        break;
      }
	  else if(digitalRead(limitSwitch2Pin) == LOW && digitalRead(limitSwitch1Pin) == LOW){ //CASO DE ERROR
	  safetyActivated=1;
	  break;
	  }
	}
    if (digitalRead(limitSwitch2Pin) == LOW && safetyActivated==0) {  // Verifica si el limit switch 2 fue presionado
  
      servo_motor();  // Llama función del servo

      if (stopServo == 1 && digitalRead(limitSwitch1Pin) == HIGH && digitalRead(limitSwitch2Pin)==LOW) {  // Fin del movimiento del servo
        stopServo = 0;

         while(1){
            if (Serial.available()> 0){
              String letra=Serial.readStringUntil('\n');
              if(letra.equals("reinicio")){
                Serial.println("reiniciooooo");//LIMPIAMOS BUFFER
                reinicio=1;
                Serial.flush();
                limpiarBuffer();
                break;
              }
            }
         motor.setSpeed(100); //retrocede
         motor.runSpeed();
          if (digitalRead(limitSwitch1Pin) == LOW) {  // Verifica si el limit switch de seguridad fue presionado
            break;
          }
		  else{ //CASO DE ERROR
		  safetyActivated=1; 
		  break;
		  }
		}
      }
      else if (digitalRead(limitSwitch1Pin) == LOW && digitalRead(limitSwitch2Pin)==LOW) {
        safetyActivated=1;
      }
    }

    if(safetyActivated==1){
    digitalWrite(enablePin, HIGH); //detiene el motor en la posición en la que se encuentre
    myServo.write(90); // se reinicia la posición del servo
    }
  }
  else if(reinicio==1 &&safetyActivated==0){ 
    reinicio=0; 
    posicion_inicial(); 
    
  }
}