#include <ESP32Servo.h>

Servo baseServo;     // Controla la base (rotación)
Servo eslabon1Servo; // Controla el primer eslabón
Servo eslabon2Servo; // Controla el segundo eslabón

const float L1 = 15.0; // Longitud del primer eslabón
const float L2 = 12.0; // Longitud del segundo eslabón

// Variables de la matriz
const float x_inicial = 2.0;   // Coordenada X de la primera posición 1-1
const float y_inicial = 2.0;   // Coordenada Y de la primera posición 1-1
const float d = 5.0;           // Distancia entre las celdas

void setup() {
  baseServo.attach(26);     // Pin para el servo de la base
  eslabon1Servo.attach(27); // Pin para el primer eslabón
  eslabon2Servo.attach(14); // Pin para el segundo eslabón

  Serial.begin(9600);
}

void loop() {
  // Elegir la posición en la matriz (fila y columna)
  int fila = 3;    // Fila en la matriz (1, 2, o 3)
  int columna = 1; // Columna en la matriz (1, 2, o 3)

  // Obtener las coordenadas X e Y para esa posición
  float x = x_inicial + (columna - 1) * d; // Calcular coordenada X
  float y = y_inicial + (fila - 1) * d;    // Calcular coordenada Y

  // Verificar si las coordenadas están dentro del alcance del brazo
  float distancia = sqrt(x * x + y * y);
  if (distancia > (L1 + L2)) {
    Serial.println("Objetivo fuera del alcance");
    return; // Salir si el objetivo está fuera del alcance
  }

  // Calcular el ángulo de la base
  float theta_base = atan2(y, x); // Ángulo de rotación de la base
  int angle_base = degrees(theta_base);
  angle_base = constrain(angle_base, 0, 180); // Ajustar al rango del servo

  // Cálculo de la cinemática inversa para los eslabones
  float D = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2);
  D = constrain(D, -1.0, 1.0); // Asegurar que D esté en el rango válido

  float theta2 = atan2(sqrt(1 - D * D), D); // Ángulo para el segundo eslabón
  float theta1 = atan2(y, x) - atan2(L2 * sin(theta2), L1 + L2 * cos(theta2)); 
  // Ángulo para el primer eslabón

  // Convertir los ángulos a grados y ajustar para el rango del servo
  int angle1 = degrees(theta1);
  int angle2 = degrees(theta2);

  angle1 = constrain(angle1, 0, 180); // Asegurar que el ángulo esté en el rango permitido
  angle2 = constrain(angle2, 0, 180);

  // Mover los servos
  baseServo.write(angle_base);    // Mueve la base
  eslabon1Servo.write(angle1);    // Mueve el primer eslabón
  eslabon2Servo.write(angle2);    // Mueve el segundo eslabón

  // Imprimir los ángulos para depuración
  Serial.print("Base angle: ");
  Serial.print(angle_base);
  Serial.print(" | Eslabón 1 angle: ");
  Serial.print(angle1);
  Serial.print(" | Eslabón 2 angle: ");
  Serial.println(angle2);

  delay(2000); // Esperar antes del siguiente movimiento
}