#include <math.h>
#include <Servo.h>
float BR = 0; //base rotation
float UR = 0; //upper arm rotation
float LR = 0; //lower arm rotation
float calculateAngle(float A,float B,float C){
  return acos((B * B + C * C - A * A) / (2 * B * C)) * 180.0 / PI;}
Servo upperarmS;
Servo lowerarmS;
Servo BASEarmS;
void setup() {
  Serial.begin(9600);
  upperarmS.attach(5, 0, 180);
  lowerarmS.attach(6, 0, 90);
  BASEarmS.attach(7, 0, 180);
}
void loop() {
  float pot1 = map(A0,0,1023,0,15);
  float pot2 = map(A1,0,1023,0,15);
  float pot3 = map(A2,0,1023,0,15);
  delay(1000);
  moveTo(pot1,pot2,pot3);
}
void moveTo(float x, float y ,float z){
  z= z-6.6;
  float FA = sqrt((x*x)+(y*y));//floor angle
  float HA = sqrt((x*x)+(y*y)+(z*z)); // height angle
  int URMOM = (calculateAngle(HA,0,FA));
  LR=URMOM+(calculateAngle(10,HA,10));
  if(x>0 && y==0){
    BR = 180 ;}
  else if (x<0 && y==0){
    BR = 0;}
  else if (x==0 && y>0){
    BR = 90;} 
  else if (x<0){
 BR=calculateAngle(x,y,FA);}
  else if (x>0){
  BR=calculateAngle(x,y,FA)+90;}
  UR=calculateAngle(sqrt((FA*FA)+(z*z)),10,10);
  lowerarmS.write(LR);
  upperarmS.write(UR);
  BASEarmS.write(BR);
    Serial.println(UR);
  Serial.println(LR);
  Serial.println(BR);}