/*
   >Assignemnt 3 inverse kinematics.ino
   >DM6001 - LOW COST AUTOMATED SYSTEMS 2024/5 SEM1
   >Assignment 3 - Inverse Kinematics
   >Date  :   26/11/2024
   >Author: Jimin P Mathew [24044245]
   >Wokwi : https://wokwi.com/projects/415462765036315649
*/

/*---------------------------------------------------------------------------------
  LIBRARIES
  -----------------------------------------------------------------------------------*/
#include <Stepper.h>  // Include the Stepper motor library
#include <math.h>     // Include the math library for calculations

/*---------------------------------------------------------------------------------
  CONSTANTS
  -----------------------------------------------------------------------------------*/
#define RES 50       // Resolution for path interpolation (number of steps for smooth movement)
#define L1 10        // Arm 1 length in units
#define L2 10        // Arm 2 length in units
#define pi 3.14159   // Pi constant for angle calculations
/*
    conversion factor 200 steps per rev
    = 200 steps per 2mm
    100 steps per mm
    multiply mm by 100 to get steps
    then converto from float to int.
*/
const int stepsPerRevolution = 200;                       // Steps per full revolution for the stepper motors
Stepper stepper_joint1(stepsPerRevolution, 8, 9, 10, 11); // Stepper for X-axis motion
Stepper stepper_joint2(stepsPerRevolution, 4, 5, 6, 7);   // Stepper for Y-axis motion

/*---------------------------------------------------------------------------------
  GLOBAL VARIABLES
  -----------------------------------------------------------------------------------*/
float xpos;                  // Target X position input
float ypos;                  // Target Y position input
int path = 1;                // Path type (default is 1)

// Initial end-effector position (starting point)
float initialX = 10;         // Initial X position
float initialY = 0;          // Initial Y position (origin)

float currentX = initialX;   // Current X coordinate of the end effector
float currentY = initialY;   // Current Y coordinate of the end effector

/*---------------------------------------------------------------------------------
  MAIN SETUP
  -----------------------------------------------------------------------------------*/
void setup()
{
  Serial.begin(9600); // Initialize serial communication at 9600 baud rate
  Serial.println("Please enter X and Y coordinates:"); // Prompt user for input

  int motorSpeed = 3000;               // Set motor speed in RPM

  stepper_joint1.setSpeed(motorSpeed); // Set motor speed for Joint 1
  stepper_joint2.setSpeed(motorSpeed); // Set motor speed for Joint 2
}

/*---------------------------------------------------------------------------------
  MAIN LOOP
  -----------------------------------------------------------------------------------*/
void loop()
{
  if (Serial.available())           // Check if data is available in the serial input buffer to read
  { 
    char inputChar = Serial.read(); // Read the next character from serial input

    switch (inputChar) {
      case 'X': // If 'X' or 'x' is received, read target X position
      case 'x':
        xpos = Serial.parseFloat(); // Parse the input as a float for X position
        break;

      case 'Y': // If 'Y' or 'y' is received, read target Y position
      case 'y':
        ypos = Serial.parseFloat(); // Parse the input as a float for Y position
        break;

      case 'P': // If 'P' or 'p' is received, set the path type
      case 'p' : path = Serial.parseInt();  // Parse the input as an integer for path type
        break;

      case '*':  // Allow multiple commands separated by a '*'
      case '\n': // Execute movement when the Enter key is pressed

        if (path == 1)Path1_move(xpos, ypos);
        if (path == 2)Path2_move(xpos, ypos);
        break;

      default:
        if (inputChar != ' '&& inputChar != '\r')
          Serial.println("Invalid input"); // Display error if input is not recognized
        break;
    }
  }
}

/*---------------------------------------------------------------------------------
  Function to perform Path 1 movement (linear interpolation)
  -----------------------------------------------------------------------------------*/
