#include <Servo.h>
#include <AccelStepper.h>

// Definisi pin servo
#define SERVO1_PIN 9
#define SERVO2_PIN 10
#define SERVO3_PIN 11

// Definisi pin stepper
#define STEP_PIN 2
#define DIR_PIN 3

Servo servo1;
Servo servo2;
Servo servo3;

AccelStepper stepper(1, STEP_PIN, DIR_PIN);

// Panjang segmen lengan
const float L1 = 21.0;  // Panjang segmen pertama (cm)
const float L2 = 31.0;  // Panjang segmen kedua (cm)
const float L3 = 28.0;  // Panjang segmen ketiga (cm)

// Fungsi untuk menghitung inverse kinematics
void calculateInverseKinematics(float x, float y, float z, float &theta1, float &theta2, float &theta3) {
    // Menghitung sudut basis (theta1)
    theta1 = atan2(y, x) * 180.0 / PI;

    // Menghitung posisi planar (r) dan tinggi (z)
    float r = sqrt(x * x + y * y);

    // Menghitung sudut untuk segmen pertama dan kedua
    float D = (r * r + z * z - L1 * L1 - L2 * L2) / (2 * L1 * L2);
    theta3 = atan2(sqrt(1 - D * D), D) * 180.0 / PI;
    theta2 = (atan2(z, r) - atan2(L2 * sin(theta3 * PI / 180.0), L1 + L2 * cos(theta3 * PI / 180.0))) * 180.0 / PI;
}

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

    // Menghubungkan servo ke pin
    servo1.attach(SERVO1_PIN);
    servo2.attach(SERVO2_PIN);
    servo3.attach(SERVO3_PIN);

    // Mengatur kecepatan stepper
    stepper.setMaxSpeed(1000);
    stepper.setAcceleration(500);
}

void loop() {
    // Contoh posisi target (x, y, z)
    float targetX = 28;
    float targetY = 15.0;
    float targetZ = 5;

    // Variabel untuk menyimpan hasil inverse kinematics
    float theta1, theta2, theta3;

    // Menghitung inverse kinematics
    calculateInverseKinematics(targetX, targetY, targetZ, theta1, theta2, theta3);

    // Menggerakkan stepper motor untuk basis
    long steps = theta1 * (200.0 / 360.0); // Mengkonversi derajat ke langkah
    stepper.moveTo(steps);
    stepper.runToPosition();

    // Menggerakkan servo motor
    servo1.write(90 + theta1);  // Menambahkan offset untuk basis
    servo2.write(90 + theta2);  // Menambahkan offset untuk elbow
    servo3.write(90 + theta3);  // Menambahkan offset untuk wrist

    // Menampilkan sudut-sudut pada Serial Monitor
    Serial.print("Theta1: ");
    Serial.print(theta1);
    Serial.print(" degrees, ");
    Serial.print("Theta2: ");
    Serial.print(theta2);
    Serial.print(" degrees, ");
    Serial.print("Theta3: ");
    Serial.print(theta3);
    Serial.println(" degrees");

    delay(1000);  // Delay 1 detik
}