#include <AccelStepper.h>  // Librería para el control del motor paso a paso
#include <ESP32Servo.h>    // Librería para el control del servomotor

// Configuración del Servomotor
Servo myservo;             // Creando el objeto para el manejo del Servo
const int SERVO_PIN = 13;  // Pin del servomotor
const int angulomin = 15;  // Ángulo mínimo del servomotor
const int angulomax = 135; // Ángulo máximo del servomotor

// Configuración del Motor Paso a Paso
#define A4988_DIR  11      // Pin de dirección de giro del Motor
#define A4988_STEP 10      // Pin para generar el paso del Motor
#define ENABLE     12      // Pin de enable con pull down
#define PRENDIDO LOW
#define APAGADO  HIGH  

// Configuración de la librería AccelStepper para el motor paso a paso
AccelStepper stepper(AccelStepper::DRIVER, A4988_STEP, A4988_DIR);  

// Configuración de los Limit Switch (Interruptores de fin de carrera)
const int switch1pin = 7;  // Pin del limit switch lejos de los clamshells
const int switch2pin = 8;  // Pin del limit switch cerca de los clamshells

// Variables de estado
volatile bool switch1State = false; // Estado del Switch 1 (activo)
volatile bool switch2State = false; // Estado del Switch 2 (activo)

// Funciones de interrupción
void IRAM_ATTR switch1Interrupt() {
  switch1State = true;  // Se activa cuando el interruptor 1 se presiona
}

void IRAM_ATTR switch2Interrupt() {
  switch2State = true;  // Se activa cuando el interruptor 2 se presiona
}

void setup() {
  Serial.begin(115200);
  Serial.println("Programa de prueba");  
  
  pinMode(ENABLE, OUTPUT);
  pinMode(switch1pin, INPUT_PULLUP);
  pinMode(switch2pin, INPUT_PULLUP);

  digitalWrite(ENABLE, PRENDIDO);

  stepper.setMaxSpeed(1000);     // Velocidad máxima
  stepper.setAcceleration(250);  // Aceleración

  myservo.attach(SERVO_PIN);     // Enlazando el pin de control del servo
  myservo.write(angulomin);      // Inicializando el ángulo del Servo

  // Configurar interrupciones
  attachInterrupt(digitalPinToInterrupt(switch1pin), switch1Interrupt, FALLING); // Interrupción en FALLING (cuando se presiona)
  attachInterrupt(digitalPinToInterrupt(switch2pin), switch2Interrupt, FALLING); // Interrupción en FALLING (cuando se presiona)
}

void loop() {
  // Verificar el estado de los switches y tomar acción
  if (switch1State) {               // Switch 1 presionado
    delay(6000);  // Pausa para dar tiempo al sistema a responder
    digitalWrite(A4988_DIR, HIGH); // Sentido ANTIHORARIO
    while (!switch2State) {        // Continuar hasta que se active el Switch 2
      stepper.setSpeed(-500);      // Movimiento en sentido antihorario
      stepper.runSpeed();
    }
    stepper.stop();                // Detener el motor al alcanzar Switch 2
    myservo.write(angulomin);      // Mover el servomotor a su posición mínima
    switch1State = false;          // Resetear el estado del switch1
    switch2State = false;          // Resetear el estado del switch2
  }

  else if (switch2State) {        // Switch 2 presionado
    delay(16000);                  // Pausa para soportar los clamshells
    digitalWrite(A4988_DIR, LOW);  // Sentido HORARIO
    while (!switch1State) {        // Continuar hasta que se active el Switch 1
      stepper.setSpeed(500);       // Movimiento en sentido horario
      stepper.runSpeed();
    }
    stepper.stop();                // Detener el motor al alcanzar Switch 1
    myservo.write(angulomax);      // Mover el servomotor a su posición máxima
    switch1State = false;          // Resetear el estado del switch1
    switch2State = false;          // Resetear el estado del switch2
  }
}