#include <Stepper.h>              // Add the library to control steppers
#include<math.h>                  // Add the library to use math functions
#define STEPS_PER_REV 200         // Steps per revolution for the motors
#define a1 10.0                   // Length of link 1 (a1)
#define a2 10.0                   // Length of link 2 (a2)
#define MOTOR_SPEED 60    // Speed for stepper motors in RPM

#define PT1 1            // Associate the number 1 with linear movement
#define PT2 2         // Associate the number 2 with clockwise circular movement
int path;             // To register the type of path
// Define stepper motor objects
Stepper motor1(STEPS_PER_REV, 2, 3, 4, 5); // Joint 1
Stepper motor2(STEPS_PER_REV, 6, 7, 8, 9);  // Joint 2

// Variables for current position
float currentX = 10.0;
float currentY = 0.0;
float currentTheta1 = 60.0; // Angle of the first joint
float currentTheta2 = 120.0; // Angle of the second joint

// Convert degrees to motor steps
int degreesToSteps(float degrees) {
  return (int)(degrees * STEPS_PER_REV / 360.0);
}

// Function to calculate inverse kinematics and move the steppers
void moveToPosition(float targetX, float targetY) {
  // Calculate inverse kinematics
  float r = sqrt(targetX * targetX + targetY * targetY);
  float theta2 = -acos(((targetX * targetX + targetY * targetY) - (a1 * a1 + a2 * a2)) / (2 * a1 * a2));
  float theta1 = atan2(targetY, targetX) - atan2(a2 * sin(theta2), a1 + a2 * cos(theta2));
  


  
  // Convert angles to degrees
  theta1 *= 180.0 / PI;
  theta2 *= 180.0 / PI;

  // Calculate steps for each motor
  int steps1 = degreesToSteps(theta1 - currentTheta1);
  int steps2 = degreesToSteps(theta2 - currentTheta2);

  // Move motors
  motor1.step(steps1);
  motor2.step(steps2);

  // Update current angles
  currentTheta1 = theta1;
  currentTheta2 = theta2;

  // Update current position
  currentX = targetX;
  currentY = targetY;

  // Output to Serial for verification
  Serial.print("Moved to position: ");
  Serial.print(targetX);
  Serial.print(", ");
  Serial.println(targetY);
}

// Path 1 (Linear Interpolation)
void path1(float targetX, float targetY) {
  Serial.println("Following Path 1...");
  int steps = 50; // Number of points to generate
  for (int i = 0; i <= steps; i++) {
    float t = (float)i / steps;
    float x = currentX + t * (targetX - currentX);
    float y = currentY + t * (targetY - currentY);
    moveToPosition(x, y);
    Serial.print(x);
    Serial.print(",");
    Serial.println(y);
  }
}

// Path 2 (Right-Angle Movement)
void path2(float targetX, float targetY) {
  Serial.println("Following Path 2...");
  float midX = targetX;
  float midY = currentY;

  // Move horizontally
  for (int i = 0; i <= 25; i++) {
    float t = (float)i / 25;
    float x = currentX + t * (midX - currentX);
    float y = currentY;
    moveToPosition(x, y);
    Serial.print(x);
    Serial.print(",");
    Serial.println(y);
  }

  // Move vertically
  for (int i = 0; i <= 25; i++) {
    float t = (float)i / 25;
    float x = midX;
    float y = currentY + t * (targetY - currentY);
    moveToPosition(x, y);
    Serial.print(x);
    Serial.print(",");
    Serial.println(y);
  }
}

void setup() {
  Serial.begin(9600);
  Serial.println("2-Link Robot Control");
  motor1.setSpeed(MOTOR_SPEED); // Set motor speed (RPM)
  motor2.setSpeed(MOTOR_SPEED);
}

void loop()
{
  if (Serial.available()) {   // If something has been entered
    char ch = Serial.read();  // Read the input character

    switch (ch) {   // Analyse the input character
      case'P':                        // If a "G" is read
      case'p':                        // If a "g" is read
        path = Serial.parseInt(); // Convert the numbers following in an integer and store them as a movement type input
        
        break;                        // Exit the switch case

      case 'X':                       // If a "X" is read
      case 'x':                       // If a "x" is read
        targetX = Serial.parseFloat(); // Convert the numbers following in a float and store them as an X input value
        break;                        // Exit the switch case

      case 'Y':                       // If a "Y" is read
      case 'y':                       // If a "y" is read
        targetY = Serial.parseFloat(); // Convert the numbers following in a float and store them as an Y input value
        break;                        // Exit the switch case
      case  '\n':  
        if (path == 1) {
        path1(targetX, targetY);
        } else if (path == 2) {
        path2(targetX, targetY);
        } else {
        Serial.println("Invalid path!");
        }
        break;                                              // Exit the switch case
    }
  }
}