#include<Stepper.h>
#include<math.h>
 float x0 = 10;
 float y0 = 0;

void setup() {

 Serial.begin(9600);
 int x;
 int y;



Serial.println("START");
}

void loop() {


  float x, y;
  float alpha2;
  float alpha1;
  float theta;
  float phi;
  int lk1 = 10;
  int lk2 = 10;
  float a2;
  float a1;
  int j1steps = int(x);
  int j2steps = int(y);
  int j1count=0, j2count=0;
  bool j1flag = 1;
  bool j2flag = 1;
  float elbow_x;
  float elbow_y;
  float Xpos;
  float Ypos;
  int i = 0;
  int res = 20;
  int range = 5;
  float xa;
  float xb;
  float ya;
  float yb;

  

     if (Serial.available()) {
    x = Serial.parseFloat();  
    y = Serial.parseFloat();  

a2 = (-acos((sq(x)+sq(y) - (sq(lk1)+sq(lk2))) / (2*lk1*lk2)));

phi = (atan(y/x));
theta = (atan(lk2 * sin(abs(a2))/(lk1+lk2 *cos(abs(a2)))));
a1 = phi + theta;

alpha2 = (a2 * 180/3.14);
alpha1 = (a1 * 180/3.14);

//Serial.print("Alpha1= ");
//Serial.print(alpha1);
//Serial.write(9);
//Serial.print("Alpha2= ");
//Serial.println(alpha2);

j1steps = alpha1;
j2steps = alpha2;

//Serial.print("J1steps= ");
//Serial.print(j1steps);
//Serial.write(9);
//Serial.print("J2steps= ");
//Serial.println(j2steps);

elbow_x = (lk1*(cos(a1))); //find the x position of the elbow
elbow_y = (lk1*(sin(a1))); //find the y position of the elbow

 // Serial.print("Elbow X= ");
 // Serial.print(elbow_x);
 // Serial.write(9);
 // Serial.print("Elbow Y= ");
  //Serial.println(elbow_y);

Xpos = x;//elbow_x - (lk2*(cos(a2)));
Ypos = y;//elbow_y + (lk2*(sin(a2)));

 // Serial.print("Xpos= ");
 // Serial.print(Xpos);
 // Serial.write(9);
 // Serial.print("Ypos= ");
 // Serial.println(Ypos);

for (i = 0; i <= res; i++)
{
 xa = x0+((i/res)*range);
 xb = x0+(((i+1)/res)*range);

 ya = y0+((i/res)*range);
 yb = y0+(((i+1)/res)*range);

  Serial.print("X = ");
  Serial.print(xa);
  Serial.write(9);
  Serial.print(xb);
  Serial.write(9);
  Serial.println();
  Serial.print("Y = ");
  Serial.print(ya);
  Serial.write(9);
  Serial.print(yb);
  Serial.write(9);
  Serial.println(i);
  delay(500);
  }

}
}