/*
   >CNC Assignment FINAL.ino
   >DM6001 - LOW COST AUTOMATED SYSTEMS 2024/5 SEM1
   >Assignment 2 - CNC X-Y Table
   >Date:   22/10/2024
   >Author: Jimin P Mathew [24044245]
*/
/*---------------------------------------------------------------------------------
  LIBRARIES
  -----------------------------------------------------------------------------------*/
#include <Stepper.h>
#include <math.h>
/*---------------------------------------------------------------------------------
  CONSTANTS
  -----------------------------------------------------------------------------------*/
#define RESOLUTION 0.01  //resolution of angle for arc
// Movement Modes
#define RAPID 0     //Rapid move
#define LINE 1      //Line mode
#define ARC_CW 2    //CW Arc
#define ARC_CCLW 3  //CCW Arc

/*---------------------------------------------------------------------------------
  GLOBAL VARIABLES
  -----------------------------------------------------------------------------------*/
int movemode; //stores the movement mode from G-code
int speed;    //stores the speed from F command
/*
    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;
Stepper X_Axis(stepsPerRevolution, 2, 3, 5, 6);   // Create an instance of X AXIS Stepper
Stepper Y_Axis(stepsPerRevolution, 7, 8, 9, 10);  // Create an instance of Y AXIS Stepper
float xpos = 0, ypos = 0; // Initialize current position of X and Y
float ival = 0, jval = 0; // Initialize current position of I and J
const int blade = 13;     //CNC cutting blade arm moves down

float currentX = 0.0;     // Initial position of X axis in mm
float currentY = 0.0;     // Initial position of Y axis in mm

/*---------------------------------------------------------------------------------
  MAIN SETUP
  -----------------------------------------------------------------------------------*/
void setup()
{
  Serial.begin(9600);   //Initialize serial communication
  X_Axis.setSpeed(30);  // set x axis speed
  Y_Axis.setSpeed(30);  // set y axis speed
  Serial.println("CNC type X-Y Table");
  Serial.println("------------------");
  Serial.println("Please enter the G-code commands below to proceed"); // Print instruction to entre G-code
  Serial.println("-------------------------------------------------");
}

/*---------------------------------------------------------------------------------
  MAIN LOOP
  -----------------------------------------------------------------------------------*/
void loop()
{
  if (Serial.available())     //Checks if there are any characters to read
  {
    char ch = Serial.read();  // Read the characters available

    switch (ch)
    {
      case 'G': // If character is G
      case 'g':
        movemode = Serial.parseInt(); // Read movement mode after the letter G,i.e, 01,02 or 03
        Serial.print("Received G-code movement command: ");
        // Print the type of movement whether rapid(00), line(01), CW arc(02), or CCW arc(03)
        switch (movemode)
        {
          case 00: Serial.println("Rapid mode");
          break;
          
          case 01: Serial.println("Line mode");
          break;

          case 02: Serial.println("Clockwise Arc mode");
          break;

          case 03: Serial.println("Counter clockwise arc mode");
          break;
        }
        break;

      case 'X': // If character is 'X' or 'x'
      case 'x':
        xpos += Serial.parseFloat(); // Read and update current X position
        Serial.print("Received X,Y co-ordinates: ");
        Serial.print(xpos);          //Print the current updated x position
        break;

      case 'Y': // If character is 'Y' or 'y'
      case 'y':
        ypos += Serial.parseFloat(); // Read and update current Y position
        Serial.print(',');
        Serial.println(ypos);        //Print the current updated Y position
        break;

      case 'F': // If character is 'F' or 'f'
      case 'f':
        speed = Serial.parseInt();  // Read speed and store for next move
        Serial.print("Feedrate: ");
        Serial.println(speed);      //Print speed
        X_Axis.setSpeed(speed);  // set x axis speed
        Y_Axis.setSpeed(speed);  // set y axis speed
        break;

      case 'I': // If character is 'i' or 'I'
      case 'i':
        ival = currentX + Serial.parseFloat(); // Read I-value and store X coordinate of arc centre
        break;

      case 'J': // If character is 'J' or 'j'
      case 'j':
        jval = currentY + Serial.parseFloat(); // Read J-value and store Y coordinate of arc center (radius)
        break;

      case '#': // If character is '#' or '\n'[newline]
      case '\n':
        if (movemode == RAPID)     //If movement mode is line(1)
        {
          rapidMove(xpos, ypos);   //Move in a line towards xpos and ypos position
        }
        else if (movemode == LINE) //If movement mode is line(1)
        {
          digitalWrite(blade, HIGH);// Activates the LED to symbolize the blade of the CNC

          lineMove(xpos, ypos);    //Move in a line towards xpos and ypos position

          digitalWrite(blade, LOW);// Deactivates the LED to symbolize the blade of the CNC
        }
        else if (movemode == ARC_CCLW || movemode == ARC_CW)//If movement mode is either of clockwise or counter clockwise arc (3)
        {
          digitalWrite(blade, HIGH);                 // Activates the LED to symbolize the blade of the CNC

          arcMove(xpos, ypos, ival, jval, movemode); // Move in an counter clockwise arc with desired centre and radius

          digitalWrite(blade, LOW);                  // Deactivates the LED to symbolize the blade of the CNC
        }
        break;

      default:
        Serial.print(ch); //Print unrecognised character
        if (ch != ' ' && ch != '\r') Serial.println("Not recognised");// Print Not Recognised if input is error
        break;
    }
  }
}