void Path1_move(float tarX, float tarY)
{
  Serial.println(String(initialX, 3) + ", " + String(initialY, 3)); // Print initial position

  // Check for vertical or non-vertical lines
  if (tarX != initialX)     //this if statement is to solve the problem of a vertical line on a graph being undefined by creating options for both
  {
    float m = (tarY - initialY) / (tarX - initialX);      // Compute the slope (m) of the line
    float c = tarY - m * tarX;                            // Compute the y-intercept (c) of the line

    float deltaX = tarX - initialX;                       // Change in X over the course of the move

    for (float i = 0; i < RES; i++) //Compute intermediate X and Y positions by incrementing i by the amount of times the resolution
    {
      // Compute intermediate X and Y positions
      float interX1 = initialX + deltaX * (i / RES)  ;                  //intermediate location of X found adding the change of X / the incremental step to the initial positon of X
      float interY1 = m * interX1 + c;                                  //intermediate location of Y found by using the equation of the line and Intermediate value of x
      float interX2 = initialX + ((i + 1) / RES) * deltaX;              //next intermediate location of x found same method
      float interY2 = m * interX2 + c;                                  //next intermediate location of y found same method

      // Calculate joint angles and step counts for the first joint
      float alpha1_inc1 = calcAlpha1(interX1, interY1);                 //incremental change of angle in joint 1 after move to first intermediate location
      float alpha1_inc1_deg = radiansToDegrees(alpha1_inc1);            //converts angle from radians to degrees

      float alpha1_inc2 = calcAlpha1(interX2, interY2);                 //incremental change of angle in joint 1 after move to second intermediate location
      float alpha1_inc2_deg = radiansToDegrees(alpha1_inc2);            //converts angle from radians to degrees

      int stepsAlpha1 = degreesToSteps(alpha1_inc1_deg, alpha1_inc2_deg);//converts the angle calculated to steps
      stepper_joint1.step(stepsAlpha1);                                 //moves stepper motor by steps calculated with stepper library function

      // Calculate joint angles and step counts for the second joint
      float alpha2_inc1 = calcAlpha2(interX1, interY1);                 //incremental change of angle in joint 2 after move to first intermediate location
      float alpha2_inc1_deg = radiansToDegrees(alpha2_inc1);            //converts angle from radians to degrees

      float alpha2_inc2 = calcAlpha2(interX2, interY2);                 //incremental change of angle in joint 2 after move to second intermediate location
      float alpha2_inc2_deg = radiansToDegrees(alpha2_inc2);            //converts angle from radians to degrees

      int stepsAlpha2 = degreesToSteps(alpha2_inc1_deg, alpha2_inc2_deg);//converts the angle calculated to steps
      stepper_joint2.step(stepsAlpha2);                                 //moves stepper motor by steps calculated with stepper library function

      currentX = forwardKinematicsX(alpha1_inc2, alpha2_inc2);          // Update current X after movement
      currentY = forwardKinematicsY(alpha1_inc2, alpha2_inc2);          // Update current Y after movement

      Serial.println(String(currentX, 3) + ", " + String(currentY, 3)); //print to serial output where end affector has moved (x,y)
    }
  }
  else
  {
    // Special case for vertical lines (X is constant)
    float deltaY = tarY - initialY;

    for (float i = 0; i < RES; i++) 
    { //Compute intermediate positions for vertical movementincrementing i by the amount of times the resolution
      float interX1 = initialX;                                           //intermediate position of X is the same as initial postion as line is vertical
      float interY1 = initialY + deltaY * (i / RES);                      //intermediate location of Y found adding the change of Y / the incremental step to the initial positon of Y

      float interX2 = initialX;                                           //next intermediate location of x found same method
      float interY2 = initialY + deltaY * ((i + 1) / RES);                //next intermediate location of y found same method

      float alpha1_inc1 = calcAlpha1(interX1, interY1);                   //incremental change of angle in joint 1 after move to first intermediate location
      float alpha1_inc1_deg = radiansToDegrees(alpha1_inc1);              //converts angle from radians to degrees

      float alpha1_inc2 = calcAlpha1(interX2, interY2);                   //incremental change of angle in joint 1 after move to second intermediate location
      float alpha1_inc2_deg = radiansToDegrees(alpha1_inc2);              //converts angle from radians to degrees

      int stepsAlpha1 = degreesToSteps(alpha1_inc1_deg, alpha1_inc2_deg); //converts the angle calculated to steps
      stepper_joint1.step(stepsAlpha1);                                   //moves stepper motor by steps calculated with stepper library function

      float alpha2_inc1 = calcAlpha2(interX1, interY1);                   //incremental change of angle in joint 2 after move to first intermediate location
      float alpha2_inc1_deg = radiansToDegrees(alpha2_inc1);              //converts angle from radians to degrees

      float alpha2_inc2 = calcAlpha2(interX2, interY2);                   //incremental change of angle in joint 2 after move to second intermediate location
      float alpha2_inc2_deg = radiansToDegrees(alpha2_inc2);              //converts angle from radians to degrees

      int stepsAlpha2 = degreesToSteps(alpha2_inc1_deg, alpha2_inc2_deg); //converts the angle calculated to steps
      stepper_joint2.step(stepsAlpha2);                                   //moves stepper motor by steps calculated with stepper library function

      currentX = forwardKinematicsX(alpha1_inc2, alpha2_inc2);            // Update current X after movement
      currentY = forwardKinematicsY(alpha1_inc2, alpha2_inc2);            // Update current Y after movement

      Serial.println(String(currentX, 3) + ", " + String(currentY, 3)); //print to serial output where end affector has moved (x,y)

    }
  }
  // Update the initial position for the next movement
  initialX = currentX; 
  initialY = currentY;
}

