#include <math.h>

// Robot link lengths

const double L1 = 0;   // Vertical link length (distance between joints 1 and 2)

const double L2 = 10;  // Length of link 2

const double L3 = 10;  // Length of link 3

// Function for forward kinematics

void forward_kinematics(double theta1, double theta2, double theta3, double &x, double &y, double &z) {

    // Convert angles from degrees to radians

    theta1 = theta1 * M_PI / 180.0;

    theta2 = theta2 * M_PI / 180.0;

    theta3 = theta3 * M_PI / 180.0;

    // Compute the position of the end-effector

    x = (L2 * cos(theta2) + L3 * cos(theta2 + theta3)) * cos(theta1);

    y = (L2 * cos(theta2) + L3 * cos(theta2 + theta3)) * sin(theta1);

    z = L1 + L2 * sin(theta2) + L3 * sin(theta2 + theta3);

}

// Function for inverse kinematics

bool inverse_kinematics(double x, double y, double z, double &theta1, double &theta2, double &theta3) {

    // Solve for theta1 (rotation around the z-axis)

    theta1 = atan2(y, x);

    // Compute the horizontal distance to the wrist center

    double r = sqrt(x * x + y * y);

    double s = z - L1;

    // solve for theta3

    double D = (r * r + s * s - L2 * L2 - L3 * L3) / (2 * L2 * L3);

    if (fabs(D) > 1) {

        return false;  // The position is unreachable

    }

    theta3 = acos(D);

    // Solve for theta2 

    double phi1 = atan2(s, r);

    double phi2 = atan2(L3 * sin(theta3), L2 + L3 * cos(theta3));

    theta2 = phi1 - phi2;

    // Convert radians to degrees

    theta1 = theta1 * 180.0 / M_PI;

    theta2 = theta2 * 180.0 / M_PI;

    theta3 = theta3 * 180.0 / M_PI;

    return true;

}

void setup() {

    Serial.begin(115200);

    while (!Serial) {

        ; 

    }

    Serial.println("Enter '1' for forward kinematics or '2' for inverse kinematics:");

}

void loop() {

    if (Serial.available() > 0) {

        String input = Serial.readString();

        if (input.startsWith("1")) {

            // Forward kinematics: input joint angles, output x, y, z

            double theta1, theta2, theta3, x, y, z;

            Serial.println("Enter theta1 (base angle in degrees):");

            while (Serial.available() == 0);

            theta1 = Serial.parseFloat();

            Serial.println("Enter theta2 (shoulder angle in degrees):");

            while (Serial.available() == 0);

            theta2 = Serial.parseFloat();

            Serial.println("Enter theta3 (elbow angle in degrees):");

            while (Serial.available() == 0);

            theta3 = Serial.parseFloat();

            // Calculate forward kinematics

            forward_kinematics(theta1, theta2, theta3, x, y, z);

            Serial.print("End-effector position: x = ");

            Serial.print(x);

            Serial.print(", y = ");

            Serial.print(y);

            Serial.print(", z = ");

            Serial.println(z);

        } else if (input.startsWith("2")) {

            // Inverse kinematics: input x, y, z, output joint angles

            double x, y, z, theta1, theta2, theta3;

            // Prompt for end-effector position

            Serial.println("Enter x coordinate:");

            while (Serial.available() == 0);

            x = Serial.parseFloat();

            Serial.println("Enter y coordinate:");

            while (Serial.available() == 0);

            y = Serial.parseFloat();

            Serial.println("Enter z coordinate:");

            while (Serial.available() == 0);

            z = Serial.parseFloat();

            // Calculate inverse kinematics

            if (inverse_kinematics(x, y, z, theta1, theta2, theta3)) {

                // Display the result

                Serial.print("Joint angles: theta1 = ");

                Serial.print(theta1);

                Serial.print(" degrees, theta2 = ");

                Serial.print(theta2);

                Serial.print(" degrees, theta3 = ");

                Serial.print(theta3);

                Serial.println(" degrees");

            } else {

                Serial.println("The position is unreachable.");

            }

        } else {

            Serial.println("Invalid option. Please enter '1' or '2'.");

        }

        Serial.println("\nEnter '1' for forward kinematics or '2' for inverse kinematics:");

    }

}