#include <AccelStepper.h>
#include <MultiStepper.h>

// Definiciones de pines para los motores
#define PIN_STEP0 2
#define PIN_DIR0 5
#define PIN_STEP1 3
#define PIN_DIR1 6

// Creación de los objetos AccelStepper para cada motor
AccelStepper stepperX(AccelStepper::DRIVER, PIN_STEP0, PIN_DIR0);
AccelStepper stepperY(AccelStepper::DRIVER, PIN_STEP1, PIN_DIR1);
MultiStepper steppers;

// Variables para calcular el área de trabajo
const float pasosPorRev = 200; // Asumiendo 200 pasos por revolución por defecto
const float mmPorRev = 200; // Milímetros por revolución, ajustar según la mecánica del sistema
float pasosPorMM = pasosPorRev / mmPorRev; // Calcula los pasos por milímetro
float pasosPorCM = pasosPorMM * 10; // Calcula los pasos por centímetro

// Dimensiones del área de trabajo en centímetros
const float anchoEspacioTrabajo = 30.0; // Ejemplo: 30 cm de ancho
const float altoEspacioTrabajo = 30.0; // Ejemplo: 30 cm de alto

String inputString = "";         // Una cadena para almacenar los comandos entrantes
boolean stringComplete = false;  // Para saber si se ha recibido toda la entrada



void setup() {
  Serial.begin(9600); // Inicia la comunicación serial
  inputString.reserve(200); // Reserva espacio para la entrada

  // Configuración inicial de los motores
  stepperX.setMaxSpeed(200);
  stepperX.setAcceleration(100);
  stepperY.setMaxSpeed(200);
  stepperY.setAcceleration(100);
    
  steppers.addStepper(stepperX);
  steppers.addStepper(stepperY);
  Serial.println("Sistema inicializado. Envía 'info' para obtener información.");
}

void loop() {
   // Verifica si se ha completado la entrada
  if (stringComplete) {
    // Comando para mostrar información
    if (inputString == "info") {
      printInfo();
    }
    // Comando para dibujar círculo
  if (inputString.startsWith("dibujar ")) {
      float radio = inputString.substring(8).toFloat();
      if (radio > 0) {
        Serial.print("Dibujando círculo con radio: ");
        Serial.println(radio);
        dibujarCirculo(radio);
      } else {
        Serial.println("Radio inválido. Por favor, introduce un valor positivo.");
      }
    }

    // Limpia la cadena para el próximo comando
    inputString = "";
    stringComplete = false;
  }
}


void printInfo() {
  // Ajustamos pasosPorMM para que ahora represente pasos por centímetro
  float pasosPorCM = pasosPorRev / (mmPorRev / 10); // Convertimos mmPorRev a cmPorRev dividiendo por 10

  Serial.println("Información del sistema:");
  Serial.print("Pasos por centímetro: ");
  Serial.println(pasosPorCM, 4);
  Serial.print("Área de trabajo estimada para X y Y en cm (ajustar según la mecánica): ");
  Serial.print("X: ");
  Serial.print(200 * (mmPorRev / 10) / pasosPorRev); // Convertimos mm a cm dividiendo por 10
  Serial.print(" cm, Y: ");
  Serial.print(200 * (mmPorRev / 10) / pasosPorRev); // Convertimos mm a cm dividiendo por 10
  Serial.println(" cm");
  // Agregar cualquier otra información relevante aquí
}

void dibujarCirculo(float radio) {
 // Convertir radio a pasos (usando pasosPorCM ya que el radio está en cm)
  float radioEnPasos = radio * pasosPorCM; // Usando pasosPorCM correctamente

  // Verificar si el círculo cabe en el espacio de trabajo
  float diametroEnPasos = radioEnPasos * 2;
  if (diametroEnPasos > (anchoEspacioTrabajo * 10 * pasosPorMM) || diametroEnPasos > (altoEspacioTrabajo * 10 * pasosPorMM)) {
    Serial.println("El radio excede el espacio de trabajo.");
    return;
  }

  // Mover los motores al centro del espacio de trabajo (ajustar según configuración)
  // Aquí necesitas calcular el punto central en pasos y mover los motores a esa posición
  long centroX = (anchoEspacioTrabajo / 2) * pasosPorCM;
  long centroY = (altoEspacioTrabajo / 2) * pasosPorCM;
  moverA(centroX, centroY); // Esta función debería mover los motores a las coordenadas X, Y proporcionadas

  // Dibujar el círculo
  // Aquí necesitarías implementar un algoritmo para dibujar el círculo punto por punto
  // Por ejemplo, un algoritmo de Bresenham para círculos o algo similar
  // Esto implica calcular los puntos alrededor del círculo y mover los motores a cada punto
  for (float angulo = 0; angulo < 360; angulo += 0.5) { // Incremento de 0.5 grados para mayor precisión
    float x = centroX + radioEnPasos * cos(angulo * PI / 180);
    float y = centroY + radioEnPasos * sin(angulo * PI / 180);
    moverA(x, y); // Mueve los motores a la posición calculada
  }
}

void moverA(long x, long y) {
  // Implementa el movimiento de los motores a las coordenadas x, y
  // Esto dependerá de cómo estén configurados tus motores y cómo se traducen las coordenadas a pasos
  // Por ejemplo, podrías usar algo como:
  stepperX.moveTo(x);
  stepperY.moveTo(y);
  while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
    stepperX.run();
    stepperY.run();
  }
}
void serialEvent() {
  while (Serial.available()) {
    // Obtiene el nuevo byte:
    char inChar = (char)Serial.read();
    // Añade el caracter a la cadena de entrada:
    if (inChar == '\n') {
      stringComplete = true;
    } else {
      inputString += inChar;
    }
  }
}