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

#define Multiplier 8.889//91.02222222
#define HomeAngle -15 //80 degrees from vertical or 79 degrees from horizontal


#define Speed 1000
#define Maxspeed 2000
#define Acceleration 50000000.0

#define AStepPin 2
#define BStepPin 14
#define CStepPin 0

#define ADirPin 3
#define BDirPin 15
#define CDirPin 1

#define ALimitPin 9
#define BLimitPin 10
#define CLimitPin 11

#define enablePin 8

/*int X=0; 
int Y=0;
int Z=0;
boolean hasNew=false;*/

DeltaKinematics DK(63,125,030,85);

AccelStepper stepperA(1,AStepPin,ADirPin);
AccelStepper stepperB(1,BStepPin,BDirPin);
AccelStepper stepperC(1,CStepPin,CDirPin);

void setup() 
{  
  delay(10000);
  Serial.begin(115200);
  
  pinMode(ALimitPin, INPUT_PULLUP);
  pinMode(BLimitPin, INPUT_PULLUP);
  pinMode(CLimitPin, INPUT_PULLUP);
  pinMode(enablePin, OUTPUT);
  digitalWrite(enablePin,HIGH);
  
  stepperA.setMaxSpeed(Speed);
  stepperA.setSpeed(Speed);
  stepperA.setCurrentPosition(0);
  stepperA.setAcceleration(Acceleration);
  
  stepperB.setMaxSpeed(Speed);
  stepperB.setSpeed(Speed);
  stepperB.setCurrentPosition(0);
  stepperB.setAcceleration(Acceleration);
  
  stepperC.setMaxSpeed(Speed);
  stepperC.setSpeed(Speed);
  stepperC.setCurrentPosition(0);
  stepperC.setAcceleration(Acceleration);

  
  Serial.println("START");
  Serial.println("Enabling");
  Enabled();
  Serial.println("HOME");
  HomeMachine();
  Serial.println("Ready, set, go!");
}


void loop() 
{
  /*
  Serial.println("Moving x: 0, y: 0, z:-60");
  DK.x = 0;
  DK.y = 0;
  DK.z = -60;
  SetMotors();  // cal of inverse Delta Kinematics is done inside SetMotors()
  delay(3000);
  
  
  Serial.println("Moving x: -45, y: 45, z:-100");
  DK.x =  -45;
  DK.y =  -45;
  DK.z =  -100;
  SetMotors();  // cal of inverse Delta Kinematics is done inside SetMotors()
  delay(3000);

  Serial.println("Moving x: -45, y: 45, z:-100");
  DK.x =  -45;
  DK.y =  45;
  DK.z =  -100;
  SetMotors();  // cal of inverse Delta Kinematics is done inside SetMotors()
  delay(3000);

  
  Serial.println("Moving x: 45, y: 45, z:-100");
  DK.x =  45;
  DK.y =  45;
  DK.z =  -100;
  SetMotors();  // cal of inverse Delta Kinematics is done inside SetMotors()
  delay(3000);

  Serial.println("Moving x: 45, y: -45, z:-100");
  DK.x =  45;
  DK.y =  -45;
  DK.z =  -100;
  SetMotors();  // cal of inverse Delta Kinematics is done inside SetMotors()
  delay(3000);

  Serial.println("Moving x: -45, y: 45, z:-100");
  DK.x =  -45;
  DK.y =  -45;
  DK.z =  -100;
  SetMotors();  // cal of inverse Delta Kinematics is done inside SetMotors()
  delay(3000);

  Serial.println("Moving x: 0, y: 0, z:-100");
  DK.x = 0;
  DK.y = 0;
  DK.z = -60;
  SetMotors();  // cal of inverse Delta Kinematics is done inside SetMotors()
  delay(3000);*/

 while (Serial.available()){

  Serial.println("Received position");
  String received=Serial.readStringUntil('\n');
  //Format of string: x:y:z
  int x_value_separator=received.indexOf(':');
  String x_value = received.substring(0,x_value_separator);
  int y_value_separator=received.indexOf(':',x_value_separator+1);
  String y_value = received.substring(x_value_separator+1, y_value_separator);
  String z_value = received.substring(y_value_separator+1);
  DK.x=x_value.toInt();
  DK.y=y_value.toInt();
  DK.z=z_value.toInt();
  SetMotors();  // cal of inverse Delta Kinematics is done inside SetMotors()
  delay(3000);
  Serial.println("End running motor");
 }
 /*
 if (hasNew) {
    DK.x = X;
    DK.y = Y; 
    DK.z = Z;
    SetMotors();  // cal of inverse Delta Kinematics is done inside SetMotors()
    delay(3000);
    Serial.println("End running motor");
    hasNew=false;
  }*/
  //delay(5000);
  //HomeMachine();
}

/*void serialEvent() {
  Serial.println("Received position");
  String received=Serial.readStringUntil('\n');
  //Format of string: x:y:z
  int x_value_separator=received.indexOf(':');
  String x_value = received.substring(0,x_value_separator);
  int y_value_separator=received.indexOf(':',x_value_separator+1);
  String y_value = received.substring(x_value_separator+1, y_value_separator);
  String z_value = received.substring(y_value_separator+1);
  X=x_value.toInt();
  Y=y_value.toInt();
  Z=z_value.toInt();
  Serial.print("x=");
  Serial.println(X);
  Serial.print("y=");
  Serial.println(Y);
  Serial.print("z=");
  Serial.println(Z);
  hasNew=true;
 }*/

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(ALimitPin) == HIGH || digitalRead(BLimitPin) == HIGH || digitalRead(CLimitPin) == HIGH)
  {
    if(digitalRead(ALimitPin) == HIGH)
    {
      stepperA.move(-10);
      stepperA.run();
    }
    if(digitalRead(BLimitPin) == HIGH)
    {
      stepperB.move(-10);
      stepperB.run();
    }
    if(digitalRead(CLimitPin) == HIGH)
    {
      stepperC.move(-10);
      stepperC.run();
    }    
  }
  
  
  stepperA.setCurrentPosition(HomeAngle*Multiplier);
  stepperB.setCurrentPosition(HomeAngle*Multiplier);
  stepperC.setCurrentPosition(HomeAngle*Multiplier);
  delay(5000);
  Serial.println("Moving x: 0, y: 0, z:-60");
  DK.x = 0;
  DK.y = 0;
  DK.z = -60;
  SetMotors();  // cal of inverse Delta Kinematics is done inside SetMotors()
  delay(3000);
}