#include <Stepper.h>
#include<ESP32Servo.h>
//200 pasos por revolucion ya vienen ocnfigurados MOTOR
// Pines para limit switches y señal de seguridad
const int limitSwitch1Pin = 7;//limit de adelante SW1
const int limitSwitch2Pin = 8;//limit de atras
int safetyActivated = 0;
bool enable = false;
const int enable_sistema=9;//para simular enable_sistema SW3
const int servo_enable=6;//SW4
// Variables para guardar el estado anterior de cada switch

int lastStateSwitch1 ;
int lastStateSwitch2 ;
int lastStateEnableSistema ;
int lastStateServoEnable ;

int currentStateSwitch1;
int currentStateSwitch2 ;
int currentStateEnableSistema ;
int currentStateServoEnable ;

int pasosPorRevolucion=200;
Stepper motor(pasosPorRevolucion, 10, 11, 12, 13);

Servo myServo;
//por mientras se usara esta velocidad debido a que velocidad_0 necesita un maestro
int velocidad;
int angulo=0;
//La direccion que seria este paquete en este caso como es paquete B sera direccion 0x02
const int SignalServo = 15;
const uint8_t sw0 = 5;
const uint8_t sw1 = 16;
//el sw1 representara el posicionamiento del servomotor(despues del retroceso del Stepper)
//el sw2 representara el sensor de fin de carrera encargado de coordinar el movimiento

////////////////////////////////////////
void setup() {
  motor.setSpeed(10);
  // 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);
  motor.step(0);
  //servo
  myServo.write(angulo);
  myServo.attach(SignalServo);
  lastStateSwitch1 = digitalRead(limitSwitch1Pin);
  lastStateSwitch2 = digitalRead(limitSwitch2Pin);
  lastStateEnableSistema = digitalRead(enable_sistema);
  lastStateServoEnable = digitalRead(servo_enable);
  posicion_inicial();//empieza siempre aqui 
}

void loop() {
   currentStateSwitch1 = digitalRead(limitSwitch1Pin);
   currentStateSwitch2 = digitalRead(limitSwitch2Pin);
   currentStateEnableSistema = digitalRead(enable_sistema);
   currentStateServoEnable = digitalRead(servo_enable);

  // Verificar si el estado del limitSwitch1 ha cambiado
  if (currentStateSwitch1 != lastStateSwitch1 || currentStateSwitch2 != lastStateSwitch2 ||currentStateEnableSistema != lastStateEnableSistema||currentStateServoEnable != currentStateServoEnable ) {
    Serial.println("limitswitch1(adelante): ");
    delay(1000);
    Serial.println(digitalRead(limitSwitch1Pin));
    Serial.println("limitswitch2(atras): ");
    delay(1000);
    Serial.println(digitalRead(limitSwitch2Pin));
    Serial.println("enable_sistema: ");
    delay(1000);
    Serial.println(digitalRead(enable_sistema));
    Serial.println("servo_enable: ");
    delay(1000);
    Serial.println(digitalRead(servo_enable));
  }
  

  if (safetyActivated==1) {
    Serial.println("Señal de seguridad activada. Motor apagado.");
     delay(5000);
    motor.step(0);
  }else if(safetyActivated==0 && digitalRead(enable_sistema)==LOW ){
    while(digitalRead(limitSwitch1Pin)==HIGH){//mientrsa no este presionado avanza 
      Serial.println("entro al bucle 1, avanza");
      delay(1000);
      motor.step(pasosPorRevolucion);
    }
      if(digitalRead(limitSwitch1Pin)== LOW){  //Se comprobaria que el limit switch fue presionado 
        motor.step(0);//se detiene
        servo_motor(); //llama funcion del servo
        if(banderita==1){//el servo termino de moverse 
          while(digitalRead(limitSwitch2Pin)==HIGH){//MIENTRAS EL SW2 NO esta precionado retrocede
              Serial.println("entro al bucle 2, retrocede");
              delay(1000);
              motor.step(-pasosPorRevolucion);
              if(digitalRead(limitSwitch1Pin)== LOW ){  //Se comprobaria que el limit switch SIGUE presionado MIENTRAS EL EN SERVO ES PRESIONADO
              safetyActivated=1; 
              }
          }
          if(safetyActivated==1){//si no se ha mmovido 
            Serial.println("Señal de seguridad activada. Motor apagado.");
            delay(5000);
            motor.step(0);
            posicion_inicial();
          }
        }
      }
    
  
}

void posicion_inicial(){
  //configura servo inicial
  angulo = 90;
  myServo.write(angulo);
  enable_3=0;
  //configura motor inicial
  if(digitalRead(limitSwitch2Pin)==LOW){//el sw de abajo esta presionado
    while(digitalRead(limitSwitch2Pin)==LOW){ // ESTA PRESIONADO
      motor.step(pasosPorRevolucion);
    }
    motor.step(0);
  }else{//RETROCEDE Hasta presionarlo
    while(digitalRead(limitSwitch2Pin)==HIGH){//mientras no esta presionado
      motor.step(-pasosPorRevolucion);
    }
    while(digitalRead(limitSwitch2Pin)==LOW){ // ESTA PRESIONADO
      motor.step(pasosPorRevolucion);
    }
    motor.step(0);
  }
}
/*
void servo_motor(){
    // Solo mueve el servo si ambos switch están en LOW
    if (digitalRead(limitSwitch1Pin) == LOW && currSw1 == LOW) {
      enable_0 = 1;
      enable_1 = 1;  
      if (enable_0 && enable_1) {
        Serial.println("Inicio de movimiento");
       
        for (int angulo = 90; angulo <= 150; angulo++) {
          myServo.write(angulo);
          //delay(velocidad_0)
          delay(velocidad);
        }
        delay(1000);
        for (int angulo = 150; angulo >= 90; angulo--) {
          myServo.write(angulo);
          //delay(velocidad_0)
          delay(velocidad);
        }
        delay(1000);
       
        // Señal de que el movimiento está completo
        enable_3 = 1;
        enable_0 = 0;
        enable_1 = 0;
        Serial.println("Se terminó un movimiento");
        banderita=1;
        delay(400);
      }
    }
    else {
      // Si alguno de los interruptores no está activado, reestablece el ángulo a 90
      angulo = 90;
      myServo.write(angulo);
      enable_3=0;
    }
  
}

  if (currSw0 != prevSw0 || currSw1 != prevSw1) {
    prevSw1 = currSw1;
    prevSw0 = currSw0;
   
    // Solo mueve el servo si ambos switch están en LOW
    if (currSw0 == LOW && currSw1 == LOW) {
      enable_0 = 1;
      enable_1 = 1;  
      if (enable_0 && enable_1) {
        Serial.println("Inicio de movimiento");
       
        for (int angulo = 90; angulo <= 150; angulo++) {
          myServo.write(angulo);
          //delay(velocidad_0)
          delay(velocidad);
        }
        delay(1000);
        for (int angulo = 150; angulo >= 90; angulo--) {
          myServo.write(angulo);
          //delay(velocidad_0)
          delay(velocidad);
        }
        delay(1000);
       
        // Señal de que el movimiento está completo
        enable_3 = 1;
        enable_0 = 0;
        enable_1 = 0;
        Serial.println("Se terminó un movimiento");
        delay(400);
      }
    }
    else {
      // Si alguno de los interruptores no está activado, reestablece el ángulo a 90
      angulo = 90;
      myServo.write(angulo);
      enable_3=0;
    }
  }

*/