// the complexity lies in determining the kinetics principly.
#define PI 3.1415926535897932384626433832795
int r = 3;
int L2 = 10; 
int L1 = 10;
int Xp = 1;
int Yp = -5; 
int Zp = 1; 
int theta1 = 0; 
int theta2 = 120*PI/180;
int theta3 = 240*PI/180;

// all the constants equations. All the values except for Xp Yp and Yp can be simplified to speed up calculations. 
float C4 = Xp*Xp + r*r + L2*L2 + Zp*Zp + Yp*Yp - L1*L1;

float l1 =   2*r*L2 - 2*L2*Xp*cos(theta1) - 2*L2*Yp*sin(theta1) ;
float m1 =   2*L2*Zp;
float n1 = - 2*r*Xp*cos(theta1) - 2*r*Yp*sin(theta1) + C4 ;

float l2 =   2*r*L2 - 2*L2*Xp*cos(theta2) - 2*L2*Yp*sin(theta2) ;
float m2 =   2*L2*Zp;
float n2 = - 2*r*Xp*cos(theta2) - 2*r*Yp*sin(theta2) + C4 ;

float l3 =   2*r*L2 - 2*L2*Xp*cos(theta3) - 2*L2*Yp*sin(theta3) ;
float m3 =   2*L2*Zp;
float n3 = - 2*r*Xp*cos(theta3) - 2*r*Yp*sin(theta3) + C4 ;

//n1*n1 - (l1*l1 + m1*m1)



// the equations that needs to be solved. 

#include <AccelStepper.h>
AccelStepper stepper1(AccelStepper::DRIVER, 2, 5);
AccelStepper stepper2(AccelStepper::DRIVER, 3, 6);
AccelStepper stepper3(AccelStepper::DRIVER, 4, 7);

void setup() {
  Serial.begin(9600); // open the serial port at 9600 bps:

  // check if it is solvable
  if (n1*n1 - (l1*l1 + m1*m1) >= 0 || n2*n2 - (l2*l2 + m2*m2) >= 0 || n3*n3 - (l3*l3 + m3*m3) >= 0
  ||  abs(n1/sqrt((l1*l1+m1*m1)))> 1 ||  abs(n2/sqrt((l2*l2+m2*m2)))> 1 ||  abs(n3/sqrt((l3*l3+m3*m3)))> 1 )
  {
  Serial.println("unsolvable"); 
  }

  float a = m1/l1;
  float b = n1/l1;
  
  float A = (a*a+1);
  float B = 2*a*b;
  float C = (1-(b*b));

  float y11 = (-B + sqrt(4*A*C))/(2*A);
  float y12 = (-B - sqrt(4*A*C))/(2*A);

        a = m2/l2;
        b = n2/l2;
  
        A = (a*a+1);
        B = 2*a*b;
        C = (1-(b*b));

  float y21 = (-B + sqrt(4*A*C))/(2*A);
  float y22 = (-B - sqrt(4*A*C))/(2*A);

        a = m3/l3;
        b = n3/l3;
  
        A = (a*a+1);
        B = 2*a*b;
        C = (1-(b*b));

 //Serial.println(4*A*C)

  float y31 = (-B + sqrt(4*A*C))/(2*A);
  float y32 = (-B - sqrt(4*A*C))/(2*A);

  Serial.println(y11); 
  Serial.println(y12); 
  Serial.println(y21); 
  Serial.println(y22);
  Serial.println(y31); 
  Serial.println(y32);

  stepper1.setMaxSpeed(150);
  stepper1.setAcceleration(100);
  stepper1.moveTo(500);

  stepper2.setMaxSpeed(300);
  stepper2.setAcceleration(150);
  stepper2.moveTo(500);

  stepper3.setMaxSpeed(450);
  stepper3.setAcceleration(200);
  stepper3.moveTo(500);
}

void loop() {
  //Serial.println(l1);
  delay(200); 

    stepper1.run();
    stepper2.run();
    stepper3.run();
}

//if we can set the number of steps this would be sufficient to start filling in the kinetics.