#include <ESP32Servo.h>

Servo servo_CD;
const int Pin_Servo_CD = 13;

// Pines para motor_C (ahora stepper: Dir para dirección, Step para pasos)
const int Pin_Motor_C_Dir = 11;      // Pin para dirección del motor_C
const int Pin_Motor_C_Step = 10;     // Pin para step del motor_C (anteriormente Enable)
const int Pin_Limit_C_Final = 7;    // Pin para limit switch final (PUNTO DE SUJECIÓN)
const int Pin_Limit_C_Inicial = 8;  // Pin para limit switch inicial (PUNTO DE NO SUJECIÓN)

// Posiciones del servo
const int Pos_inicial = 90;
int Pos_CD = 90;

// Variables para velocidad (interpretada como delay en ms entre toggles del step para el stepper; RPS se asume como revoluciones por segundo aproximado, pero adaptado a delay)
int velocidad = 15;  // Valor por defecto, se pedirá al usuario (delay en ms entre HIGH/LOW del step)

void setup() {
  Serial.begin(115200);

  // Inicializar servo_CD
  servo_CD.attach(Pin_Servo_CD, 500, 2500);
  servo_CD.write(Pos_inicial);
  Serial.println("Servo CD en 90 grados (posición inicial)");
  delay(2000);

  // Inicializar pines para motor_C (stepper)
  pinMode(Pin_Motor_C_Dir, OUTPUT);
  pinMode(Pin_Motor_C_Step, OUTPUT);
  pinMode(Pin_Limit_C_Final, INPUT_PULLUP);  // Asumiendo pullup para switches normalmente abiertos
  pinMode(Pin_Limit_C_Inicial, INPUT_PULLUP);

  // Inicializar motor_C apagado (step en LOW)
  digitalWrite(Pin_Motor_C_Step, LOW);
  digitalWrite(Pin_Motor_C_Dir, LOW);

  Serial.println("Sistema inicializado. Presione ENTER para continuar con la calibración.");
}

void loop() {
  // Estado inicial: Esperar ENTER
  if (Serial.available()) {
    String input = Serial.readStringUntil('\n');
    input.trim();  // Remover espacios y saltos de línea
    if (input == "") {  // ENTER detectado (cadena vacía)
      Serial.println("ENTER recibido. Iniciando calibración.");
      
      // Pedir velocidad en RPS (interpretada como delay en ms entre toggles del step para el stepper)
      Serial.println("Ingrese velocidad en RPS (delay en ms entre toggles del step para el stepper):");
      while (!Serial.available()) {
        delay(10);  // Esperar input
      }
      velocidad = Serial.parseInt();  // Leer como entero
      if (velocidad <= 0) velocidad = 15;  // Valor por defecto si inválido
      Serial.print("Velocidad establecida en: ");
      Serial.println(velocidad);
      
      // Proceso de calibración
      calibrar();
      
      // Después de calibración, entrar en ciclo de etapas
      cicloEtapas();
    }
  }
}

void calibrar() {
  Serial.println("Iniciando calibración...");
  
  // Mover motor_C hacia PUNTO DE SUJECIÓN (usando LIMIT_SWITCH_C_FINAL)
  Serial.println("Moviendo motor_C hacia PUNTO DE SUJECIÓN...");
  digitalWrite(Pin_Motor_C_Dir, HIGH);  // Dirección hacia final
  while (digitalRead(Pin_Limit_C_Final) != 1) {
    digitalWrite(Pin_Motor_C_Step, HIGH);
    delay(velocidad);
    digitalWrite(Pin_Motor_C_Step, LOW);
    delay(velocidad);
  }
  Serial.println("Motor_C en PUNTO DE SUJECIÓN (motor_C = 0)");
  
  // Mover motor_C hacia PUNTO DE NO SUJECIÓN (usando LIMIT_SWITCH_C_INICIAL)
  Serial.println("Moviendo motor_C hacia PUNTO DE NO SUJECIÓN...");
  digitalWrite(Pin_Motor_C_Dir, LOW);  // Dirección hacia inicial
  while (digitalRead(Pin_Limit_C_Inicial) != 1) {
    digitalWrite(Pin_Motor_C_Step, HIGH);
    delay(velocidad);
    digitalWrite(Pin_Motor_C_Step, LOW);
    delay(velocidad);
  }
  Serial.println("Motor_C en PUNTO DE NO SUJECIÓN (punto inicial)");
  
  // Mover Servo_CD de 90° a 180° con la velocidad recibida
  Serial.println("Moviendo Servo_CD de 90° a 180°...");
  Pos_CD = 90;
  while (Pos_CD <= 180) {
    Pos_CD++;
    servo_CD.write(Pos_CD);
    delay(1000 / velocidad);  // Delay basado en velocidad (grados por segundo)
  }
  Pos_CD = 180;
  servo_CD.write(180);
  Serial.println("Servo_CD en 180 grados");
  
  // Esperar 2 segundos
  delay(2000);
  
  // Mover Servo_CD de 180° a 90°
  Serial.println("Moviendo Servo_CD de 180° a 90°...");
  while (Pos_CD >= 90) {
    Pos_CD--;
    servo_CD.write(Pos_CD);
    delay(1000 / velocidad);
  }
  Pos_CD = 90;
  servo_CD.write(90);
  Serial.println("Servo_CD en 90 grados");
  
  Serial.println("Calibración exitosa.");
}

