#include <Servo.h>

#define PI 3.141592654
#define RAD(deg)  (deg*PI/180.0)
#define DEG(rad)  (rad*180.0/PI)

Servo servo1;
Servo servo2;
int pos1;
int pos2;

const float L1 = 1.0;
const float L2 = 1.0;

void setup() {
  Serial.begin(115200);
  // put your setup code here, to run once:
  servo1.attach(3);
  servo2.attach(5);

  servo1.write(0);
  servo2.write(0);

}




void loop() {
  static float y = 0.0;
  static float x = 0.0;
  static int dir = 0;
  const float vel = 0.002;
  
  switch(dir) {
    case 0: x += vel; if (x >= 0.0) ++dir; break;
    case 1: y += vel; if (y >= 1.5) ++dir; break;
    case 2: x -= vel; if (x <= -1.0) ++dir; break;
    default: y -= vel; if (y <= .5) dir = 0; break;
  }

  Serial.print(dir); Serial.print(", "); Serial.print(x); Serial.print(", "); Serial.println(y);

  float t1, t2;
  if (inverse_kinematics(x, y, &t1, &t2)) {
    servo1.write(DEG(t1));
    servo2.write(DEG(t2));
  }
    delay(10);
}


int is_reachable(float x, float y) {
  float distance = sqrt(x * x + y * y);
  return (distance <= (L1 + L2)) && (distance >= fabs(L1 - L2));
}

// Function to compute theta2
int inverse_kinematics(float x, float y, float *t1, float *t2) {
  if (!is_reachable(x, y)) {
    return false;
  }
  float theta2 = acos((x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2));

  float k1 = L1 + L2 * cos(theta2);
  float k2 = L2 * sin(theta2);
  float theta1 = atan2(y, x) - atan2(k2, k1);
  *t1 = theta1;
  *t2 = theta2;
  return true;
}