#include <AccelStepper.h>

// Define los pines para los motores y los interruptores de límite
#define PIN_STEP_X 54
#define PIN_DIR_X 55
#define ENABLE_PIN_X 38 // Pin de habilitación para el motor X
#define PIN_LIMIT_X 3   // Interruptor de límite para el eje X

#define PIN_STEP_Y 60
#define PIN_DIR_Y 61
#define ENABLE_PIN_Y 56 // Pin de habilitación para el motor Y
#define PIN_LIMIT_Y 2   // Interruptor de límite para el eje Y

// Crea los objetos AccelStepper
AccelStepper stepperX(AccelStepper::DRIVER, PIN_STEP_X, PIN_DIR_X);
AccelStepper stepperY(AccelStepper::DRIVER, PIN_STEP_Y, PIN_DIR_Y);

// Definir la relación de pasos por milímetro
const float stepsPerMM_X = 20.225; // 200 pasos por mm para el eje X
const float stepsPerMM_Y = 33.0; // 200 pasos por mm para el eje Y

void setup() {
  Serial.begin(9600); // Inicia la comunicación serial
  
  // Configura los pines de habilitación como salida
  pinMode(ENABLE_PIN_X, OUTPUT);
  pinMode(ENABLE_PIN_Y, OUTPUT);
  
  // Habilita los motores
  digitalWrite(ENABLE_PIN_X, LOW);
  digitalWrite(ENABLE_PIN_Y, LOW);
  
  // Configura los pines de los interruptores de límite como entrada
  pinMode(PIN_LIMIT_X, INPUT_PULLUP);
  pinMode(PIN_LIMIT_Y, INPUT_PULLUP);

  // Configura la velocidad máxima y la aceleración
  stepperX.setMaxSpeed(500.0);
  stepperX.setAcceleration(100.0);
  stepperY.setMaxSpeed(500.0);
  stepperY.setAcceleration(100.0);

  // Ejecuta el proceso de "homing" para ambos ejes al inicio
  doHoming();
}

void loop() {
  if (Serial.available()) {
    char command = Serial.read();
    if (command == 'X' || command == 'Y') {
      Serial.print("Ingrese el valor en mm para el eje ");
      Serial.println(command);
      while (!Serial.available()) {} // Espera a que esté disponible el valor
      float mm = Serial.parseFloat(); // Lee el valor en mm
      if (command == 'X') {
        moveStepperX(mm);
      } else if (command == 'Y') {
        moveStepperY(mm);
      }
    }
  }
}

void doHoming() {
  // Homing para el eje X
  Serial.println("Homing en X...");
  stepperX.setSpeed(-500); // Ajusta esto según tu configuración
  while (digitalRead(PIN_LIMIT_X) == HIGH) {
    stepperX.runSpeed();
  }
  stepperX.setCurrentPosition(0); // Establece la posición actual como 0
  stepperX.setSpeed(0);

  // Homing para el eje Y
  Serial.println("Homing en Y...");
  stepperY.setSpeed(500); // Ajusta esto según tu configuración
  while (digitalRead(PIN_LIMIT_Y) == HIGH) {
    stepperY.runSpeed();
  }
  stepperY.setCurrentPosition(0); // Establece la posición actual como 0
  stepperY.setSpeed(0);

  Serial.println("Homing completado.");
}

void moveStepperX(float mm) {
  long stepsToMove = mm * stepsPerMM_X; // Convierte mm a pasos
  stepperX.move(stepsToMove);
  while (stepperX.distanceToGo() != 0) {
    stepperX.run();
  }
  float cmMoved = mm / 10.0;
  Serial.print("Eje X movido: ");
  Serial.print(cmMoved);
  Serial.println(" cm");
}

void moveStepperY(float mm) {
  long stepsToMove = mm * stepsPerMM_Y; // Convierte mm a pasos
  stepperY.move(-stepsToMove);
  while (stepperY.distanceToGo() != 0) {
    stepperY.run();
  }
    // Calcula y muestra la distancia recorrida en centímetros
  float cmMoved = mm / 10.0;
  Serial.print("Eje Y movido: ");
  Serial.print(cmMoved);
  Serial.println(" cm");
}