#define pi  3.14159 
#define ANG_MAX pi/3 
#define ANG_MIN -pi/4 

const float angBase[6] = {64.6,355.4,304.6,235.4,184.6,115.4};  
const float angPlat[6] = {40.6,19.4,280.6,259.4,160.6,139.4};   
const float beta[6] = {180.0,240.0,60.0,120.0,300.0,0.0};       

const float r_b = 104.89/1000, r_p = 90.545/1000, h_0 = 85.112/1000, 
l_var = 100.0/1000, l_brz = 15.0/1000; 

float Bx_b[6], By_b[6], Bz_b[6];      
float Px_p[6], Py_p[6], Pz_p[6];      

static float T[3], R[3][3], Q[3][6], L[3][6], 
longitud[6],a[6],b[6],c[6],d[6],ang[6]; 

void setup() { 
  Serial.begin(9600); // Inicialización del puerto serie

  for (int j=0; j<=5; j++){ 
    Bx_b[j]=r_b*cos(angBase[j]*pi/180);  
    By_b[j]=r_b*sin(angBase[j]*pi/180);  
    Px_p[j]=r_p*cos(angPlat[j]*pi/180);  
    Py_p[j]=r_p*sin(angPlat[j]*pi/180);  
  } 

  // Espera a que el usuario ingrese los valores
  Serial.println("Ingrese los valores de x, y, z, alpha, theta, psi separados por comas:");
  while (Serial.available() == 0) {} // Espera a que haya datos disponibles

  String input = Serial.readStringUntil('\n'); // Lee la línea de entrada
  input.trim(); // Elimina espacios en blanco al inicio y final

  // Divide la entrada en partes
  int index = 0;
  float values[6];
  while (input.length() > 0 && index < 6) {
    int commaIndex = input.indexOf(',');
    if (commaIndex == -1) {
      values[index] = input.toFloat();
      break;
    } else {
      values[index] = input.substring(0, commaIndex).toFloat();
      input = input.substring(commaIndex + 1);
    }
    index++;
  }

  // Llama a giroServos con los valores ingresados
  giroServos(values[0], values[1], values[2], values[3], values[4], values[5]);
} 

void giroServos(float x,float y,float z,float alpha_grad,float theta_grad,float psi_grad){ 
  T[0] = x/1000;                     
  T[1] = y/1000; 
  T[2] = z/1000 + h_0; 

  float alpha = alpha_grad*pi/180; 
  float theta = theta_grad*pi/180; 
  float psi = psi_grad*pi/180; 

  R[0][0] = cos(alpha)*cos(theta); 
  R[0][1] = -sin(alpha)*cos(psi)+cos(alpha)*sin(theta)*sin(psi); 
  R[0][2] = sin(alpha)*sin(psi)+cos(alpha)*sin(theta)*cos(psi); 
  R[1][0] = sin(alpha)*cos(theta); 
  R[1][1] = cos(alpha)*cos(psi)+sin(alpha)*sin(theta)*sin(psi); 
  R[1][2] = -cos(alpha)*sin(psi)+sin(alpha)*sin(theta)*cos(psi); 
  R[2][0] = -sin(theta); 
  R[2][1] = cos(theta)*sin(psi); 
  R[2][2] = cos(theta)*cos(psi);      

  for (int j=0; j<=5; j++){ 
    Q[0][j] = T[0] + R[0][0]*Px_p[j] + R[0][1]*Py_p[j] + R[0][2]*Pz_p[j];  
    Q[1][j] = T[1] + R[1][0]*Px_p[j] + R[1][1]*Py_p[j] + R[1][2]*Pz_p[j]; 
    Q[2][j] = T[2] + R[2][0]*Px_p[j] + R[2][1]*Py_p[j] + R[2][2]*Pz_p[j]; 
  }  

  for (int j=0; j<=5; j++){ 
    L[0][j] = Q[0][j] - Bx_b[j];  
    L[1][j] = Q[1][j] - By_b[j]; 
    L[2][j] = Q[2][j] - Bz_b[j]; 
  } 

  for (int j=0; j<=5; j++){ 
    longitud[j] = sqrt(pow(L[0][j],2) + pow(L[1][j],2) + pow(L[2][j],2));  
  } 

  for (int j=0; j<=5; j++){ 
    a[j] = 2 * l_brz * L[2][j]; 
    b[j] = 2 * l_brz * (sin(beta[j]*pi/180)*L[1][j] + cos(beta[j]*pi/180)*L[0][j]); 
    c[j] = pow(longitud[j],2) - pow(l_var,2) + pow(l_brz,2); 
    d[j] = constrain(c[j]/sqrt(pow(a[j],2)+pow(b[j],2)), -1, 1); 
    ang[j] = constrain(asin(d[j]) - atan(b[j]/a[j]) ,ANG_MIN, ANG_MAX); 
  } 

  // Imprime los ángulos calculados
  for (int j=0; j<=5; j++){
    Serial.print("Ángulo ");
    Serial.print(j+1);
    Serial.print(": ");
    Serial.println(ang[j] * 180 / pi); // Convertir a grados para imprimir
  }
} 

void loop() { 
  // El bucle principal está vacío ya que el cálculo se realiza en setup
}