#include <math.h>
#include <FastTrig.h>

int l1 = 0;
int l2 = 17;
int l3 = 12;

void setup() {
  Serial.begin(9600);
  Serial.println("Introduce los valores de X, Y, Z separados por espacios:");
}

void loop() {
  if (Serial.available() > 0) {
    // Leer X, Y, Z de una sola línea
    double x = Serial.parseInt();
    double y = Serial.parseInt();
    double z = Serial.parseInt();

    // Limpia el buffer del Serial para evitar lecturas falsas en el próximo loop
    while (Serial.available() > 0) Serial.read();

    // Llamar a la función con los valores leídos
    Serial.print("Valores leídos - X: ");
    Serial.print(x);
    Serial.print(", Y: ");
    Serial.print(y);
    Serial.print(", Z: ");
    Serial.println(z);

    // Aquí puedes llamar a CinematicaInversa3DOF(x, y, z, l1, l2, l3);
    // Por ejemplo, sólo imprimir que la función ha sido llamada con los valores proporcionados
    Serial.println("Llamando a CinematicaInversa3DOF con los valores proporcionados...");
    CinematicaInversa3DOF(x,y,z,l1,l2,l3);
    // Preparar para la próxima entrada
    Serial.println("Introduce los valores de X, Y, Z separados por espacios:");
  }
}

int CinematicaInversa3DOF(double px ,double py,double pz, int l1, int l2, int l3){
double th1EU = atan(py/px);

double r1EU = sqrt(pow((px),2) + pow((py),2) );
double r2EU = pz-l1;
double r3EU = sqrt(pow(r1EU,2) + pow(r2EU,2));
double phi2EU = atan2(r2EU,r1EU);

double Hphi1EU = (pow((l3),2)-pow((l2),2)-pow((r3EU),2))/(-2*l2*r3EU);
double phi1EU = acos(Hphi1EU);

double hphi3EU = (pow((r3EU),2)-pow((l2),2)- pow((l3),2))/(-2*l2*l3);
double phi3EU = acos(hphi3EU);

double th2EU = phi2EU+phi1EU;


double th3EU = -deg2rad(180.00000) + phi3EU ;

    Serial.print("Theta1(Cintura): ");
    Serial.print(rad2deg(th1EU));
    Serial.print(", Theta2(Hombro): ");
    Serial.print(rad2deg(th2EU));
    Serial.print(", Theta3(Codo): ");
    Serial.println(rad2deg(th3EU));

}

// Función para convertir radianes a grados
float rad2deg(float radianes) {
  return radianes * (180.000000 / PI);
}

// Función para convertir grados a radianes
float deg2rad(float grados) {
  return grados * (PI / 180.000000);
}