#include <math.h>

// Panjang lengan
const float l1 = 30.0; // cm
const float l2 = 30.0; // cm

// Posisi target
const float x_target = 30.0; // cm
const float y_target = 10.0; // cm

// Parameter iterasi
const float tolerance = 0.01; // Toleransi untuk konvergensi
const int max_iterations = 100;

// Fungsi untuk menghitung posisi end-effector dari sudut sendi
void forwardKinematics(float theta1, float theta2, float &x, float &y) {
  x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
  y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
}

// Fungsi untuk menghitung Jacobian
void computeJacobian(float theta1, float theta2, float J[2][2]) {
  J[0][0] = -l1 * sin(theta1) - l2 * sin(theta1 + theta2);
  J[0][1] = -l2 * sin(theta1 + theta2);
  J[1][0] = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
  J[1][1] = l2 * cos(theta1 + theta2);
}

// Fungsi untuk invers matriks 2x2
void inverseJacobian(float J[2][2], float J_inv[2][2]) {
  float det = J[0][0] * J[1][1] - J[0][1] * J[1][0];
  J_inv[0][0] = J[1][1] / det;
  J_inv[0][1] = -J[0][1] / det;
  J_inv[1][0] = -J[1][0] / det;
  J_inv[1][1] = J[0][0] / det;
}

void setup() {
  Serial.begin(9600);

  // Inisialisasi sudut sendi (perkiraan awal)
  float theta1 = 0.0;
  float theta2 = 0.0;

  // Iterasi untuk mencari solusi
  for (int iter = 0; iter < max_iterations; iter++) {
    float x, y;

  // Cetak hasil
  Serial.print("Theta1: ");
  Serial.println(theta1 * 180.0 / PI); // Konversi ke derajat
  Serial.print("Theta2: ");
  Serial.println(theta2 * 180.0 / PI); // Konversi ke derajat
    forwardKinematics(theta1, theta2, x, y);

    // Cek konvergensi
    float dx = x_target - x;
    float dy = y_target - y;
    if (sqrt(dx * dx + dy * dy) < tolerance) {
      break;
    }

    // Hitung Jacobian
    float J[2][2];
    computeJacobian(theta1, theta2, J);

    // Hitung invers Jacobian
    float J_inv[2][2];
    inverseJacobian(J, J_inv);

    // Update sudut sendi
    theta1 += J_inv[0][0] * dx + J_inv[0][1] * dy;
    theta2 += J_inv[1][0] * dx + J_inv[1][1] * dy;
  }

  // Cetak hasil
  Serial.println();
  Serial.println("Hasil Akhir");
  Serial.print("Theta1: ");
  Serial.println(theta1 * 180.0 / PI); // Konversi ke derajat
  Serial.print("Theta2: ");
  Serial.println(theta2 * 180.0 / PI); // Konversi ke derajat
}

void loop() {
  // Tidak ada tindakan yang diperlukan di loop
}