#include <ESP32Servo.h>

Servo myservo1;  // Crear objeto Servo

#define switch1 4
#define switch2 5

//motor a paso
#define A4988_1_STEP 10 
#define A4988_1_DIR 11
#define A4988_1_ENABLE 12
//servo
#define servo1 13     // Asignar el servomotor al pin 13

int angulo = 90;
int estSensorInicio;   // Estado del sensor de fin de carrera (inicio)
int estSensorFinal;    // Estado del sensor de fin de carrera (final)

#define HORARIO LOW
#define ANTI_HORARIO HIGH

// Variable para contar los pasos
int pasosContados = 0;

void moverMotor(int dirPin, int stepPin, int steps, int direccion, int delayTime) {
    digitalWrite(dirPin, direccion);
    for (int i = 0; i < steps; i++) {
        digitalWrite(stepPin, HIGH);
        delay(delayTime);
        digitalWrite(stepPin, LOW);
        delay(delayTime);
        
        // Incrementar o decrementar el contador de pasos
        if (direccion == HORARIO) {
            pasosContados++;
        } else {
            pasosContados--;
        }
    }
}

int obtenerPasosContados() {
    return pasosContados;
}

void setup() {
    Serial.begin(115200);
    Serial.println("Hello, ESP32-S3!");

    myservo1.attach(servo1);
    pinMode(switch1, INPUT_PULLUP);
    pinMode(switch2, INPUT_PULLUP);
    pinMode(A4988_1_DIR, OUTPUT);
    pinMode(A4988_1_STEP, OUTPUT);
    
    myservo1.write(angulo);
    Serial.print("Pasos de inicio: ");
    Serial.println(pasosContados);
}

uint8_t caso = 0;

void loop() {
    uint8_t switch1_ = digitalRead(switch1);
    uint8_t switch2_ = digitalRead(switch2);
    
    if(switch2_ == 1 && switch1_ == 1 && pasosContados == 0) {
        moverMotor(A4988_1_DIR, A4988_1_STEP, 50, HORARIO, 10); // avanzar 50 pasos
        delay(1000);

        // Actiivar movimiento del servomotor
        angulo = 150;  // Cambiar el ángulo a 150 grados
        myservo1.write(angulo);
        
        delay(3000); // esperar tiempo a que el modulo B se coloque en su posicion

        // Regresar servo a posicion inicial = 90
        angulo = 90;  // Cambiar el ángulo a 90 grados
        myservo1.write(angulo);
        delay(3000);

        moverMotor(A4988_1_DIR, A4988_1_STEP, 50, ANTI_HORARIO, 10); // retroceder 50 pasos
        delay(3000);
    }

    // Imprimir el número de pasos contados
    Serial.print("Pasos contados: ");
    Serial.println(obtenerPasosContados());
}