////////////////////////////// MOTOR NEMA  ///////////////////////////////
#include <AccelStepper.h>
// Definir los pines del A4988 y los sensores
#define A4988_DIR  11
#define A4988_STEP 10
#define SENSOR1    7   // Botón para movimiento horario
#define SENSOR2    8   // Botón para movimiento antihorario
// Crear una instancia de AccelStepper en modo DRIVER (requiere dirección y pasos)
AccelStepper motor(AccelStepper::DRIVER, A4988_STEP, A4988_DIR);
// Constantes para lógica
#define ADELANTE 0 //El NEMA se moverá hasta el LS que activa el servomotor     (horario)
#define ATRAS    1 //El NEMA se moverá hasta el LS que solo cambia de dirección (antihorario)
// Variables para lógica
uint8_t direccion = 0;
uint8_t estado_anterior_sensor1 = HIGH;  // Estado anterior del botón SENSOR1
uint8_t estado_anterior_sensor2 = HIGH;  // Estado anterior del botón SENSOR2
int NEMA_fin=0;

////////////////////////////// SERVOMOTOR ///////////////////////////////
#include<ESP32Servo.h> // Librería para el manejo del Servo
// Definir el pin para el contro del Servomotor
#define SERVO_PIN  13
// Creando el objeto para el manejo del Servo#define HORARIO LOW
Servo myservo;
// Variables de lógica
int Angulo = 90; // Ángulo de inicio del Servo
int iniciar_mov_servo = 1;
int resultado;

//variables para el loop
int clamshers_separados=0;
int estado=2;

char control='C';

void setup() {
  pinMode(SENSOR1, INPUT_PULLUP);         // Botón 1 con resistencia de pull-up interna
  pinMode(SENSOR2, INPUT_PULLUP);         // Botón 2 con resistencia de pull-up interna
  Serial.begin(115200);
  // Configurar el motor
  motor.setMaxSpeed(100);      // Velocidad máxima del motor
  motor.setAcceleration(50);  // Configurar la aceleración

   // Configura los pines del servo
  myservo.attach(SERVO_PIN);
  myservo.write(Angulo);
  // Configura los pines del motor Nema
}

int servoMotor(int iniciar_mov_servo) {
  delay(100);
  if (iniciar_mov_servo == 1) {
  // Movimiento decremental
    
    while (Angulo > 30) {
      Angulo--;
      delay(15);
      if (Angulo < 30) Angulo = 30;
      myservo.write(Angulo); // Actualiza el ángulo del Servo
    }  
    delay(1000); // Tiempo de espera necesario
    // Movimiento incremental
  
    while (Angulo < 90) {
      Angulo++;
      delay(15);
      if (Angulo > 90) Angulo = 90;
      myservo.write(Angulo);     
    }

      // Señal de fin de movimiento     
      iniciar_mov_servo = 0; // Detener el movimiento
      return 1; // Indica que el movimiento ha terminado
  }
  return 0; // El servo no se movió
}

void revisar_control() {
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');
    input.trim(); // Elimina espacios en blanco adicionales
    if (input.equalsIgnoreCase("R")) {
      control='R';
    } else if (input.equalsIgnoreCase("S")) {
      control='S';
    } else if (input.equalsIgnoreCase("C")) {
      control='C';
    } else if (input.equalsIgnoreCase("E")) {
      control='E';
    } else
      Serial.println("Ingrese un comando valido");
  }
}

int Motor_NEMA() {
  while (1){
    revisar_control();
    if (control!='C') return 0;
    // Leer el estado de los botones
    uint8_t estado_sensor1 = digitalRead(SENSOR1);
    uint8_t estado_sensor2 = digitalRead(SENSOR2);

    // Detectar cambio de estado en SENSOR1 (dirección hacia atras)
    if (estado_sensor1 == LOW && estado_anterior_sensor1 == HIGH && direccion == 0) {
      direccion = ATRAS;
      delay(2000);
      return 0;       // Regresar al loop despues de un ida y vuelta
    }
    // Detectar cambio de estado en SENSOR2 (dirección hacia adelante)
    if (estado_sensor2 == LOW && estado_anterior_sensor2 == HIGH && direccion == 1){
      direccion = ADELANTE;
      delay(2000);
    }
    // Actualizar los estados anteriores
    estado_anterior_sensor1 = estado_sensor1;
    estado_anterior_sensor2 = estado_sensor2;

    if (direccion == ATRAS) {
      motor.setSpeed(-100); 
    } else {
      motor.setSpeed(100); 
    }     
    motor.runSpeed();
  }
}

void loop(){
  Serial.println(estado); // impresion de estado exclusiva para la prueba.
  delay(10);
  estado = 2;
  Serial.println(estado); // impresion de estado exclusiva para la prueba.
  delay(10);
  clamshers_separados = servoMotor(1);
  iniciar_mov_servo = 1;  
  Serial.println("Indicar al modulo C, clamshers listos para plataforma");
  // digitalWrite(Pin_Mod_C, LOW);
  delay(10);
  estado = 3; 
  Serial.println(estado);// impresion de estado exclusiva para la prueba.
  delay(10);
  estado = 4;
  //S4 Estado de actividad de reposicionamiento de la garras, el nema se encarga de dica actividad
  Serial.println(estado);// impresion de estado exclusiva para la prueba.
  delay(10);
  // Iniciar movimiento del motor NEMA ida y vuelta desde cualquier posicion de las garras
  NEMA_fin=Motor_NEMA();
  Serial.println("Modulo A listo para siguiente ciclo");
  estado = 1;
  Serial.println(estado);
  delay(10);
}