//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.

/*
DM6001 Assignment 2
Ruo Yi Chew (24163147)
*/

#include<Stepper.h>
#include<math.h>

#define RESOLUTION 0.001

#define RAPID 0         // define the movemodes
#define LINE 1
#define ARC_CW 2
#define ARC_CCLW 3
int movemode = 4;       // declare a random number that is not the movemode

const int stepsPerRevolution = 200;
Stepper X_Axis(stepsPerRevolution, 2, 3, 5, 6);   // X Axis motor
Stepper Y_Axis(stepsPerRevolution, 7, 8, 9, 10);  // Y Axis motor

int feed = stepsPerRevolution;      // initial feed
float xval, yval;                   // distance to move
float ival, jval;                   // arc centres from point
float xcur = 0.00, ycur = 0.00;     // set origin point

void setup() {

  Serial.begin(9600);                   // start serial monitor
  X_Axis.setSpeed(30);                  // set speed for motors
  Y_Axis.setSpeed(30);
  Serial.println("Enter the G code");   // eyboard input from User
}

void loop() {

  if (Serial.available()) {  // something has been typed
    char ch = Serial.read();  // read the input character


    switch (ch) {

      case 'G': movemode = Serial.parseInt(); // if it is a G, expect an integer to follow.
        break;                                // value is stored. Do nothing for now.

      case 'X':                               
      case 'x': 
        if (movemode == 4) Serial.println("Movemode not defined");
        else xval = Serial.parseFloat();      // if it is a X, expect a float to follow.
        break;                                // Values are stored. Do nothing for now.

      case 'Y':
      case 'y': 
        if (movemode == 4) Serial.println("Movemode not defined");
        else yval = Serial.parseFloat();      // if it is a Y, expect a float to follow.
        break;                                // Values are stored. Do nothing for now.

      case 'F':
      case 'f': 
        if (movemode == 4) Serial.println("Movemode not defined");
        else feed = Serial.parseInt();        // if it is a F, expect an integer to follow.
        break;                                // Values are stored. Do nothing for now.

      case 'I':
      case 'i': 
        if (movemode == 4) Serial.println("Movemode not defined");
        else ival = Serial.parseFloat();      // if it is an I, expect a float to follow
        break;                                // Values are stored. Do nothing for now. 

      case 'J':
      case 'j': 
        if (movemode == 4) Serial.println("Movemode not defined");
        else jval = Serial.parseFloat();     // if it is a J, expect a float to follow
        break;                               // Values are stored. Do nothing for now.

      case '#':  // if it is a #, process the G codes
        if (movemode == 4) Serial.println("Movemode not defined");
        if (movemode == RAPID) rapid(xval, yval);                         // do rapid using X and Y
        if (movemode == LINE) linemove(xval, yval, feed);                 // do line using X, Y and F
        if (movemode == ARC_CW) arcmove_CW(xval, yval, ival, jval);       // do clockwise arc using X, Y, I and J
        if (movemode == ARC_CCLW) arcmove_CCLW(xval, yval, ival, jval);   // do anticlockwuse arc using X, Y, I and J
        break;

      case '\n':  // return key has been hit.
      break;

      default:
        if (ch != ' ') Serial.println("Not recognized");      // invalid input
        break;
    }
  }
}

// for rapid movement, no coordinate plotted
void rapid(float x, float y)
{
  Serial.print("Rapid to : ");        // print the movemode and final location
  Serial.print(x + xcur);             // final location is current location plus moving distance
  Serial.print(", ");
  Serial.println(y + ycur);

  int xsteps = int(abs(x * 100));     // calculate number of steps to go with resolution 0.01
  int ysteps = int(abs(y * 100));
  float xcount = 0, ycount = 0;       // declare a count

  do {

    if (xcount < xsteps) {                // if count is smaller than number of steps
      if (x > 0) X_Axis.step(1);          // if x is positive, go forward one step
      else if (x < 0) X_Axis.step(-1);    // if x is negative, go backward one step
      xcount = xcount + 1;                // count + 1
    }

    if (ycount < ysteps) {                // same but for y
      if (y > 0) Y_Axis.step(1);          
      else if (y < 0) Y_Axis.step(-1);
      ycount = ycount + 1;
    }

  } while (xcount < xsteps || ycount < ysteps);     // loop this as long as either x count or y count is smaller than respective number of steps

  xcur = xcur + x;    // set the current location
  ycur = ycur + y;

  xval = 0;           // reset the user input value
  yval = 0;
  movemode = 4;       // reset the movemode

  return;
}

