#include <Servo.h>

#define pot_x A0
#define pot_y A1

Servo servo1;  // create servo object to control a servo
Servo servo2;  // create servo object to control a servo
Servo servo3;  // create servo object to control a servo
// twelve servo objects can be created on most boards

int arm1 = 25;
int arm2 = 25;

void setup() {
  servo1.attach(3); 
  servo2.attach(5); 
  servo3.attach(6); 
  Serial.begin(9600);
}

void loop() {
  int val_x = analogRead(pot_x);
  int val_y = analogRead(pot_y);

  double x_coord = map(val_x, 0, 1023, 0, 50);
  double y_coord = map(val_y, 0, 1023, 0, 50);
  // Serial.println(x_coord);

  if(x_coord <= 10){    // X Axis limit
    x_coord = 10;
  }

  double y_max = sqrt(((arm1+arm2)*(arm1+arm2))-(x_coord*x_coord));
  if(y_coord >= y_max){
    y_coord = y_max;
  }

  double theta = atan(y_coord/x_coord); // radian
  double theta_deg = theta * (180/PI);  //degree

  double T = (x_coord/(cos(theta)));
  if(T > 50){
    T = 50;
  }

  double alpha = acos((((arm1*arm1)+(arm2*arm2))-(T*T))/(2*arm1*arm2)); //radian
  double alpha_deg = alpha * (180/PI);  // degree

  if(x_coord == 0 && y_coord == 50){
    alpha_deg = 180;
    theta_deg = 90;
  }

  // double alpha_limit = 23;
  // if(alpha_deg < alpha_limit){
  //   alpha_deg = alpha_limit;
  // }

  double gamma = ((180-alpha_deg)/2); // degree
  
  double servo1_val = gamma + theta_deg;
  double servo2_val = alpha_deg;
  double servo3_val = gamma + (180 - (90 + theta_deg));


  Serial.print(servo1_val);
  Serial.print("  ");
  Serial.print(servo2_val);
  Serial.print("  ");
  Serial.print(servo3_val);
  Serial.print("  ");
  Serial.print(x_coord);
  Serial.print("  ");
  Serial.println(y_coord);


  servo1.write(servo1_val);
  servo2.write(servo2_val);
  servo3.write(servo3_val);

  /*
  for (pos = 0; pos <= 180; pos += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
  for (pos = 180; pos >= 0; pos -= 1) { // goes from 180 degrees to 0 degrees
    myservo.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }*/
}