#include <Servo.h>
#include <math.h>

Servo myservo_g;  //Servo 2 (theta 2)
Servo myservo_d;  //Servo 1 (theta 1)
Servo myservo_plat; //Servo pour platforme

//Entree
/*
#define joy_x A0
#define joy_y A1
#define bp_joystick 5
#define bp_cercle 2
#define bp_trait 3
#define bp_type 4
#define led_cercle 6
#define led_trait 7
#define led_type 12
*/
#define joy_x A1
#define joy_y A0
#define bp_joystick 5
#define bp_cercle 7
#define bp_trait 8
#define bp_type 6
#define led_cercle 4
#define led_trait 3
#define led_type 2

int pos = 0;    // variable to store the servo position
int angle_dessine;  //Angle s3 niveau haut
int angle_dessine_pas;  //Angle s3 niveau bas
//int niveau;
int angle_t1;
int angle_t2;
int L = 6;
int l = 2.5;
int type;

struct coordonnee {
  float x;
  float y;
};

coordonnee centre;  //Centre carré
coordonnee act;     //coordonnee actuelle
coordonnee obj;     //objectif
coordonnee angle[4];

void setup() {
  myservo_g.attach(9);  // attaches the servo on pin 9 to the servo object
  myservo_d.attach(10); // attaches the servo on pin 10 to the servo object
  myservo_plat.attach(11);  // attaches the servo on pin 11 to the servo object

  pinMode(bp_cercle, INPUT);
  pinMode(bp_trait, INPUT);
  pinMode(bp_type, INPUT);
  pinMode(led_cercle, OUTPUT);
  pinMode(led_trait, OUTPUT);
  pinMode(led_type, OUTPUT);
  pinMode(bp_joystick, INPUT_PULLUP);
  pinMode(joy_x, INPUT);
  pinMode(joy_y, INPUT);
  myservo_g.write(0);
  myservo_d.write(0);
  myservo_plat.write(0);

  angle[0].x = centre.x-2.5;
  angle[0].y = centre.y-2.5;
  angle[1].x = centre.x-2.5;
  angle[1].y = centre.y+2.5;
  angle[2].x = centre.x+2.5;
  angle[2].y = centre.y+2.5;
  angle[3].x = centre.x+2.5;
  angle[3].y = centre.y-2.5;
  

  digitalWrite(led_cercle, LOW);
  digitalWrite(led_trait,LOW);
  digitalWrite(led_type, LOW);

  //Centre de la plaque
  centre.x = 0.0-2;
  centre.y = 6.5+2;

  angle[0].x = centre.x-2.5;
  angle[0].y = centre.y-2.5;
  angle[1].x = centre.x-2.5;
  angle[1].y = centre.y+2.5;
  angle[2].x = centre.x+2.5;
  angle[2].y = centre.y+2.5;
  angle[3].x = centre.x+2.5;
  angle[3].y = centre.y-2.5;
  
  obj.x = centre.x;
  obj.y = centre.y;
  aller();
  calcul_co();    //Init co actuelle
//  angle_t1 = 45;//*PI/180;
//  angle_t2 = 45;//*PI/180;
  angle_dessine = 40;  //Angle pour platforme niveau haut
  angle_dessine_pas = 5; //Angle pour platforme niveau bas
  pos = 0;
  type = 0;

  Serial.begin(9600);
}

void loop() {
  float stepx, stepy;
  
  //Serial.println(digitalRead(bp_cercle));
  if (digitalRead(bp_cercle)) dessiner_cercle();
  
  if (digitalRead(bp_trait)){
    //obj.x = centre.x;
    //obj.y = centre.y;
    //aller();
    dessiner_trait();
  }// dessiner_trait();
  
  stepy = - ( analogRead(joy_x) - 512);
  stepy /= 512;
  stepy /= 100;
  stepx = -( analogRead(joy_y) - 512);
  stepx /= 512;
  stepx /= 100;
  //Serial.print(analogRead(joy_x));Serial.print(":");Serial.print(analogRead(joy_y));Serial.print(" ");Serial.print(stepx);Serial.print(":");Serial.println(stepy);
  //Serial.print("joy_x : ");Serial.print(analogRead(joy_x));Serial.print(" stepx : ");Serial.print(stepx);
  //Serial.print("joy_y : ");Serial.print(analogRead(joy_y));Serial.print(" stepy : ");Serial.println(stepy);
  if ( (obj.x + stepx) >= centre.x-2.5 && (obj.x + stepx) <= centre.x+2.5 && abs(analogRead(joy_y)-512) > 80) obj.x += stepx;
  if ( (obj.y + stepy) >= centre.y-2.5 && (obj.y + stepy) <= centre.y+2.5 && abs(analogRead(joy_x)-512) > 80) obj.y += stepy;
  aller();
  
  //Serial.println(digitalRead(bp_joystick));

  if (!digitalRead(bp_joystick)) {
    pos = 1 - pos;
    //Serial.println(pos);
    dessine(pos);
    while (!digitalRead(bp_joystick)) {}
  }

  //Serial.println(digitalRead(bp_type));
  if (digitalRead(bp_type)) {
    type = 1 - type;
    digitalWrite(led_type, !digitalRead(led_type));
    
    while (digitalRead(bp_type)) {}
  }
  
  //-----------Serial.print(obj.x);Serial.print(" : ");Serial.println(obj.y);
  
  //Serial.println("sorti sorti sorti sorti sorti sorti sorti sorti sorti sorti sorti sorti sorti sorti ");
  //while (Serial.readString() != "0") {}
  delay(1);
  
}