// for linear movement
void linemove(float x, float y, int f)
{
  int step = stepsPerRevolution / f;    // calculate stepper step size with feed rate
  int xsteps = int(abs(x * 100));       // calculate number of steps to go with resolution 0.01
  int ysteps = int(abs(y * 100));
  float xstart = xcur;                  // declare the initial position
  float ystart = ycur;
  float xcount = 0, ycount = 0;         // declare a count
  
  Serial.print("Line to : ");           // print the movemode and final location
  Serial.print(xcur + x);               // final location is current location plus moving distance
  Serial.print(", ");
  Serial.println(ycur + y);

  do {

    if (x > 0) Serial.print((xcount + (xstart * 100.0)) / 100.0);           // if x is positive, coordinate is increasing
    else if (x < 0) Serial.print(((xstart * 100.0) - xcount) / 100.0);      // if x is negative, coorindtae is decreasing
    else if (x == 0) Serial.print(xstart);                                  // if x is 0, coordinate is not moving

    Serial.print(", ");

    if (y > 0) Serial.println((ycount + (ystart * 100.0)) / 100.0);         // same but for y
    else if (y < 0) Serial.println(((ystart * 100.0) - ycount) / 100.0);
    else if (y == 0) Serial.println(ystart);

    if (xcount < xsteps) {                    // if count is smaller than number of steps
      if (x > 0) X_Axis.step(step);           // if x is positive, go forward one step size
      else if (x < 0) X_Axis.step(-step);     // if x is negative, go backward one step size
      xcount = xcount + step;                 // count + 1
    }

    if (ycount < ysteps) {                    // same but for y
      if (y > 0) Y_Axis.step(step);
      else if (y < 0) Y_Axis.step(-step);
      ycount = ycount + step;
    }

  } while (xcount < xsteps || ycount < ysteps);     // loop this as long as either x count or y count is smaller than respective number of steps

  xcur = xstart + x;    // set current location
  ycur = ystart + y;

  xval = 0;             // reset the user input value
  yval = 0;             // feed rate is to be maintained until the next value was input
  movemode = 4;        // reset the movemode

  return;
}

// for clockwise arc
void arcmove_CW(float x, float y, float i, float j)
{
  float deltax, deltay;                   // change in x and y
  float xpos, ypos;                       // x and y position along the calculation
  float xstart = xcur;                    // declare the initial position
  float ystart = ycur;                    
  float xbegin, ybegin, xfinal, yfinal;   // for start and end coordinate
  float start_angle, end_angle;           // for start and end angle of arc
  float radius;

  deltax = abs(x - i);
  deltay = abs(y - j);
  radius = sqrt((deltax * deltax) + (deltay * deltay));   // pythagoras to find the radius
  Serial.print("Rad = ");       // print the radius of arc
  Serial.println(radius);
  Serial.print("Centre = ");    // print centre of arc
  Serial.print(xcur + i);       // centre is current location plus distance from point
  Serial.print(", ");
  Serial.println(ycur + j);

  // assume centre as (0, 0) for calculation
  xbegin = - i;     // begin location is how far the centre has moved
  ybegin = - j;
  xfinal = x - i;   // final location is how far the final location has moved from the start location
  yfinal = y - j;

  // the start and end coordinate is true location
  Serial.print("Start Coordinate: ");
  Serial.print(xcur);
  Serial.print(", ");
  Serial.println(ycur);
  Serial.print("End Coordinate: ");
  Serial.print(x + xcur);
  Serial.print(", ");
  Serial.println(y + ycur);

  // base formula is tan(angle) = ycoord / xcoord
  float start_tangent = atan(ybegin / xbegin);    // find the starting angle
  if (start_tangent == 0){                        // if result is 0
    if (xbegin > 0) start_angle = 0;              // if starting x is positive, start angle is 0°
    else if (xbegin < 0) start_angle = 180;       // if starting x is negative, start angle is 180°
  } else {                                        // if result is not 0 (any result or undefined) 
    if (ybegin > 0) start_angle = 90;             // if starting y is positive, start angle is 90°
    else if (ybegin < 0) start_angle = 270;       // if starting y is negative, start angle is 270°
  }

  float end_tangent = atan(yfinal / xfinal);      // same but 0° will be 360° in the end calculation
  if (end_tangent == 0){
    if (xfinal > 0) end_angle = 360;
    else if (xfinal < 0) end_angle = 180;
  } else {
    if (yfinal > 0) end_angle = 90;
    else if (yfinal < 0) end_angle = 270;
  }

  start_angle = (start_angle * PI) / 180.0;       // convert the angle from degrees to radians
  end_angle = (end_angle * PI) / 180.0;

  for (float inc = start_angle; inc < end_angle; inc += RESOLUTION) {   // loop as long as angle is saller than end angle
    
    // if centre is (a, b)
    // round value to 2 decimal places
    xpos = round((j + (radius * sin(inc))) * 100.0) / 100.0;    // x = b + rcos(angle)
    ypos = round((i + (radius * cos(inc))) * 100.0) / 100.0;    // y = a + rsin(angle)

    Serial.print(xpos + xstart);      // x coordinate is moved x plus starting x
    Serial.print(", ");
    Serial.println(ypos + ystart);    // y coordinate is moved y plus starting y

    if ((xpos + xstart) > xcur) X_Axis.step(1);         // if x coordinate is larger than last coordinate, step one step forward
    else if ((xpos + xstart) < xcur) X_Axis.step(-1);   // if x coordinate is smaller ta=han last coordinate, step one step backward

    if ((ypos + ystart) > ycur) Y_Axis.step(1);         // same but for y
    else if ((ypos + ystart) < ycur) Y_Axis.step(-1);

    xcur = xstart + xpos;   // set the last coordinate
    ycur = ystart + ypos;
  }

  xcur = xstart + x;    // set current location
  ycur = ystart + y;

  xpos = 0;             // reset the moved x and y values in the calculation
  ypos = 0;
  xval = 0;             // reset user input values
  yval = 0;
  ival = 0;
  jval = 0;
  movemode = 4;         // reset the movemode

  return;
}

