//==================constant data==============
#define base 225.000
#define shoulder 175.000
#define arm1 340.000
#define elbowh 50.000
#define elbowl 95.000
#define arm2 275.000
#define wrist 85.000
//=============================================
float x, y, z;
float ofsx, ofsy, ofsz;
float xofsx, yofsy, zofsz;
float x0, y0, z0;
float A, B, C;
float j1, j2, j3, j4, j5, j6;



void setup() {
  Serial.begin(9600);
  Serial.println("serial ready!");
  x = 370.000;
  y = 0.000;
  z = 705.000;
  z0 = shoulder+base;
  A = -135.000;
  B = 30.000;   //+B angle is wrong
  C = 0.000;
//==========================================
kinematics();
}

void loop() {

}

void kinematics () {
  float za = -cos(radians(A))*wrist;
  float ya = (sin(radians(A))*wrist);
  float Bdiag = sqrt(2*sq(za)-2*sq(za)*cos(radians(B)));
  float Bb = 90-(180-B)/2;
  float zb = sin(radians(Bb))*Bdiag;
  float xb = -cos(radians(Bb))*Bdiag;
  Serial.println(zb,3);
  Serial.println(xb,3);
  ofsx=xb;
  ofsy=ya;
  ofsz=za+zb;
  Serial.print("X offset: ");Serial.println(ofsx,3);
  Serial.print("Y offset: ");Serial.println(ofsy,3);
  Serial.print("Z offset: ");Serial.println(ofsz,3);
  xofsx = x+ofsx;
  yofsy = y+ofsy;
  zofsz = z+ofsz;
  Serial.print("X offset: ");Serial.println(xofsx,3);
  Serial.print("Y offset: ");Serial.println(yofsy,3);
  Serial.print("Z offset: ");Serial.println(zofsz,3);
  float dh = sqrt(sq(x-x0)+sq(y-y0));
  Serial.print("horizontal distance: ");Serial.print(dh,3);Serial.println("mm");
  float d = sqrt(sq(dh)+sq(z-z0));
  Serial.print("distance: ");Serial.print(d,3);Serial.println("mm");
  float dj5h = sqrt(sq(xofsx-x0)+sq(yofsy-y0));
  float dj5 = sqrt(sq(dj5h)+sq(zofsz-z0));
  Serial.print("distance J5: ");Serial.print(dj5,3);Serial.println("mm");
  Serial.print("distance horizontal J5: ");Serial.print(dj5h,3);Serial.println("mm");
  float arm2diag = sqrt(sq(elbowh)+sq(elbowl+arm2));
  Serial.print("arm2 diag: ");Serial.print(arm2diag,3);Serial.println("mm");
  float j21 = degrees(atan((z-z0)/dh));
  Serial.print("distance angle: ");Serial.print(j21,3);Serial.println("°");
  float j22 = degrees(acos(dj5h/dj5));
  Serial.print("J5 distance angle: ");Serial.print(j22,3);Serial.println("°");
  float j31 = degrees(acos((sq(dj5)-sq(arm2diag)-sq(arm1))/(-2*arm2diag*arm1)));
  Serial.print("J31 distance angle: ");Serial.print(j31,3);Serial.println("°");
  float j32 = degrees(atan(elbowh/(elbowl+arm2)));
  Serial.print("J32 distance angle: ");Serial.print(j32,3);Serial.println("°");
  float j23 = degrees(acos((sq(arm2diag)-sq(dj5)-sq(arm1))/(-2*dj5*arm1)));
  float j51 = 180-j23-j31;
  float j52 = degrees(acos((sq(d)-sq(wrist)-sq(dj5))/(-2*dj5*wrist)));
  j5 = 180-(j51+j52+j32);
  j2 = -((j22+j23)-90);
  j3 = 180-(j31-j32);
  float dhdiag = sqrt(sq(dj5h-xofsx)+sq(y0-yofsy));
  Serial.print("distance horizontal diag: ");Serial.print(dhdiag,3);Serial.println("mm");
  j1 = ((yofsy)/abs(yofsy))*degrees(acos((sq(dhdiag)-sq(dj5h)-sq(dj5h))/(-2*dj5h*dj5h)));
  Serial.print("J1 angle: ");Serial.print(j1,3);Serial.println("°");
  Serial.print("J2 angle: ");Serial.print(j2,3);Serial.println("°");
  Serial.print("J3 angle: ");Serial.print(j3,3);Serial.println("°");
  Serial.print("J5 angle: ");Serial.print(j5,3);Serial.println("°");
}