float l1 = 50;
float l2 = 50;
float l3 = 50;
float l4 = 50;

float q1, q2, q3, q4;

typedef String  str;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
}

float x = 100;
float y = 100;
float z = 0;
float q = -90;

void loop() {
  q1 = atan2(y,x);
  q1 = int(q1 * 180/PI);
    
  float p1 = sqrt(pow(x,2) + pow(y,2));

  float x1 = p1 - (l3 * cos(radians(q)));
  float y1 = p1 - (l3 * sin(radians(q)));
  
  float p2 = sqrt(pow(x1,2) + pow(y1,2));
  
  float q21 = atan2(y1,x1);
  q21 = int(q21 * 180/PI);
  float q22 = acos(radians((pow(l1,2) + pow(p2,2) - pow(l2,2))) / (2*l1*p2));
  
  q2 = q21 + q22;

  // q1 = int(q1 * 180/PI);
  // q2 = int(q2 * 180/PI);
  // q3 = int(q3 * 180/PI);
  // q4 = int(q4 * 180/PI);
  
  // setCoxa(1,q1);
  // setFemur(1,q2);
  // setTibia(1,q3);
  // setTarsus(1,q4);

  Serial.println(str("p1:") + p1 + str(" x1:") + x1 + str(" y1:") + y1 + str(" p2:") + p2 + str(" q21:") + q21 + str(" q22:") + q22);
  Serial.println(str("q1:") + q1 + str(" q2:") + q21 + str(" q3:") + q3 + str(" q4:") + q4);

  delay(1000);
  while(1){}

}