// for counterclockwise arc
void arcmove_CCLW(float x, float y, float i, float j)
{
  float deltax, deltay;                   // change in x and y
  float xpos, ypos;                       // x and y position along the calculation
  float xstart = xcur;                    // declare the initial position
  float ystart = ycur;                    
  float xbegin, ybegin, xfinal, yfinal;   // for start and end coordinate
  float start_angle, end_angle;           // for start and end angle of arc
  float radius;

  deltax = abs(x - i);
  deltay = abs(y - j);
  radius = sqrt((deltax * deltax) + (deltay * deltay));   // pythagoras to find the radius
  Serial.print("Rad = ");       // print the radius of arc
  Serial.println(radius);
  Serial.print("Centre = ");    // print centre of arc
  Serial.print(xcur + i);       // centre is current location plus distance from point
  Serial.print(", ");
  Serial.println(ycur + j);

  // assume centre as (0, 0) for calculation
  xbegin = - i;     // begin location is how far the centre has moved
  ybegin = - j;
  xfinal = x - i;   // final location is how far the final location has moved from the start location
  yfinal = y - j;

  // the start and end coordinate is true location
  Serial.print("Start Coordinate: ");
  Serial.print(xcur);
  Serial.print(", ");
  Serial.println(ycur);
  Serial.print("End Coordinate: ");
  Serial.print(x + xcur);
  Serial.print(", ");
  Serial.println(y + ycur);

  // base formula is tan(angle) = ycoord / xcoord
  float start_tangent = atan(ybegin / xbegin);    // find the starting angle
  if (start_tangent == 0){                        // if result is 0
    if (xbegin > 0) start_angle = 0;              // if starting x is positive, start angle is 0°
    else if (xbegin < 0) start_angle = 180;       // if starting x is negative, start angle is 180°
  } else {                                        // if result is not 0 (any result or undefined) 
    if (ybegin > 0) start_angle = 90;             // if starting y is positive, start angle is 90°
    else if (ybegin < 0) start_angle = 270;       // if starting y is negative, start angle is 270°
  }

  float end_tangent = atan(yfinal / xfinal);      // same but 0° will be 360° in the end calculation
  if (end_tangent == 0){
    if (xfinal > 0) end_angle = 360;
    else if (xfinal < 0) end_angle = 180;
  } else {
    if (yfinal > 0) end_angle = 90;
    else if (yfinal < 0) end_angle = 270;
  }

  start_angle = (start_angle * PI) / 180.0;       // convert the angle from degrees to radians
  end_angle = (end_angle * PI) / 180.0;

  for (float inc = start_angle; inc < end_angle; inc += RESOLUTION) {   // loop as long as angle is saller than end angle
    
    // if centre is (a, b)
    // round value to 2 decimal places
    xpos = round((i + (radius * cos(inc))) * 100.0) / 100.0;    // x = a + rsin(angle)
    ypos = round((j + (radius * sin(inc))) * 100.0) / 100.0;    // y = b + rcos(angle)

    Serial.print(xpos + xstart);      // x coordinate is moved x plus starting x
    Serial.print(", ");
    Serial.println(ypos + ystart);    // y coordinate is moved y plus starting y

    if ((xpos + xstart) > xcur) X_Axis.step(1);         // if x coordinate is larger than last coordinate, step one step forward
    else if ((xpos + xstart) < xcur) X_Axis.step(-1);   // if x coordinate is smaller ta=han last coordinate, step one step backward

    if ((ypos + ystart) > ycur) Y_Axis.step(1);         // same but for y
    else if ((ypos + ystart) < ycur) Y_Axis.step(-1);

    xcur = xstart + xpos;   // set the last coordinate
    ycur = ystart + ypos;
  }

  xcur = xstart + x;    // set current location
  ycur = ystart + y;

  xpos = 0;             // reset the moved x and y values in the calculation
  ypos = 0;
  xval = 0;             // reset user input values
  yval = 0;
  ival = 0;
  jval = 0;
  movemode = 4;         // reset the movemode

  return;
}

// End of code