/*---------------------------------------------------------------------------------
  FUNCTION FOR RAPID MOVEMENT
  -----------------------------------------------------------------------------------*/
void rapidMove(float x, float y)
{
  // Convert target positions to steps [1 mm = 100 steps]
  int target_xsteps = int(x * 100);
  int target_ysteps = int(y * 100);

  // Calculate differences between current and target positions
  int dx = target_xsteps - int(currentX * 100); // Ensure currentX is in steps
  int dy = target_ysteps - int(currentY * 100); // Ensure currentY is in steps

  // Determine the maximum number of steps required
  int steps = max(abs(dx), abs(dy));

  // Initialize step counts for x and y axes
  int xcount = 0;
  int ycount = 0;

  // Move along the line until both axes reach their target positions
  while (xcount < abs(dx) || ycount < abs(dy))
  {
    // Move along the X-axis if there are remaining steps
    if (xcount < abs(dx)) 
    {
      int xdir = (dx > 0) ? 1 : -1;   // Determine direction
      X_Axis.step(xdir);              // Move in the appropriate direction
      currentX += xdir * (1.0 / 100); // Update current position in mm
      xcount++;                       // Increment step count for the X-axis
    }

    // Move along the Y-axis if there are remaining steps
    if (ycount < abs(dy)) 
    {
      int ydir = (dy > 0) ? 1 : -1;   // Determine direction
      Y_Axis.step(ydir);              // Move in the appropriate direction
      currentY += ydir * (1.0 / 100); // Update current position in mm
      ycount++;                       // Increment step count for the Y-axis
    }
  }
  
  // Update global xpos and ypos after movement
  xpos = currentX;
  ypos = currentY;
}

/*---------------------------------------------------------------------------------
  FUNCTION TO MOVE IN A LINE USING BRESENHAM'S LINE ALGORITHM
  -----------------------------------------------------------------------------------*/
void lineMove(float x, float y)
{ 
  // Convert target positions to steps [1 mm = 100 steps]
  int target_xsteps = int(x * 100);
  int target_ysteps = int(y * 100);

  // Calculate differences between current and target positions
  int dx = target_xsteps - int(currentX * 100); // Ensure currentX is in steps
  int dy = target_ysteps - int(currentY * 100); // Ensure currentY is in steps

  // Determine the maximum number of steps required
  int steps = max(abs(dx), abs(dy));

  // Initialize step counts for x and y axes
  int xcount = 0;
  int ycount = 0;

  // Move along the line until both axes reach their target positions
  while (xcount < abs(dx) || ycount < abs(dy))
  {
    // Move along the X-axis if there are remaining steps
    if (xcount < abs(dx)) 
    {
      int xdir = (dx > 0) ? 1 : -1;   // Determine direction
      X_Axis.step(xdir);              // Move in the appropriate direction
      currentX += xdir * (1.0 / 100); // Update current position in mm
      xcount++;                       // Increment step count for the X-axis
    }

    // Move along the Y-axis if there are remaining steps
    if (ycount < abs(dy)) 
    {
      int ydir = (dy > 0) ? 1 : -1;   // Determine direction
      Y_Axis.step(ydir);              // Move in the appropriate direction
      currentY += ydir * (1.0 / 100); // Update current position in mm
      ycount++;                       // Increment step count for the Y-axis
    }
    
    // Prints the travel co-ordinates in x,y format in mm
    Serial.print(currentX);
    Serial.print(",");
    Serial.println(currentY);
  }
  // Update global xpos and ypos after movement
  xpos = currentX;
  ypos = currentY;
}

/*---------------------------------------------------------------------------------
  FUNCTION TO MOVE IN AN ARC USING LINEMOVE FUNCTION
  -----------------------------------------------------------------------------------*/
void arcMove(float x, float y, float i, float j, float dir)
{
  // Calculate the radius
  float dx = currentX - i;
  float dy = currentY - j;
  float radius = sqrt(dx * dx + dy * dy);

  // Find angle of arc (sweep)
  float angle1 = atan2(dy, dx);
  float angle2 = atan2(y - j, x - i);
  float theta = angle2 - angle1;

  // Adjust for direction (CW or CCW)
  if (dir == ARC_CCLW && theta < 0) 
  {
    angle2 += 2 * PI;      // Counter-Clockwise
  } 
  else if (dir == ARC_CW && theta > 0) 
  {
    angle1 += 2 * PI;      // Clockwise
  }

  theta = angle2 - angle1; // Sweep angle

  // Arc length and segments count
  float arcLength = abs(theta) * radius;
  int segments = ceil(arcLength / RESOLUTION); // Divide arc into segments

  // Print initial position
  Serial.print(currentX);
  Serial.print(",");
  Serial.println(currentY);

  for (int s = 0; s < segments; s++) 
  {
    // Interpolate around the arc
    float scale = (float)s / (float)segments;
    float angle3 = angle1 + (theta * scale);

    // Calculate the interpolated X, Y positions
    float nextX = i + cos(angle3) * radius;
    float nextY = j + sin(angle3) * radius;

    // Move to the next interpolated position using lineMove
    lineMove(nextX, nextY);
  }

  // After the loop, ensure final position is set exactly to target X, Y
  lineMove(x, y);
  currentX = x;
  currentY = y;
}

/*--------------------------------------------**END OF CODE**--------------------------------------------*/