#include <HardwareSerial.h>
#include <Servo.h>
#include <math.h>
//#include <Array.h>

Servo servo1;
Servo servo2;
double x, y;
double a1 = 5;
double a2 = 5;
double length = 2.54;
int input;
void stop_sketch(){ 
  noInterrupts();
  while(1){}
}

void setup() {
  Serial.begin(9600);
  servo1.attach(8);
  servo2.attach(7);
}

void choice(){
  if(input == 10){ //test plan
    kinematic(0, 10); //speed test
    delay(3000);
    linear(9, 5, -2.5, 5); //10inches test
    delay(3000); 
    triangle(0, 6, 2, 8, 5, 7); //drawing #1
    delay(3000);
    // right_triangle(0, 3, 0, 6, 4, 3); //drawing #2
    // square(0, 5, 0, 8, 8, 5, 5, 5); //drawing #3
    // rectangle(0, 3, 0, 8, 8, 3, 3, 3); //drawing #4
    // pentagon(0, 7, 4, 5, 2, 3, -2, 3, -4, 5); //drawing #5
    // linear(9, 5, //inches->mm (coordinate * 2.54)
  }
  if(input == 0){ //G0
    //get (X, Y)
  }
  if(input == 1){ //G1
    //get (X, Y, F)
  }
  if(input == 2){ //G90
    //default setting
  }
  if(input == 3){ //G91
    //change to relative
  }
  if(input == 4){ //G20
    //default setting
  }
  if(input == 5){ //G21
    a1 = a1 * length; 
    a2 = a2 * length; 
  }
  if(input == 6){ //M2
    stop_sketch();
  }
  if(input == 7){ //M6
    delay(15000);
  }
  if(input == 8){ //M72
    
  }
  
}
void loop() {
  //input = 1000;
  delay(2000);
  Serial.println("servo initializing");
  Serial.println();
  //initial();
  delay(2000);
  //ask input
  //choice();
  
  //stop_sketch();
  // kinematic(0, 10); //speed test
  // delay(3000);
  // linear(9, 5, -2.5, 5); //10inches test
  // delay(3000); 
  triangle(0, 6, 2, 8, 5, 7); //drawing #1
  delay(3000);
  // right_triangle(0, 3, 0, 6, 4, 3); //drawing #2
  // square(0, 5, 0, 8, 8, 5, 5, 5); //drawing #3
  // rectangle(0, 3, 0, 8, 8, 3, 3, 3); //drawing #4
  // pentagon(0, 7, 4, 5, 2, 3, -2, 3, -4, 5); //drawing #5
  // linear(9, 5, //inches->mm (coordinate * 2.54)
}

void initial() {
  servo1.write(180);
  servo2.write(0);
}

// void update_joints(self, _joint_angles) {
//   self joint_angles = _joint_angles;
//   double n = 2;
//   double *ret = new double(n);
//   Serial.println(n);
//   *ret = kinematic(x, y);
//   servo1.write(ret[0]);
//   servo2.write(ret[1]);
// }

double linear(double x_i, double y_i, double x_f, double y_f){ //make to line
  double chop, x_s, y_s;
  chop = 100;
  for (double n = 1; n < chop; n++){
    x_s = x_i + n * ((x_f - x_i) / chop);
    y_s = y_i + n * ((y_f - y_i) / chop);
    kinematic(x_s, y_s);
  }
  //x_s = x_i + (x_s - x_i) / (x_f - x_i);
}
double kinematic(double x, double y){ //movement
  double q2, q1, z;
  z = 57.297;
  
  q2 = acos((pow(x, 2) + pow(y, 2) - pow(a1, 2) - pow(a2, 2)) / (2 * a1 * a2));
  q1 = atan2(y, x) - atan2(a2 * sin(q2), a2 * cos(q2) + a1);
  
  double n1, n2;
  n1 = 180 -(q1 * z);
  n2 = (q2 * z);
  
  Serial.print("The angle for servo1:");
  Serial.println(n1);
  servo1.write(n1);
  delay(0.01);

  Serial.println();

  Serial.print("The angle for servo2:");
  Serial.println(n2);
  servo2.write(n2);
  delay(0.01);
}

double triangle(double x_a, double y_a, double x_b, double y_b, double x_c, double y_c){
  linear(x_a, y_a, x_b, y_b); //side A
  linear(x_b, y_b, x_c, y_c); //side B
  linear(x_c, y_c, x_a, y_a); //side C
}
double right_triangle(double x_a, double y_a, double x_b, double y_b, double x_c, double y_c){
  linear(x_a, y_a, x_b, y_b); //side A
  linear(x_b, y_b, x_c, y_c); //side B
  linear(x_c, y_c, x_a, y_a); //side C
}
double square(double x_a, double y_a, double x_b, double y_b, double x_c, double y_c, double x_d, double y_d){
  linear(x_a, y_a, x_b, y_b); //side A
  linear(x_b, y_b, x_c, y_c); //side B
  linear(x_c, y_c, x_d, y_d); //side C
  linear(x_d, y_d, x_a, y_a); //side D
}
double rectangle(double x_a, double y_a, double x_b, double y_b, double x_c, double y_c, double x_d, double y_d){
  linear(x_a, y_a, x_b, y_b); //side A
  linear(x_b, y_b, x_c, y_c); //side B
  linear(x_c, y_c, x_d, y_d); //side C
  linear(x_d, y_d, x_a, y_a); //side D
}
double pentagon(double x_a, double y_a, double x_b, double y_b, double x_c, double y_c, double x_d, double y_d, double x_e, double y_e){
  linear(x_a, y_a, x_b, y_b); //side A
  linear(x_b, y_b, x_c, y_c); //side B
  linear(x_c, y_c, x_d, y_d); //side C
  linear(x_d, y_d, x_e, y_e); //side D
  linear(x_e, y_e, x_a, y_a); //side E
}