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

#include<Stepper.h>       //Include Stepper library for controlling stepper motors
#include<math.h>          //Math library
#define RESOLUTION 0.001  //Step Resolution defined for arc movements

#define RAPID 0           // Types of G Code movements are defined 
#define LINE 1            
#define ARC_CW 2
#define ARC_CCLW 3

int movemode;             //integer defined that will store the current type of G code operation while it is running

const int stepsPerRevolution = 200;
#define STEP_PIN_X 2  // Define step pin for X motor
#define DIR_PIN_X 3   // Define direction pin for X motor

float xpos, ypos;
int xsteps;
int speed;
int feedrate;
float ival, jval; // arc centres


void setup() {

  
  Serial.begin(9600);
  X_Axis.setSpeed(30);
  Serial.println("Enter an X displacement and speed "); // Keyboard input from User
  pinMode(STEP_PIN_X, OUTPUT);  // Set the X step pin as output
  pinMode(DIR_PIN_X, OUTPUT);   // Set the X direction pin as output
}

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


    switch (ch) {

      case'G': movemode = Serial.parseInt();
        Serial.println(movemode);
        break;

      case 'X':
      case 'x': xpos = Serial.parseFloat();  // if it is X then expect a float to follow
        break; // Values are stored.. Do nothing more for now.

      case 'Y':
      case 'y': ypos = Serial.parseFloat();  // if it is X then expect a float to follow
        break; // Values are stored.. Do nothing more for now.

      case 'S':
      case 's': speed = Serial.parseInt();   // get value for speed and store. Value is for the next move so do nothing else right now.
        break;

      case 'F':
      case 'f': feedrate = Serial.parseInt();   // get value for speed and store. Value is for the next move so do nothing else right now.
        break;

      case 'I':
      case 'i': ival = Serial.parseFloat();  // if it is X then expect a float to follow
        break; // Values are stored.. Do nothing more for now. case 'X':

      case 'J': jval    = Serial.parseFloat();  // if it is X then expect a float to follow
        break; // Values are stored.. Do nothing more for now.

      case  '#':
      case '\n':   // return key has been hit. Output a move if the X value has changed since the last CR.
        //Add Code Here:
        // If Movemode == LINE, do a line using X, Y and F
        // If Movemode ==ARC do an arc using X,Y,I,J and F
        // If Movemode ==RAPID do a rapid move using X and Y

        if (movemode == LINE) linemove(xpos, ypos);
        if (movemode == ARC_CCLW)arcmove_CCLW(xpos, ypos, ival, jval);
        break;

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

void linemove(float x, float y)
{
  int xsteps = int(x * 100);
  int ysteps = int(y * 100);
  int xcount=0, ycount=0;
  
  Serial.print("Line to :");
  Serial.print(x);
  Serial.print(" ");
  Serial.println (y);

  while (xcount < xsteps && ycount <= ysteps)
  {
    if (xcount < xsteps)X_Axis.step(1);
    if (ycount < ysteps)Y_Axis.step(1);
    xcount++;
    ycount++; // will only work for 45 degree angles, also won't work for negative values of x and y

  }



}

void arcmove_CCLW(float x, float y, float i, float j)
{
  float deltax, deltay;
  float xpos, ypos;
  float radius;

  deltax = abs(x - i);
  deltay = abs(y - j);
  radius = sqrt((deltax * deltax) + (deltay * deltay)); //pythagoras to find the radius
  Serial.print( deltax);
  Serial.print(" ");
  Serial.print(deltay);
  Serial.print(" Rad = : ");
  Serial.println(radius);
  float tangent_slope = atan(y / x);
  float included_angle = tangent_slope * 2;

  for (float inc = 0; inc < included_angle; inc += RESOLUTION) {
    xpos = radius * sin(inc);
    ypos = j-(radius * cos(inc));
    // Serial.print("X :");
    Serial.print(xpos);
    //Serial.print(',');
    Serial.write(9);
    Serial.println(ypos);
  // linemove(xpos,ypos);
  }
}