#include <ESP32Servo.h>  // Librería para el servo
// Pines del modulo C
#define IO1  8    // Sensor de no sujeción
#define IO2  7    // Sensor de sujeción
#define EN   12
#define DIR  11
#define STEP 10
#define PWM  13
#define SIF  14 // Sensor infrarrojo
// Servo
Servo ServoCD;
int pinServo = PWM;
int angulo;          // Ángulo actual del servo
// Variables
int contador = 0;        // total de pasos del recorrido
int pos_actual = 0;      // posición actual
int destino = 0;
int angulo_dest = 0;
int CalAng = 0;
// Parámetros de velocidad
#define VelNema 1000 // Velocidad del motor a pasos
#define VelServo 1000 // Velocidad del servomotor

void setup() {
  Serial.begin(115200);
  // Configuración de pines
  pinMode(IO1,  INPUT_PULLUP);
  pinMode(IO2,  INPUT_PULLUP);
  pinMode(SIF,  INPUT_PULLUP);
  pinMode(EN,   OUTPUT);
  pinMode(DIR,  OUTPUT);
  pinMode(STEP, OUTPUT);

  digitalWrite(EN, LOW); // activo en bajo
  // Configuración del servo
  ServoCD.attach(pinServo, 500, 2400);
  angulo = 90;
  ServoCD.write(angulo);
  Serial.println("Servo inicializado en 90° (posición horizontal)");

  // Calibración inicial
  Serial.println("Presione 'C' para iniciar calibración");
  while (Serial.read() != 'C');

  // Buscar el sensor de sujeción
  Serial.println("Buscando sensor de sujeción");
  digitalWrite(DIR, LOW); // Mueve hacia el lado lejano
  while (digitalRead(IO2) == HIGH) {
    unPaso();
  }

  // Buscar el sensor de no sujeción
  Serial.println("Buscando sensor de no sujeción");
  digitalWrite(DIR, HIGH); // Mueve hacia el motor
  while (digitalRead(IO1) == HIGH) {
    unPaso();
    contador++;
  }
  Serial.print("Longitud total del recorrido en pasos = ");
  Serial.println(contador);
  // Calibrar el servomotor
  CalAng = 180; // Empieza en 180° y barre hacia abajo hasta detectar una distancia de 14cm (Distancia del sensor al Piso)
  while(digitalRead(SIF) == HIGH){
    moverServo(CalAng);
    CalAng = CalAng - 1;
    delay(100);
  }
  Serial.print("Posición horizontal del Servomotor CD = ");
  Serial.println(CalAng);
  Serial.println("Calibración completa");

  // PROTOCOLO DE FUNCIONAMIENTO 

  Serial.println("Iniciando protocolo");
}

void loop() {
  // Ir a la posición máxima
  moverA(contador);
  // Servo a 90°
  moverServo(CalAng);
  delay(5000); // Esperar 5 segundos

  // Regresar a posición inicial
  moverA(0);
  // Servo a 0°
  moverServo(0); 
}

// Función: un paso del motor
void unPaso() {
  digitalWrite(STEP, HIGH);
  delayMicroseconds(VelNema);
  digitalWrite(STEP, LOW);
  delayMicroseconds(VelNema);
}

// Función: mover el motor a una posición dada
void moverA(int destino) {
  int pasos = abs(destino - pos_actual);
  if (pasos == 0) {
    Serial.println("Ya estás en esa posición.");
    return;
  }
  digitalWrite(DIR, (destino > pos_actual) ? LOW : HIGH);
  for (int i = 0; i < pasos; i++) {
    unPaso();
  }
  pos_actual = destino;
  Serial.print("Posición actual del motor = ");
  Serial.println(pos_actual);
}

// Función: mover el servo al nuevo ángulo
void moverServo(int nuevoAngulo) {
  if (nuevoAngulo < 0 || nuevoAngulo > 180) return;
  int paso = (nuevoAngulo > angulo) ? 1 : -1;
  for (int i = angulo; i != nuevoAngulo; i += paso) {
    ServoCD.write(i);
    delayMicroseconds(VelServo);
  }
  angulo = nuevoAngulo;
  Serial.printf("Servo movido a %d°\n", angulo);
}