#include <ServoSmooth.h>
#include <math.h>

#define SERVO1 9
#define SERVO2 3
#define L1 1
#define L2 1

ServoSmooth servo1;
ServoSmooth servo2;

float l1;
float l2;

float alphaRad;
float betaRad;

float X;
float Y;

bool flag;
int del = 500;

void setup() {
  l1 = L1;
  l2 = L2;
  
  servo1.attach(SERVO1);
  servo1.setSpeed(60);
  servo1.setAccel(0);
  
  servo2.attach(SERVO2);
  servo2.setSpeed(60);
  servo2.setAccel(0);

  // servo1.setAutoDetach(false);
  // servo2.setAutoDetach(false);

  alphaRad = servo1.getCurrentDeg() * PI / 180;
  betaRad = servo2.getCurrentDeg() * PI / 180;

  Serial.begin(9600);
  Serial.write("Start");
}

void loop() {
  if (Serial.available() > 0)
  {
    X = Serial.parseFloat();
    Y = Serial.parseFloat();
    char r = Serial.read();
    Serial.println(r);

    Serial.print("X:=");
    Serial.println(X);
    Serial.print("Y:=");
    Serial.println(Y);

    betaRad = getBeta(X, Y);
    alphaRad = getAlpha(betaRad, X, Y);
    
    if(isnan(betaRad) || isnan(alphaRad))
    {
      Serial.println("[Error] Input coordinates is not correct");
      return;
    }

    Serial.print("[Command] Servo1 turn to alpha:=");
    Serial.print(alphaRad / PI * 180);
    Serial.println(" degrees;");

    Serial.print("[Command] Servo2 turn to beta:=");
    Serial.print(betaRad / PI * 180);
    Serial.println(" degrees;");

    servo1.setTargetDeg(alphaRad / PI * 180);
    servo2.setTargetDeg(betaRad / PI * 180);
  }

  servo1.tick();
  servo2.tick();
}

float getBeta(float Ex, float Ey)
{
  return acos((l1 * l1 + l2 * l2 - (Ex * Ex + Ey * Ey)) / 2 * l1 * l2);
}

float getAlpha(float betaRad, float Ex, float Ey)
{
  float ll = Ex * Ex + Ey * Ey;
  float alphaOne = acos(
    (l1 * l1 + ll - l2 * l2)
    /(2 * l1 * sqrt(ll)));

  float alphaTwo = atan(Ey / Ex);

  return alphaOne + alphaTwo;
}