#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

// 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);

Serial.print(j11); Serial.print(" ");
Serial.print(j12); Serial.print(" ");
Serial.print(j21); Serial.print(" ");
Serial.println(j22);


}

// Fungsi untuk menghitung invers Jacobian
bool calculateInverseJacobian(float j11, float j12, float j21, float j22, float &ij11, float &ij12, float &ij21, float &ij22) {
  float det = j11 * j22 - j12 * j21;
  if (fabs(det) < 1e-6) return false; // Cek untuk singularitas

  ij11 = j22 / det;
  ij12 = -j12 / det;
  ij21 = -j21 / det;
  ij22 = j11 / det;
  return true;
}

// Fungsi untuk menghitung kinematika balik
void inverseKinematics(float x, float y, float &theta1, float &theta2) {
  // Inisialisasi sudut sendi
  theta1 = 0.0001;
  theta2 = 0.0001;

  // Tentukan parameter iterasi
  const int maxIter = 100;
  const float tol = 0.01;
  
  for (int i = 0; i < maxIter; i++) {
    // Hitung posisi ujung efektor

  Serial.print(i);
  Serial.print("] ");
  Serial.print("Theta1:");
  Serial.print(theta1*180/PI); // Konversi ke derajat
  Serial.print(" Theta2:");
  Serial.print(theta2*180/PI); // Konversi ke derajat


    float xe = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
    float ye = L1 * sin(theta1) + L2 * sin(theta1 + theta2);

  Serial.print(" X:");
  Serial.print(xe); // Konversi ke derajat
  Serial.print(" Y:");
  Serial.println(ye); // Konversi ke derajat

    // Hitung error
    float ex = x - xe;
    float ey = y - ye;
    
    if (sqrt(ex * ex + ey * ey) < tol) break; // Cek konvergensi

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

Serial.print(j11); Serial.print(" ");
Serial.print(j12); Serial.print(" ");
Serial.print(j21); Serial.print(" ");
Serial.println(j22);

    // Hitung invers Jacobian
    float ij11, ij12, ij21, ij22;
    if (!calculateInverseJacobian(j11, j12, j21, j22, ij11, ij12, ij21, ij22)) {
      Serial.println("Singular");
      break; // Jika Jacobian singular
    }
    // Hitung perubahan sudut sendi
    float dtheta1 = ij11 * ex + ij12 * ey;
    float dtheta2 = ij21 * ex + ij22 * ey;

    // Perbarui sudut sendi
    theta1 += dtheta1; 
    theta2 += dtheta2;
    theta1 = fmod(theta1, TWO_PI);
    theta2 = fmod(theta2, TWO_PI);
  }

  // Konversi sudut dari radian ke derajat
  theta1 = theta1 * 180.0 / PI;
  theta2 = theta2 * 180.0 / PI;
}

void setup() {
  Serial.begin(9600);
  // Menetapkan pin servo
  servo1.attach(9);
  servo2.attach(10);
}

void loop() {
  // Posisi yang diinginkan untuk ujung efektor
  float x = 20.0; // cm
  float y = 0.0; // cm

  // Variabel untuk menyimpan sudut sendi
  float theta1, theta2;
  
  // Menghitung sudut sendi
  inverseKinematics(x, y, theta1, theta2);

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

  // Menggerakkan servo ke sudut yang dihitung
  servo1.write(theta1);
  servo2.write(theta2);
  
  // Menunggu sejenak untuk melihat pergerakan
  while(1);
  delay(2000);
}