#include <Servo.h>
#include <math.h>

// Deklarasi servo motor
Servo servo1;
Servo servo2;


// Panjang link
const float L1 = 10.0; // Panjang link 1 dalam cm
const float L2 = 10.0; // Panjang link 2 dalam cm

// Faktor skala
const float alpha = 0.1;

// Fungsi untuk menghitung Jacobian
void calculateJacobian(float theta1, float theta2, float &j11, float &j12, float &j21, float &j22) {
  j11 = -L1 * sin(theta1) - L2 * sin(theta1 + theta2);
  j12 = -L2 * sin(theta1 + theta2);
  j21 = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
  j22 = L2 * cos(theta1 + theta2);
}

// Fungsi untuk menghitung pseudo-inverse dari Jacobian
void calculatePseudoInverseJacobian(float j11, float j12, float j21, float j22, float &j11_inv, float &j12_inv, float &j21_inv, float &j22_inv) {
  float det = j11 * j22 - j12 * j21;
  if (det == 0) {
    // Jika determinan nol, gunakan metode lain atau tambahkan regularisasi
    j11_inv = j12_inv = j21_inv = j22_inv = 0;
  } else {
    float inv_det = 1.0 / det;
    j11_inv = j22 * inv_det;
    j12_inv = -j12 * inv_det;
    j21_inv = -j21 * inv_det;
    j22_inv = j11 * inv_det;
  }
}

// Fungsi untuk menghitung kinematika maju
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 kinematika balik menggunakan Pseudo-Inverse Jacobian
void inverseKinematicsPseudoInverse(float x_d, float y_d, float &theta1, float &theta2) {
  const int maxIter = 100;
  const float tol = 0.01;

  for (int i = 0; i < maxIter; i++) {
    float x, y;
    forwardKinematics(theta1, theta2, x, y);

    float ex = x_d - x;
    float ey = y_d - y;

    if (sqrt(ex * ex + ey * ey) < tol) break;

    float j11, j12, j21, j22;
    calculateJacobian(theta1, theta2, j11, j12, j21, j22);

    float j11_inv, j12_inv, j21_inv, j22_inv;
    calculatePseudoInverseJacobian(j11, j12, j21, j22, j11_inv, j12_inv, j21_inv, j22_inv);

    float dtheta1 = alpha * (j11_inv * ex + j12_inv * ey);
    float dtheta2 = alpha * (j21_inv * ex + j22_inv * ey);

    theta1 += dtheta1;
    theta2 += dtheta2;
  }

  theta1 = theta1 * 180.0 / PI;
  theta2 = theta2 * 180.0 / PI;
}

void setup() {
  servo1.attach(9);
  servo2.attach(10);
}

void loop() {
  float x_d = 15.0;
  float y_d = 10.0;

  float theta1 = 0;
  float theta2 = 0;

  inverseKinematicsPseudoInverse(x_d, y_d, theta1, theta2);

  servo1.write(theta1);
  servo2.write(theta2);

  delay(2000);
}