#include <Servo.h>
#include <math.h>

Servo bServo;
Servo shServo;
Servo elServo;

void setup() {
  Serial.begin(9600);

  bServo.attach(3);
  shServo.attach(5);
  elServo.attach(6);
}
//----------------------------------------------
void loop() {

  kinamatics();
}
//----------------------------------------------
void kinamatics() {

  float X = 75;
  float Z = 90;

  float C = calculateHypotenuse(X, Z);
Serial.print("Hypotenuse ");
Serial.print (C);
Serial.print( "     ");
 // float numerator = (100 * 100 + C * C - 100 * 100);
//  float denominator = (2 * 100 * C);
    float angleRad = acos(X / C);
  float Aang = angleRad * 180 / PI;
  float Bang = 180 - 90 - Aang;
  Serial.print (" Angle A ");
  Serial.print(Aang);
  Serial.print("  Angle B  ");
  Serial.println(Bang);

}
//----------------------------------------------
float calculateHypotenuse(float X, float Z) {
  // Calculate the square of a and b
  float XSquared = pow(X, 2);
  float ZSquared = pow(Z, 2);

  // Calculate the sum of squares
  float sumOfSquares = XSquared + ZSquared;

  // Calculate the square root of the sum
  float c = sqrt(sumOfSquares);

  return c;
}