#include <AccelStepper.h>
/*Me falta agregar una bandera para cada uno de los homing y luego agregar una verificacion con un if de que los 3 homing estan hechos.
*/
// Define los pines para el motor
#define PIN_STEP 3
#define PIN_DIR 2

// Define el pin para el interruptor de límite
#define PIN_LIMIT 9

//Definimos los Valores de entrada de los 
#define PIN_STEP1 20
#define PIN_DIR1 21

#define PIN_LIMIT1 8

#define PIN_STEP2 14
#define PIN_DIR2 15

#define PIN_LIMIT2 10

AccelStepper stepper(AccelStepper::DRIVER, PIN_STEP, PIN_DIR);
AccelStepper stepper1(AccelStepper::DRIVER, PIN_STEP1, PIN_DIR1);
AccelStepper stepper2(AccelStepper::DRIVER, PIN_STEP2, PIN_DIR2);

void setup() {
  // Inicializa el puerto serial
  Serial.begin(9600);

  // Configura el pin del interruptor de límite como entrada
  pinMode(PIN_LIMIT, INPUT_PULLUP);
  pinMode(PIN_LIMIT1, INPUT_PULLUP);
  pinMode(PIN_LIMIT2, INPUT_PULLUP);

  // Configura la velocidad y aceleración del motor
  stepper.setMaxSpeed(200.0);
  stepper.setAcceleration(100.0);

  // Configura la velocidad y aceleración del motor
  stepper1.setMaxSpeed(200.0);
  stepper1.setAcceleration(100.0);

  //Configura la velocidad y aceleración del motor
  stepper2.setMaxSpeed(200.0);
  stepper2.setAcceleration(100.0);

  // Ejecuta el homing al iniciar
    Homing();
}

void loop() {
  // Tu código aquí
}

void doHomingCintura() {
  Serial.println("Iniciando homing en la Cintura...");

  // Mueve el motor lentamente hacia el interruptor de límite
  stepper.setSpeed(100); // Ajusta la dirección y velocidad para tu configuración

  // Mientras el interruptor de límite no esté activado, mueve el motor
  while (digitalRead(PIN_LIMIT) == HIGH) {
    stepper.runSpeed();
  }

  // Detiene el motor
  stepper.stop();

  // Establece la posición actual como 0 (Home)
  stepper.setCurrentPosition(0);

  Serial.println("Homing completado en la Cintura.");
}

void doHomingHombro() {
  Serial.println("Iniciando homing en el Hombro...");

  // Mueve el motor lentamente hacia el interruptor de límite
  stepper1.setSpeed(100); // Ajusta la dirección y velocidad para tu configuración

  // Mientras el interruptor de límite no esté activado, mueve el motor
  while (digitalRead(PIN_LIMIT1) == HIGH) {
    stepper1.runSpeed();
  }

  // Detiene el motor
  stepper1.stop();

  // Establece la posición actual como 0 (Home)
  stepper1.setCurrentPosition(0);

  Serial.println("Homing completado en el Hombro.");
}

void doHomingCodo() {
  Serial.println("Iniciando homing en el Codo...");

  // Mueve el motor lentamente hacia el interruptor de límite
  stepper2.setSpeed(100); // Ajusta la dirección y velocidad para tu configuración

  // Mientras el interruptor de límite no esté activado, mueve el motor
  while (digitalRead(PIN_LIMIT2) == HIGH) {
    stepper2.runSpeed();
  }

  // Detiene el motor
  stepper2.stop();

  // Establece la posición actual como 0 (Home)
  stepper2.setCurrentPosition(0);

  Serial.println("Homing completado en el codo.");
}

void Homing(){
  doHomingCintura();
  doHomingHombro();
  doHomingCodo();
}