/* 
  P05 Círculos con la CNC
  Lab. Robótica
*/

#include <AccelStepper.h>

// Definir las variables globales
float xInicio, yInicio;
int dir;

#define X_Dir 9  // Motor X 
#define X_Pul 10  
#define Y_Dir 4  // Motor Y 
#define Y_Pul 3 
#define X_HOM 7 // Homing X
#define Y_HOM 6 // Homing Y

#define STEPS_PER_MM 6.06 

AccelStepper stepperY(AccelStepper::DRIVER, Y_Pul, Y_Dir);
AccelStepper stepperX(AccelStepper::DRIVER, X_Pul, X_Dir);

float xMax = 230;
float yMax = 150;
float xMin = 8;
float yMin = 27;

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

  stepperX.setMaxSpeed(1500.0);
  stepperX.setAcceleration(1500.0);
  stepperY.setMaxSpeed(1500.0);
  stepperY.setAcceleration(1500.0);

  pinMode(X_HOM, INPUT_PULLUP);
  pinMode(Y_HOM, INPUT_PULLUP);
 
  Serial.println("Homing");
  homing_X(stepperX, X_HOM);
  homing_Y(stepperY, Y_HOM);

  Serial.println("Ingresa coordenadas (x,y,r,%):");
}

void loop() {
  if (Serial.available() > 0) {
    float cX = Serial.parseFloat();
    float cY = Serial.parseFloat();
    float r = Serial.parseFloat();
    float percent = Serial.parseFloat();

    Serial.print(cX);
    Serial.print(", ");
    Serial.print(cY);
    Serial.print(" Radio: ");
    Serial.println(r);

    // Punto x,y dentro?
    if (isWithinLimits(cX, cY)) {
      PuntodeInicio(cX, cY, r); // Llama a la función para modificar las variables globales

      // Usa las variables globales modificadas
      Serial.print("xInicio: ");
      Serial.println(xInicio);
      Serial.print("yInicio: ");
      Serial.println(yInicio);
      Serial.print("dir: ");
      Serial.println(dir);

      // Llamamos a la función de círculo
      Circle(xInicio, yInicio, r, percent); 
    } else {
      Serial.println("Colocar un punto dentro de los límites.");
    }
  }
}

bool isWithinLimits(float x, float y) {
  return (x >= xMin && x <= xMax && y >= yMin && y <= yMax);
}

void PuntodeInicio(float cX, float cY, float r) {
  dir = 1; // Horario por defecto 1 
  xInicio = cX + r;
  yInicio = cY;

  if ((cX + r) > xMax) { // Derecha
    xInicio = xMax;
    float temp = pow(r, 2) - pow((xMax - cX), 2);
    yInicio = (temp >= 0) ? cY + sqrt(temp) : yInicio;  // Ajuste hacia arriba
    dir = -1;
    Serial.println("Derecha");
  } else if ((cX - r) < xMin) { // Izquierda
    xInicio = xMin;
    float temp = pow(r, 2) - pow((cX - xMin), 2);
    yInicio = (temp >= 0) ? cY - sqrt(temp) : yInicio;  // Ajuste hacia abajo
    dir = -1;
    Serial.println("Izquierda");
  } else if ((cY + r) > yMax) { // Arriba
    yInicio = yMax;
    float temp = pow(r, 2) - pow((yMax - cY), 2);
    xInicio = (temp >= 0) ? sqrt(temp) + cX : xInicio;
    dir = -1;
    Serial.println("Arriba");
  } else if ((cY - r) < yMin) { // Abajo
    yInicio = yMin;
    float temp = pow(r, 2) - pow((cY - yMin), 2);
    xInicio = (temp >= 0) ? sqrt(temp) + cX : xInicio;
    dir = 1;
    Serial.println("Abajo");
  }


  // Verifica si el punto de inicio calculado está dentro de los límites
  if (!isWithinLimits(xInicio, yInicio)) {
    Serial.println("Error: punto de inicio fuera de límites.");
  }
}

void Circle(float cX, float cY, float r, float percent) {
  cX = cX - r;
  int steps = 500;  // Número de pasos para el círculo completo
  float totalAngle = 2 * PI * (percent / 100);  // Calcular el ángulo total a dibujar basado en el porcentaje
  float angleStep = totalAngle / steps;  // Ángulo por paso

  // Comprobar si alguno de los puntos del círculo está fuera de los límites
  for (int i = 0; i <= steps; i++) {
    float theta = i * angleStep * dir;  // Cambiar el signo de theta dependiendo de la dirección
    if ((cX + r) > xMax){ // Derecha
      theta = theta + PI;
    }

    else if ((cX - r) < xMin){ // Izquierda
      theta = theta - PI;
    }

    else{
      theta = i * angleStep * dir;
    }
    
    float x = (cX + r * cos(theta)); // Calcular la coordenada X en el borde del círculo
    float y = (cY + r * sin(theta));  // Calcular la coordenada Y en el borde del círculo
    Serial.print("x: ");
    Serial.print(x);
    Serial.print(", y: ");
    Serial.println(y);
    // Verificar si la coordenada está dentro de los límites antes de continuar
    if (!isWithinLimits(x, y)) {
      Serial.println("Error: Un punto está fuera de los límites. No se puede dibujar el círculo.");
    }
    
    // Imprimir el último punto para verificar si está dentro de los límites
    if (i == steps) {
      Serial.print("Último punto calculado: ");
      Serial.print(x);
      Serial.print(", ");
      Serial.println(y);
    }
  }

  Serial.println("Círculo dentro de los límites. Procediendo a dibujar.");

  // Si todos los puntos están dentro de los límites, puedes agregar la lógica para dibujar el círculo aquí.
  // Esto es solo una verificación de límites.
}



void homing_X(AccelStepper &stepper, int homeSwitchPin) {
  stepper.setMaxSpeed(500.0); 
  stepper.setAcceleration(300.0);

  stepper.moveTo(-10000); 
  while (digitalRead(X_HOM) == HIGH) {
    stepper.run();
  }
  stepper.stop();
  stepper.setCurrentPosition(0);
}

void homing_Y(AccelStepper &stepper, int homeSwitchPin) {
  stepper.setMaxSpeed(500.0); 
  stepper.setAcceleration(300.0);

  stepper.moveTo(10000);
  while (digitalRead(Y_HOM) == HIGH) {
    stepper.run();
  }
  stepper.stop();
  stepper.setCurrentPosition(0);
}

void moveToCoordinates(float x, float y) {
  long stepsX = x * STEPS_PER_MM;
  long stepsY = y * STEPS_PER_MM;

  stepperX.moveTo(stepsX);
  stepperY.moveTo(stepsY);

  while (stepperX.distanceToGo() != 0 || stepperY.distanceToGo() != 0) {
    stepperX.run();
    stepperY.run();
  }
}