#include <ESP32Servo.h>
#include <AccelStepper.h>
  //EL DRIVER A4988 SE  MANEJA CON LOGICA INVERTIDA 

  // Pines de los limit switches y señal de seguridad
  const int limitSwitch1Pin = 7;  // limit de atrás SW1
  const int limitSwitch2Pin = 8;  // limit 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
  // Variables para guardar el estado anterior de cada switch
  int pasosPorRevolucion = 200;
  int velocidad;
  int angulo=90 ; //inicializar servo
  // 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)
  Servo myServo;
  // Variables de posicionamiento del servo
  const int SignalServo = 13;
  const uint8_t sw0 = 5;
  const uint8_t sw1 = 16;
  int servo_end = 0;
  //////// Función de calibración del posicionamiento inicial para motor y servomotor ////////
  AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

  void posicion_inicial() {
    // Configura servo inicial
    angulo = 90;
    myServo.write(angulo);
    while (digitalRead(limitSwitch1Pin) == HIGH) {  // Mientras SW1 no está presionado
      digitalWrite(dirPin, HIGH);//retrocede
      digitalWrite(stepPin, HIGH);  // Enviar un paso al motor
      delay(1);  // Ajusta la velocidad para 200 pasos por segundo
      digitalWrite(stepPin, LOW);
      delay(1);  // Esperar antes del siguiente paso
    }
    while (digitalRead(limitSwitch2Pin) == LOW) {  // Si el motor está presionando SW2
      digitalWrite(dirPin, LOW);//retrocede
      digitalWrite(stepPin, HIGH);  // Enviar un paso al motor
      delay(1);  // Ajusta la velocidad para 200 pasos por segundo
      digitalWrite(stepPin, LOW);
    delay(1);  // Esperar antes del siguiente paso
    }
    digitalWrite(enablePin, HIGH);
    digitalWrite(stepPin, LOW);  // Detener motor
    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");
    servo_end = 1;
    angulo = 90;
    myServo.write(angulo);
  }
  void setup() {
    // Configurar pines del motor
    pinMode(stepPin, OUTPUT);
    pinMode(dirPin, OUTPUT);
    pinMode(enablePin, OUTPUT);
    // Configurar pines de los limit switches y señal de seguridad
    pinMode(limitSwitch1Pin, INPUT_PULLUP);
    pinMode(limitSwitch2Pin, INPUT_PULLUP);
    pinMode(servo_enable, INPUT_PULLUP);
    pinMode(enable_sistema, INPUT_PULLUP);

    Serial.begin(9600);
    myServo.attach(SignalServo);
    while (digitalRead(enable_sistema) == LOW) {
    }
    if (digitalRead(enable_sistema) == HIGH) { // señal ENABLE del sistema en alta para poder realizar la prueba de hardware
      posicion_inicial();  // Empieza siempre aquí
    }
  }
  void loop() {
    while (digitalRead(enable_sistema) == HIGH) {
      while (digitalRead(limitSwitch2Pin) == HIGH) {  // Mientras no esté presionado
        Serial.println("Avanzando...(entra al bucle1)");
        
        stepper.setSpeed(pasosPorRevolucion);
        stepper.runSpeed();
      }
      if (digitalRead(limitSwitch2Pin) == LOW) {  // Verifica si el limit switch 2 fue presionado
        digitalWrite(stepPin, LOW);  // Detener el motor
        servo_motor();  // Llama función del servo

        if (servo_end == 1) {  // Fin del movimiento del servo
          while (digitalRead(limitSwitch1Pin) == HIGH) {  // Mientras SW1 no esté presionado
            digitalWrite(dirPin, HIGH);  // Dirección hacia atrás
            digitalWrite(stepPin, HIGH); // Enviar paso
            delay(1);  // Velocidad de 200 pasos por segundo
            digitalWrite(stepPin, LOW); // Detener paso
            delay(1);  // Esperar antes del siguiente paso
            if (digitalRead(limitSwitch1Pin) == LOW) {  // Verifica si el limit switch de seguridad fue presionado
              break;
            }
          }
        }
      }
    }
  }