#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 Transpose
void calculateJacobianTranspose(float theta1, float theta2, float &j11, float &j12, float &j21, float &j22) {
  j11 = -L1 * sin(theta1) - L2 * sin(theta1 + theta2);
  j12 = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
  j21 = -L2 * sin(theta1 + theta2);
  j22 = L2 * cos(theta1 + theta2);
}

// 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 Jacobian Transpose
void inverseKinematicsJacobianTranspose(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;
    calculateJacobianTranspose(theta1, theta2, j11, j12, j21, j22);

    float dtheta1 = alpha * (j11 * ex + j12 * ey);
    float dtheta2 = alpha * (j21 * ex + j22 * 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 = 10.0;
  float y_d = 10.0;

  float theta1 = 0;
  float theta2 = 0;

  inverseKinematicsJacobianTranspose(x_d, y_d, theta1, theta2);

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

  delay(2000);
}