#include<math.h>
#include <Servo.h>
Servo servoA;
Servo servoB;
int time=1;
int length=100;
int al=length;
double x;
double y;
int up=A0;
int down=A1;
int left=A2;
int right=A3;
double ycount=length;
double xcount=length;
double preA;
double preB;


void setup() {
  // put your setup code here, to run once:
pinMode(up, INPUT);
pinMode(down, INPUT);
pinMode(left, INPUT);
pinMode(right, INPUT);
Serial.begin(9600);
servoA.attach(3);
servoB.attach(5);
  servoA.write(90);
  servoB.write(90);
}

void angle(double x,double y)
{
  double l=sqrt(x*x+y*y);
  double phi=atan(y/x)*(180/3.1415);
  double theta=acos((l/2)/al)*(180/3.1415);
  double A=phi+theta;
  double B=180-2*theta;
  Serial.print("A value :");
  Serial.println(A);
  Serial.print("B value :");
  Serial.println(B);  
  move(A,B);
}

void move(double A,double B)
{
  if(A<0){
    servoA.write(preA);
  }
  else{
    preA=A;
    servoA.write(A);
  }
    if(B<0){
    servoB.write(preB);
  }
  else{
    preB=B;
    servoB.write(B);
  }
}
void loop() {
  // put your main code here, to run repeatedly:
if(digitalRead(up)==HIGH)
{
  ycount=ycount+time;
  angle(xcount,ycount);
}
if(digitalRead(down)==HIGH)
{
  ycount=ycount-time;
  angle(xcount,ycount);
}
if(digitalRead(right)==HIGH)
{
  xcount=xcount+time;
  angle(xcount,ycount);
}
if(digitalRead(left)==HIGH)
{
  xcount=xcount-time;
  angle(xcount,ycount);
}
}