#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 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

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("Rady, 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);

  HomeMachine();

}



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);
}