void calcul_co()
{
  act.x = L*(sin(angle_t1*PI/180) - sin(angle_t2*PI/180));
  act.y = L*(cos(angle_t1*PI/180) + cos(angle_t2*PI/180));
}

void aller() {                    //Aller au point obj.x, obj.y
  float alpha, d, a3, a2, a1;
  a1 = atan2(obj.x,obj.y)*180/PI;
  d = sqrt( pow(obj.x,2) + pow(obj.y,2));
  a3 = acos( (2*pow(L,2) - pow(d,2))/(2*pow(L,2)) )*180/PI;
  a2 = 90 - a3/2;
  angle_t1 = a1 + a2;
  angle_t2 = abs(a2 - a1);

  /*
  //Serial.print("a1 : ");
  Serial.print(a1);Serial.print(";");
  //Serial.print("d : ");
  Serial.print(d);Serial.print(";");
  //Serial.print("a3 : ");
  Serial.print(a3);Serial.print(";");
  //Serial.print("a2 : ");
  Serial.print(a2);Serial.print(";");
  */
  //Serial.print("angle_t1 : ");
  //Serial.print(angle_t1);Serial.print(";");
  //Serial.print("angle_t2 : ");
  //Serial.print(angle_t2);Serial.print(";");
  
  myservo_g.write(180-angle_t2);
  myservo_d.write(180-angle_t1);
  calcul_co();
  //delay(0.1);
}

void pt_sur_cercle_plus_proche() {  //Retourne une structure de donnee avec le point sur le cercle ([0,6.5],2.5) le plus proche
  coordonnee sortie;
  calcul_co();
  if (act.x == 0) {
    sortie.x = 0;
    if (act.y >= 0) sortie.y = centre.y + l;
    else sortie.y = centre.y - l;
  }
  else if (act.y == 0) {
    sortie.y = 0;
    if (act.x >= 0) sortie.x = centre.x + l;
    else sortie.x = centre.x - l;
  }
  else {
    float alpha;
    alpha = (atan2(act.y-centre.y,act.x-centre.x));
    //if (act.x < centre.x)alpha + PI;
    sortie.x = 2.5*cos(alpha) + centre.x;
    sortie.y = 2.5*sin(alpha) + centre.y;
    Serial.println("ici");
    Serial.println(alpha);
  }
  //obj.x = sortie.x;
  //obj.y = sortie.y;
  Serial.print("x : ");Serial.print(obj.x);Serial.print(", y : ");Serial.println(obj.y);
  for (int i = 1; i <= 10; i++) {
    obj.x = act.x + (sortie.x - act.x)*i/10;
    obj.y = act.y + (sortie.y - act.y)*i/10;
    aller();
    delay(30);
  }
  
}

void dessine(int test) {    //si test = 0, position dessine pas, sinon : position dessine
  if (test) myservo_plat.write(angle_dessine);
  else myservo_plat.write(angle_dessine_pas);
}

void dessiner_cercle() {
  digitalWrite(led_cercle,HIGH);
  Serial.println("dessiner un cercle");
  dessine(0);
  //coordonnee passage;
  Serial.print("x : ");Serial.print(act.x);Serial.print(", y : ");Serial.println(act.y);
  pt_sur_cercle_plus_proche();
  dessine(1);
  //obj.x = 2.5;
  //obj.y = 6.5;
  
  //Serial.println("\n");
  float alpha;
  //calcul_co();
  alpha = atan2(act.y-centre.y,act.x-centre.x);
  Serial.print("x : ");Serial.print(act.x);Serial.print(", y : ");Serial.print(act.y);Serial.print(", alpha : ");Serial.println(alpha);
  int cpt = 0;
  bool test = 0;
  //Serial.println("avant");
  for (float a = alpha*180/PI; a < alpha*180/PI + 360; a += 1) {
    //Serial.println(a);
    obj.x = 2.5*cos(a*PI/180) + centre.x;
    obj.y = 2.5*sin(a*PI/180) + centre.y;
    aller();
    //Serial.print(obj.x);Serial.print(";");
    //Serial.print(act.x);Serial.print(";");
    //Serial.print(obj.y);Serial.print(";");
    //Serial.print(act.y);Serial.print(";");
    //Serial.println(a);
    if ( (int) a%18 == 0 && type == 1) {
      dessine(test);
      test = !test;
    }
    delay(27);
  }
  Serial.println("fin");
  digitalWrite(led_cercle,LOW);
}

