#include <DeltaKinematics.h>
#include <AccelStepper.h>


const int centerX = 0;  // Coordenada x del centro del círculo
const int centerY = 0;  // Coordenada y del centro del círculo
const int radius = 45;   // Radio del círculo
const int numPoints = 40; // Número de puntos a generar
const int z = -285;
#define Multiplier 8.889 //8.889//91.02222222
#define HomeAngle -30 //80 degrees from vertical or 79 degrees from horizontal


#define mSpeed 2000
#define Acceleration 500000
#define Speed 1000

#define XStepPin 2
#define YStepPin 3
#define ZStepPin 4

#define XDirPin 5
#define YDirPin 6
#define ZDirPin 7

#define XLimitPin 9
#define YLimitPin 10
#define ZLimitPin 11

// #define enablePin 8

DeltaKinematics DK(115,279,30,40);

AccelStepper stepperA(1,XStepPin,XDirPin);
AccelStepper stepperB(1,YStepPin,YDirPin);
AccelStepper stepperC(1,ZStepPin,ZDirPin);

void setup() 
{  
  delay(3000);
  Serial.begin(115200);
  
  pinMode(XLimitPin, INPUT_PULLUP);
  pinMode(YLimitPin, INPUT_PULLUP);
  pinMode(ZLimitPin, INPUT_PULLUP);
  // pinMode(enablePin, OUTPUT);
  // digitalWrite(enablePin,HIGH);
  
  stepperA.setMaxSpeed(mSpeed);
  stepperA.setSpeed(Speed);
  stepperA.setCurrentPosition(0);
  stepperA.setAcceleration(Acceleration);
  
  stepperB.setMaxSpeed(mSpeed);
  stepperB.setSpeed(Speed);
  stepperB.setCurrentPosition(0);
  stepperB.setAcceleration(Acceleration);
  
  stepperC.setMaxSpeed(mSpeed);
  stepperC.setSpeed(Speed);
  stepperC.setCurrentPosition(0);
  stepperC.setAcceleration(Acceleration);

  
  Serial.println("START");
  Serial.println("Enabling");
  
  // Enabled();
  Serial.println("HOME");
  HomeMachine();
  Serial.println("Rady, set, go!");
  // Espera un momento antes de generar el siguiente punto
    DK.x = 0;
    DK.y = 0;
    DK.z = z;
    SetMotors();  // call of inverse Delta Kinematics is done inside SetMotors()
    delay(3000);
}

void loop() 
{
   if (Serial.available() > 0) {
    char inputChar = Serial.read();
    if (inputChar == '1') {
       for (int i = 0; i < 30; i++) {
         Mix();




            }

       }
      
    
    else if (inputChar == '0') {
      stepperA.stop();
      stepperB.stop();
      stepperC.stop();
      Serial.print("stop");
    }
    else if (inputChar == '3') {
      HomeMachine();
      Serial.print("homming");
    }
    }
  }
  

  




void Mix(){
  // Genera los puntos de la circunferencia

  
  for (int i = 0; i < numPoints; i++) {
    // Calcula la coordenada x
    int x = centerX + radius * cos(i * 2 * PI / numPoints);
    // Calcula la coordenada y
    int y = centerY + radius * sin(i * 2 * PI / numPoints);
    // Imprime las coordenadas en la consola serial
    Serial.print("X: ");
    Serial.print(x);
    Serial.print(", Y: ");
    Serial.println(y);
    Serial.print(", Z: ");
    Serial.println(z);
    // Espera un momento antes de generar el siguiente punto
    DK.x = x;
    DK.y = y;
    DK.z = z;
    SetMotors();  // call of inverse Delta Kinematics is done inside SetMotors()
    delay(10);
  }
}







void SetMotors()
{  
  Serial.println("SetMotors"); 
  DK.inverse(); // cal inverse Delta Kinematics
  double A = DK.a*Multiplier;
  double B = DK.b*Multiplier;
  double C = DK.c*Multiplier;
  Serial.println(DK.a);
  Serial.println(DK.b);
  Serial.println(DK.c);
  Serial.println(DK.x);
  Serial.println(DK.y);
  Serial.println(DK.z);
  Serial.println();
  stepperA.moveTo(A);
  stepperB.moveTo(B);
  stepperC.moveTo(C);
  
  while(stepperA.distanceToGo() != 0 || stepperB.distanceToGo() != 0 || stepperC.distanceToGo() != 0)
  {
    stepperA.run();
    stepperB.run();
    stepperC.run();
  }
}












// void Enabled()
// {
//   digitalWrite(enablePin,LOW);
// }

// void Disabled()
// {
//   digitalWrite(enablePin,HIGH);
// }


void HomeMachine()
{
  while(digitalRead(XLimitPin) == HIGH || digitalRead(YLimitPin) == HIGH || digitalRead(ZLimitPin) == HIGH)
  {
    if(digitalRead(XLimitPin) == HIGH)
    {
      stepperA.move(-10);
      stepperA.run();
    }
    if(digitalRead(YLimitPin) == HIGH)
    {
      stepperB.move(-10);
      stepperB.run();
    }
    if(digitalRead(ZLimitPin) == HIGH)
    {
      stepperC.move(-10);
      stepperC.run();
    }    
  }
  
  stepperA.setCurrentPosition(HomeAngle*Multiplier);
  stepperB.setCurrentPosition(HomeAngle*Multiplier);
  stepperC.setCurrentPosition(HomeAngle*Multiplier);
}