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

#define RESOLUTION 50
#define L1 10 // defining length of robot arms
#define L2 10

const int stepsPerRevolution = 200;

Stepper stepper_joint1(stepsPerRevolution, 7, 8, 9, 10); // Create an instance of Stepper
Stepper stepper_joint2(stepsPerRevolution, 3, 4, 5, 6);

const float angleResolution = 2 * PI / stepsPerRevolution; // this is the angle/step

float xpos, ypos;
int joint1, joint2;
int speed = 10;
float xCurrent = 10; // starting x coordinate
float yCurrent = 0;  // starting y coordinate
float theta1, theta2; // angle of the two arms

void setup() {
  Serial.begin(9600);
  stepper_joint1.setSpeed(speed); // set speed of motors
  stepper_joint2.setSpeed(speed);

  theta2 = 0; // initialize angles
  theta1 = 0;
  joint1 = 0; // initialize joint steps
  joint2 = 0;

  stepper_joint1.step(joint1); // moving steppers to the starting position
  stepper_joint2.step(joint2);

  Serial.println("Enter X, Y coordinates and path 1 or 2"); // Keyboard input from User

  // Save the initial point and print it
  Serial.print(xCurrent);
  Serial.print(",");
  Serial.println(yCurrent);
}

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

    switch (ch) {
      case 'X':
      case 'x':
        xpos = Serial.parseFloat();  // if it is X then expect a float to follow
        break;

      case 'Y':
      case 'y':
        ypos = Serial.parseFloat();  // if it is Y then expect a float to follow
        break;

      case 'P':
      case 'p':
        int path = Serial.parseInt();   // get value for the path
        if (path == 1) path1_Movement(xpos, ypos);
        else if (path == 2) path2_Movement(xpos, ypos);
        break;

      case '#':
      case '\n':
        // return key has been hit. Output a move if the X value has changed since the last CR.
        break;

      default:
        Serial.print(ch);
        if (ch != ' ') Serial.println("Not recognized"); // A non-valid input so ignore.
        break;
    }
  }
}

void path1_Movement(float x, float y) {
  float deltax = (x - xCurrent);
  float deltay = (y - yCurrent);

  for (int i = 1; i <= RESOLUTION; i++) {
    float incx = deltax / RESOLUTION;
    float nx = xCurrent + incx;

    float incy = deltay / RESOLUTION;
    float ny = yCurrent + incy;

    theta2 = acos((nx * nx + ny * ny - L1 * L1 - L2 * L2) / (2 * L1 * L2));
    theta1 = atan2(ny, nx) - atan2((L2 * sin(theta2)), (L1 + L2 * cos(theta2)));

    int joint1_steps = int(theta1 / angleResolution);
    int joint2_steps = int(theta2 / angleResolution);

    stepper_joint1.step(joint1_steps);
    stepper_joint2.step(joint2_steps);

    joint1 += joint1_steps;
    joint2 += joint2_steps;

    xCurrent = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
    yCurrent = L1 * sin(theta1) + L2 * sin(theta1 + theta2);

    Serial.print(xCurrent);
    Serial.print(",");
    Serial.println(yCurrent);
  }
}

void path2_Movement(float x, float y) {
  path1_Movement(x, yCurrent);
  path1_Movement(xCurrent, y);
}