void dessiner_trait() {
  Serial.println("1");
  digitalWrite(led_trait, HIGH);
  //act.x = centre.x;obj.x = centre.x;
  //act.y = centre.y;obj.y = centre.y;
  
  dessine(0);
  
  float d = 0.01;
  float cpt = 0;
  float a1, a2;
  int id;

  Serial.println("2");
  //id = pt_le_plus_proche_valable();
  Serial.println("3");
  plage_angle(&a1,&a2);  //Calcul de la plage d'angle valable
  Serial.println("4");
  int alpha;
  

  //Vers le point de départ
  //Serial.print("plage angle");Serial.print(a1);Serial.print(" : ");Serial.println(a2);Serial.print(" : ");Serial.println(alpha);
  /*
  if (id != -1) {
    alpha = random(a1, a2);
    obj.x = angle[id].x + 2.5*cos( (180 + alpha)*PI/180 );
    obj.y = angle[id].y + 2.5*sin( (180 + alpha)*PI/180 );
    //aller();
  }
  */
  
  
  Serial.println("debut du trait");
  
  obj.x = centre.x + l;  //Position défini
  obj.y = centre.y + l;  //Position défini
  //act.x = centre.x + l;
  //act.y = centre.y + l; 
  alpha = 225;  //Angle défini
  aller();
  dessine(1);

  int cpt2 = 0;  //Compteur pointillé
  bool test = 0;
  while (cpt < 5) {
//    Serial.println("dans le while");
    cpt += d;
    cpt2++;
    obj.x = obj.x + d*cos(alpha*PI/180);
    obj.y = obj.y + d*sin(alpha*PI/180);
    aller();
    Serial.print(obj.x);Serial.print(";");
    Serial.print(act.x);Serial.print(";");
    Serial.print(obj.y);Serial.print(";");
    Serial.print(act.y);Serial.print(";");
    Serial.println(cpt);

    if ( cpt2%30 == 0 && type == 1){
      dessine(test);
      test = !test;
    }
    
    delay(19);  
  }
  dessine(0);
  //Serial.println("fin du trait");
  digitalWrite(led_trait, LOW);
}

void plage_angle(float *a1, float *a2) {
  *a1 = -1;
  *a2 = 0;
  int xi, yi, xim, yim;
  xim = act.x + 5*cos(0-0.1);
  yim = act.y + 5*sin(0-01);
  for (float a = 0; a < 360; a += 0.1) {
    xi = act.x + 5*cos(a*PI/180);
    yi = act.y + 5*sin(a*PI/180);
    if ( (xi >= centre.x-2.5 && xi <= centre.x+2.5 && yi >= centre.y-2.5 && yi <= centre.y+2.5) && !(xim >= centre.x-2.5 && xim <= centre.x+2.5 && yim >= centre.y-2.5 && yim <= centre.y+2.5) ) {
      *a1 = a;
    }
    if ( !(xi >= centre.x-2.5 && xi <= centre.x+2.5 && yi >= centre.y-2.5 && yi <= centre.y+2.5) && (xim >= centre.x-2.5 && xim <= centre.x+2.5 && yim >= centre.y-2.5 && yim <= centre.y+2.5) ) {
      *a2 = a;
    }
    xim = xi;
    yim = yi;
  }
}

int pt_le_plus_proche_valable() {
  int d = 0,id;
  int max = 0;
  for (int i = 1; i < 4; i++) {
    d = sqrt( pow(act.x-angle[i].x,2) + pow(act.y-angle[i].y,2) );
    if (d > max)max = d;
    if (max < 5) id = i;
  }
  if (max >= 5) id = 0;
  if (id == 0) return -1;
  else {
    int alpha = atan2( abs(angle[id].y - act.y), abs(angle[id].x - act.x) );
    if (id == 1 || id == 2)obj.y = 5*sin(alpha) + angle[id].y;
    else obj.y = -5*sin(alpha) + angle[id].y;

    if (id == 2 || id == 3)obj.x = 5*cos(alpha) + angle[id].x;
    else obj.x = -5*cos(alpha) + angle[id].x;
  }
  Serial.print("Point valable : ");Serial.print(obj.x);Serial.print(":");Serial.println(obj.y);
  aller();
  return id;
}