void cicloEtapas() {
  while (true) {
    // Etapa 1: Colocar pila de Clamshells
    Serial.println("Etapa 1: Colocar pila de Clamshells. Presione 'S' para mover motor_C a PUNTO DE SUJECIÓN.");
    while (true) {
      if (Serial.available()) {
        char tecla = Serial.read();
        if (tecla == 'S') {
          Serial.println("'S' recibido. Moviendo motor_C a PUNTO DE SUJECIÓN...");
          digitalWrite(Pin_Motor_C_Dir, HIGH);
          while (digitalRead(Pin_Limit_C_Final) != 1) {
            digitalWrite(Pin_Motor_C_Step, HIGH);
            delay(velocidad);
            digitalWrite(Pin_Motor_C_Step, LOW);
            delay(velocidad);
          }
          Serial.println("Motor_C en PUNTO DE SUJECIÓN. Manteniendo Servo_CD en 90°. Presione 'E' para continuar.");
          break;
        }
      }
      delay(10);
    }
    
    // Esperar 'E' para Etapa 2
    while (true) {
      if (Serial.available()) {
        char tecla = Serial.read();
        if (tecla == 'E') {
          Serial.println("'E' recibido. Iniciando Etapa 2: Separar un Clamshell.");
          
          // Mover motor_C hacia PUNTO DE NO SUJECIÓN
          Serial.println("Moviendo motor_C a PUNTO DE NO SUJECIÓN...");
          digitalWrite(Pin_Motor_C_Dir, LOW);
          while (digitalRead(Pin_Limit_C_Inicial) != 1) {
            digitalWrite(Pin_Motor_C_Step, HIGH);
            delay(velocidad);
            digitalWrite(Pin_Motor_C_Step, LOW);
            delay(velocidad);
          }
          Serial.println("Motor_C en PUNTO DE NO SUJECIÓN.");
          
          // Mover Servo_CD de 90° a 180°
          Serial.println("Moviendo Servo_CD de 90° a 180°...");
          Pos_CD = 90;
          while (Pos_CD <= 180) {
            Pos_CD++;
            servo_CD.write(Pos_CD);
            delay(1000 / velocidad);
          }
          Pos_CD = 180;
          servo_CD.write(180);
          Serial.println("Servo_CD en 180 grados. Etapa 2 completada. Presione 'Y' o 'N' para validar separación.");
          break;
        }
      }
      delay(10);
    }
    
    // Etapa 3: Bajar pila de Clamshells
    while (true) {
      if (Serial.available()) {
        char tecla = Serial.read();
        if (tecla == 'Y' || tecla == 'N') {
          Serial.print("Tecla '");
          Serial.print(tecla);
          Serial.println("' recibida. Iniciando Etapa 3: Bajar pila de Clamshells.");
          
          // Mover motor_C hacia PUNTO DE SUJECIÓN
          Serial.println("Moviendo motor_C a PUNTO DE SUJECIÓN...");
          digitalWrite(Pin_Motor_C_Dir, HIGH);
          while (digitalRead(Pin_Limit_C_Final) != 1) {
            digitalWrite(Pin_Motor_C_Step, HIGH);
            delay(velocidad);
            digitalWrite(Pin_Motor_C_Step, LOW);
            delay(velocidad);
          }
          Serial.println("Motor_C en PUNTO DE SUJECIÓN.");
          
          // Mover Servo_CD de 180° a 90°
          Serial.println("Moviendo Servo_CD de 180° a 90°...");
          while (Pos_CD >= 90) {
            Pos_CD--;
            servo_CD.write(Pos_CD);
            delay(1000 / velocidad);
          }
          Pos_CD = 90;
          servo_CD.write(90);
          Serial.println("Servo_CD en 90 grados.");
          
          Serial.println("Ciclo terminado. Volviendo a Etapa 1.");
          break;
        }
      }
      delay(10);
    }
  }
}