/*---------------------------------------------------------------------------------
  Function to perform Path 2 movement (sequential X and Y moves)
  -----------------------------------------------------------------------------------*/
void Path2_move(float tarX, float tarY) 
{ 
  Path1_move(tarX, initialY);                  //x movements made first
  Path1_move(initialX, tarY);                  //y movements made after

}

/*---------------------------------------------------------------------------------
  Helper functions for calculations 
  -----------------------------------------------------------------------------------*/
float calcAlpha1(float x, float y) 
{
  float r = sqrt(x * x + y * y);                                  //distance from inital position to target point
  float phi = atan2(y, x);                                        // inverse kinematics eq.2, Angle of the line to the point
  float theta = acos((r * r + L1 * L1 - L2 * L2) / (2 * L1 * r)); //inverse kinematics eq.7, Elbow angle
  return phi + theta;
}

/*---------------------------------------------------------------------------------
  Helper functions for calculations
  -----------------------------------------------------------------------------------*/
float calcAlpha2(float x, float y) 
{
  float r = sqrt(x * x + y * y);                             //distance from inital position to end affector
  return -acos((r * r - L1 * L1 - L2 * L2) / (2 * L1 * L2)); //eq. 8 but change x and y for r as a^2 = b^2 + c^2 and using x and y is giving errors
}

/*---------------------------------------------------------------------------------
  Function to convert degree to steps
  -----------------------------------------------------------------------------------*/
int degreesToSteps(float degreeA, float degreeB) 
{
  float deltaDegrees = degreeB - degreeA;
  int steps = round(deltaDegrees / 1.8); // divide by 1.8 and round to convert to steps and can have whole number steps
  return steps;                         
}

/*---------------------------------------------------------------------------------
  Function to convert radians to degree
  -----------------------------------------------------------------------------------*/
float radiansToDegrees(float radians) 
{
  return radians * (180.0 / pi);  // Convert radians to degrees
}

/*---------------------------------------------------------------------------------
  Function to calculate X position from angles
  -----------------------------------------------------------------------------------*/
float forwardKinematicsX(float alpha1, float alpha2) 
{
  return L1 * cos(alpha1) + L2 * cos(alpha1 + alpha2);  //X position of EE when angles are known & cos used for x component
}

/*---------------------------------------------------------------------------------
  Function to calculate Y position from angles
  -----------------------------------------------------------------------------------*/
float forwardKinematicsY(float alpha1, float alpha2) 
{
  return L1 * sin(alpha1) + L2 * sin(alpha1 + alpha2);  //X position of EE when angles are known & Sin used for y component
}