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

// positive steps of stepper motor indicates clockwise movement
// negative steps of stepper motor indicates anticlockwise movement
// resolution of coordinates defines by angle resolution (0.01)
// initially, both motors are at 0° hence coordinate of (20.00,00.00)
// length of both robotic arm is 10 cm.
// the system is not expected to move into negative Y-axis due to the incapability in real-life

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

#define RESOLUTION 0.01

int path = 0;       // declare a random number that is not the movemode

const int stepsPerRevolution = 200;
Stepper Motor1(stepsPerRevolution, 2, 3, 5, 6);   // Motor 1
Stepper Motor2(stepsPerRevolution, 7, 8, 9, 10);  // Motor 2

float xval, yval;           // final coordinate
float xcur = 20, ycur = 0;  // set initial point
float angle_Motor1 = 0.00;  // set original angles
float angle_Motor2 = 0.00;
float x_ori = 10.00;        // set origin point
float y_ori = 0.00;
float xpos_Motor2 = 10.00;  // set the initial point
float ypos_Motor2 = 0.00;
float xfinal, yfinal;
float x_partial, y_partial;

void setup() {
  Serial.begin(9600);           // start serial monitor
  Motor1.setSpeed(30);          // set speed for motors
  Motor2.setSpeed(30);
  Origin(x_ori,y_ori);          // move both motors to origin position
  Serial.println("Enter the Movement Mode");    // keyboard input from User
}

