float Px;
float Py;
float Pz;
float a = 7, d = 2;      // radius of moving platform and fixed base.
float l = 4, h = 15;     // Length of the active and passive links
String receivedData;
int theta, theta3;
int theta2_degree = 0;
#include <Servo.h>
Servo myservo1;
Servo myservo2;
Servo myservo3;

void setup() {
  Serial.begin(9600);
  myservo1.attach(3); 
  myservo2.attach(5); 
  myservo3.attach(6); 
  Serial.println("G");
}

void loop() {
  if (Serial.available() > 0) {
  // Read the incoming string
  receivedData = Serial.readStringUntil('\n');
    
  // Parse the string
  int firstComma = receivedData.indexOf(',');
  int secondComma = receivedData.indexOf(',', firstComma + 1);
    
  Px = receivedData.substring(0, firstComma).toFloat();
  Py = receivedData.substring(firstComma + 1, secondComma).toFloat();
  Pz = receivedData.substring(secondComma + 1).toFloat();
  
  Serial.print(Px); Serial.print("\t");
  Serial.print(Py); Serial.print("\t");
  Serial.println(Pz);
  Equation();
  }
}

void Equation() {
// Asin(theta) + Bcos(theta) = C
// Pzsin(theta) + Q1cos(theta) = -G1  
///// Loop 1 /////
  float Q1 = Px + d - a;
  float G1 = ((pow(h,2)-pow(Q1,2)-pow(Py,2)-pow(Pz,2)-pow(l,2))/(2*l));
  Serial.print(Q1); Serial.print("\t");
  Serial.print(G1); Serial.print("\t");
  float D = pow((pow(Pz,2)+pow(Q1,2)),0.5);
  float alpha = atan2((Q1/D),(Pz/D));
  float theta  = asin(-G1/D) - alpha;
  float theta_degree = theta * (180/PI);
  Serial.print(theta_degree);
  Serial.println("\tdegree");

  ///// Loop 2 /////
  float Q21 = Px - (0.5*d) + (0.5*a);
  float Q22 = Py - (0.866*d) + (0.866*a);
  float G21 = ((pow(h,2)-pow(Q21,2)-pow(Q22,2)-pow(Pz,2)-pow(l,2))/l);
  float G22 = Q21 + (1.732*Q22);
  float G23 = (-2)*(Pz);
  Serial.print(Q21); Serial.print("\t");
  Serial.print(Q22); Serial.print("\t");
  Serial.print(G21); Serial.print("\t");
  Serial.print(G22); Serial.print("\t");
  Serial.print(G23); Serial.print("\t");
// G23sin(theta2) + G22cos(theta2) = G21
  float D2 = pow((pow(G23,2)+pow(G22,2)), 0.5);
  float alpha2 = atan2((G22/D2),(G23/D2));
  float theta2 = asin(G21/D2) - alpha2;
  float theta2_degree = theta2*(180/PI);
  int AA = abs(theta2_degree);
  Serial.print(theta2_degree); Serial.println("\tdegree");

///// Loop 3 /////
  float Q31 = Px - (0.5*d) + (0.5*a);
  float Q32 = Py + (0.866*d) - (0.866*a);
  float G31 = ((pow(h,2)-pow(Q31,2)-pow(Q32,2)-pow(Pz,2)-pow(l,2))/l);
  float G32 = Q31 -(1.732*Q32);
  float G33 = -(2*Pz);
// G33sin(theta3) + G32cos(theta) = G31
  float D3 =pow((pow(G33,2)+pow(G32,2)),0.5);
  float alpha3 = atan2((G32/D3),(G33/D3));
  float theta3 = asin(G31/D3) - alpha3;
  float theta3_degree = theta3 * (180/PI);
  Serial.print(theta3_degree); Serial.println("\tdegree");

  myservo3.write(150); 
  delay (1000);
  myservo3.write(0); 
  delay (1000);
  myservo3.write(AA);
  delay (1000);
  myservo3.write(90); 
}