void loop() {

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


    switch (ch) {

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

      case 'X':                               
      case 'x': 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': yval = Serial.parseFloat();     // if it is a Y, expect a float to follow.
        break;                                  // Values are stored. Do nothing for now.

      case '\n':  // if it is a #, process the G codes
        if (path == 0) Serial.println("Path not declared");
        if (path == 1) Path_1(xval, yval);                 // Move in Path 1 using X and Y
        if (path == 2) Path_2(xval, yval);                 // Move in Path 2 using X and Y
        break;

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

// move to origin
void Origin(float x, float y){
  float midpoint = x / 2;   // Calculate the midpoint of the movement
  float steps = round(((acos((midpoint / 10)) * 180.0) / PI) * 100.0) / 100.0;  // Calculate the steps to move

  for (float angle = 0.01; angle < steps; angle += RESOLUTION){
    Motor1.step(1);     // for every angle increment in Motor 1
    Motor2.step(-2);    // Motor 2 moves double the angle backwards
  }

  xcur = x;   // set the current position of the end effector
  ycur = y;

  angle_Motor1 = 60.00;     // set the angle moved by the motors
  angle_Motor2 = 120.00;

  // Calculate the position of the second motor
  xpos_Motor2 = midpoint;  
  ypos_Motor2 = round((10 * sin((angle_Motor1 * PI) / 180)) * 100.0) / 100.0;
}

// Path 1
void Path_1(float x, float y){
  float gradient = (y - ycur) / (x - xcur);   // Calculate gradient of slanted line path
  float steps = abs(x - xcur) / RESOLUTION;   // Calculate amount of steps to move in terms of x displacement

  // calculate the movement needed for every steps
  for (int i = 0; i <= steps; i++){
    // calculate the point to move to based on the desired destination
    if ((x - xcur) > 0) x_partial = xcur + (i * RESOLUTION);
    else if ((x - xcur) < 0) x_partial = xcur - (i * RESOLUTION);
    if ((y - ycur) > 0) y_partial = ycur + (gradient * i * RESOLUTION);
    else if ((y - ycur) < 0) y_partial = ycur - (gradient * i * RESOLUTION);

    // Calculate the final angle of Motor 1 in this movement step
    float hypo = sqrt((y_partial * y_partial) + (x_partial * x_partial));
    float yangle1_Motor1 = round(((acos((hypo * hypo) / (20 * hypo)) * 180.0) / PI) * 100.0) / 100.0;
    float yangle2_Motor1 = round(((atan2(y_partial, x_partial) * 180.0) / PI) * 100.0) / 100.0;
    float yangle_Motor1 = yangle1_Motor1 + yangle2_Motor1;
    // if the movement falls in quadrant 2, redefine the angle
    if (x_partial < 0) yangle_Motor1 = 180 - yangle_Motor1;

    // calculate the final angle of Motor 2 in this movement step
    float ypos_final_Motor2 = round((10 * sin((yangle_Motor1 * PI) / 180)) * 100.0) / 100.0;
    float yangle1_Motor2 = round(((asin((ypos_final_Motor2 - y_partial) / 10) * 180) / PI) * 100.0) / 100.0;
    // if the movement falls into quadrant 2, redefine the angle
    if (x_partial < 0) yangle1_Motor2 = 180 - yangle1_Motor2;
    float yangle_Motor2 = yangle_Motor1 + yangle1_Motor2;
  
    // calculate the steps to move by both motor in this movement step
    float ysteps_Motor1 = yangle_Motor1 - angle_Motor1;
    float ysteps_Motor2 = yangle_Motor2 - angle_Motor2;

    // declare the polarity of the movement
    float Motor1_step = (ysteps_Motor1 > 0) ? 1 : -1;
    float Motor2_step = (ysteps_Motor2 > 0) ? -1 : 1;

    // step the Motor 1 and Motor 2 based on the polarity and absolute number of steps to move
    Motor1.step(Motor1_step * round(abs(ysteps_Motor1) * 100));
    Motor2.step(Motor2_step * round(abs(ysteps_Motor2) * 100));

    // calculate the final angle after this movement step
    angle_Motor1 += ysteps_Motor1;
    angle_Motor2 += ysteps_Motor2;

    // calculate the x and y coordinate of the second motor from first motor
    xpos_Motor2 = round((10 * cos((angle_Motor1 * PI) / 180)) * 100.0) / 100.0;
    ypos_Motor2 = round((10 * sin((angle_Motor1 * PI) / 180)) * 100.0) / 100.0;

    // calculate the x and y coordinate of the end effector from second motor
    float xpos_EE = round((10 * cos(((angle_Motor2 - angle_Motor1) * PI) / 180)) * 100.0) / 100.0;
    float ypos_EE = round((10 * sin(((angle_Motor2 - angle_Motor1) * PI) / 180)) * 100.0) / 100.0;

    // calculate the final coordinate of the end effector
    xfinal = xpos_Motor2 + xpos_EE;
    yfinal = ypos_Motor2 - ypos_EE;

    // print the coordinates
    Serial.print(xfinal);
    Serial.print(", ");
    Serial.println(yfinal);
  }

  // declare the final position
  xcur = xfinal;
  ycur = yfinal;
}

// Path 2
void Path_2(float x, float y){
  float xsteps = abs(x - xcur) / RESOLUTION;    // calculate the steps to move in x direction
  float ysteps = abs(y - ycur) / RESOLUTION;    // calculate the steps to move in y direction

  // calculate the movement needed for every steps in x direction
  for (int i = 0; i <= xsteps; i++){
    // calculate the point to move to based on the desired destination
    if ((x - xcur) > 0) x_partial = xcur + (i * RESOLUTION);
    else if ((x - xcur) < 0) x_partial = xcur - (i * RESOLUTION);
    y_partial = ycur;   // y position is fixed

    // Calculate the final angle of Motor 1 in this movement step
    float hypo = sqrt((y_partial * y_partial) + (x_partial * x_partial));
    float yangle1_Motor1 = round(((acos((hypo * hypo) / (20 * hypo)) * 180.0) / PI) * 100.0) / 100.0;
    float yangle2_Motor1 = round(((atan(y_partial / x_partial) * 180.0) / PI) * 100.0) / 100.0;
    float yangle_Motor1 = yangle1_Motor1 + yangle2_Motor1;
    // if the movement falls in quadrant 2, redefine the angle
    if (x_partial < 0) yangle_Motor1 = 180 - yangle_Motor1;

    // calculate the final angle of Motor 2 in this movement step
    float ypos_final_Motor2 = 10 * sin((yangle_Motor1 * PI) / 180);
    float yangle1_Motor2 = round(((asin((ypos_final_Motor2 - y_partial) / 10) * 180) / PI) * 100.0) / 100.0;
    // if the movement falls in quadrant 2, redefine the angle
    if (x_partial < 0) yangle1_Motor2 = 180 - yangle1_Motor2;
    float yangle_Motor2 = yangle_Motor1 + yangle1_Motor2;
    
    // calculate the steps to move by both motor in this movement step
    float ysteps_Motor1 = yangle_Motor1 - angle_Motor1;
    float ysteps_Motor2 = yangle_Motor2 - angle_Motor2;

    // declare the polarity of the movement
    float Motor1_step = (ysteps_Motor1 > 0) ? 1 : -1;
    float Motor2_step = (ysteps_Motor2 > 0) ? -1 : 1;

    // step the Motor 1 and Motor 2 based on the polarity and absolute number of steps to move
    Motor1.step(Motor1_step * round(abs(ysteps_Motor1) * 100));
    Motor2.step(Motor2_step * round(abs(ysteps_Motor2) * 100));

    // calculate the final angle after this movement step
    angle_Motor1 += ysteps_Motor1;
    angle_Motor2 += ysteps_Motor2;

    // calculate the x and y coordinate of the second motor from first motor
    xpos_Motor2 = round((10 * cos((angle_Motor1 * PI) / 180)) * 100.0) / 100.0;
    ypos_Motor2 = round((10 * sin((angle_Motor1 * PI) / 180)) * 100.0) / 100.0;

    // calculate the x and y coordinate of the end effector from second motor
    float xpos_EE = round((10 * cos(((angle_Motor2 - angle_Motor1) * PI) / 180)) * 100.0) / 100.0;
    float ypos_EE = round((10 * sin(((angle_Motor2 - angle_Motor1) * PI) / 180)) * 100.0) / 100.0;

    // calculate the final coordinate of the end effector
    xfinal = xpos_Motor2 + xpos_EE;
    yfinal = ypos_Motor2 - ypos_EE;

    // print the coordinates
    Serial.print(xfinal);
    Serial.print(", ");
    Serial.println(yfinal);
  }

  // declare the final x position
  xcur = xfinal;

  // calculate the movement needed for every steps in y direction
  for (int i = 0; i <= ysteps; i++){
    // calculate the point to move to based on the desired destination
    if ((y - ycur) > 0) y_partial = ycur + (i * RESOLUTION);
    else if ((y-ycur) < 0) y_partial = ycur - (i * RESOLUTION);
    x_partial = xcur;   // x position is fixed

    // Calculate the final angle of Motor 1 in this movement step
    float hypo = sqrt((y_partial * y_partial) + (x_partial * x_partial));
    float yangle1_Motor1 = round(((acos((hypo * hypo) / (20 * hypo)) * 180.0) / PI) * 100.0) / 100.0;
    float yangle2_Motor1 = round(((atan(y_partial / x_partial) * 180.0) / PI) * 100.0) / 100.0;
    float yangle_Motor1 = yangle1_Motor1 + abs(yangle2_Motor1);
    // if the movement falls in quadrant 2, redefine the angle
    if (x_partial < 0) yangle_Motor1 = 180 - yangle_Motor1;

    // calculate the final angle of Motor 2 in this movement step
    float ypos_final_Motor2 = round((10 * sin((yangle_Motor1 * PI) / 180)) * 100.0) / 100.0;
    float yangle1_Motor2 = round(((asin((ypos_final_Motor2 - y_partial) / 10) * 180) / PI) * 100.0) / 100.0;
    // if the movement falls in quadrant 2, redefine the angle
    if (x_partial < 0) yangle1_Motor2 = 180 - yangle1_Motor2;
    float yangle_Motor2 = yangle_Motor1 + yangle1_Motor2;

    // calculate the steps to move by both motor in this movement step
    float ysteps_Motor1 = yangle_Motor1 - angle_Motor1;
    float ysteps_Motor2 = yangle_Motor2 - angle_Motor2;

    // declare the polarity of the movement
    float Motor1_step = (ysteps_Motor1 > 0) ? 1 : -1;
    float Motor2_step = (ysteps_Motor2 > 0) ? -1 : 1;

    // step the Motor 1 and Motor 2 based on the polarity and absolute number of steps to move
    Motor1.step(Motor1_step * round(abs(ysteps_Motor1) * 100));
    Motor2.step(Motor2_step * round(abs(ysteps_Motor2) * 100));

    // calculate the final angle after this movement step
    angle_Motor1 += ysteps_Motor1;
    angle_Motor2 += ysteps_Motor2;

    // calculate the x and y coordinate of the second motor from first motor
    xpos_Motor2 = round((10 * cos((angle_Motor1 * PI) / 180)) * 100.0) / 100.0;
    ypos_Motor2 = round((10 * sin((angle_Motor1 * PI) / 180)) * 100.0) / 100.0;

    // calculate the x and y coordinate of the end effector from second motor
    float xpos_EE = round((10 * cos(((angle_Motor2 - angle_Motor1) * PI) / 180)) * 100.0) / 100.0;
    float ypos_EE = round((10 * sin(((angle_Motor2 - angle_Motor1) * PI) / 180)) * 100.0) / 100.0;

    // calculate the final coordinate of the end effector
    xfinal = xpos_Motor2 + xpos_EE;
    yfinal = ypos_Motor2 - ypos_EE;

    // print the coordinates
    Serial.print(xfinal);
    Serial.print(", ");
    Serial.println(yfinal);
  }

  // declare the final y position
  ycur = yfinal;
}

// End of Code