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


Servo moteur1; //Initialise la variable "moteur" 
Servo moteur2;
Servo moteur3;
int buttondt=7;
int buttonlast=3;
int buttonct=2;
int buttongd=4;
int buttonswitch=8;
int cpt=0;
int buttondt_stat=0;
int buttonct_stat=0;
int buttongd_stat=0;
int buttonlast_stat=0;
int potValueX=A1;
int potValueY=A2;
float theta1;
float theta2;
float theta3;
float L1=11;
float L2=4;
float x=4;
float y=11;
float posx=6;
float posy=10;
float pi=3.1415;

/////////////////////////////////////////Partie modele geometrique /////////////////

float find_theta1(float xp ,float yp){
  float angle1=atan2(yp,xp)-atan2(L2*sin(find_theta2(xp,yp)*M_PI/180),L1+L2*cos(find_theta2(xp,yp)*M_PI/180));
  float theta1=(angle1*180)/M_PI;
  return theta1;
}

float find_theta2(float xp ,float yp){
  float angle2=acos((xp*xp+yp*yp-L1*L1-L2*L2)/(2*L1*L2));
  float theta2=(angle2*180)/M_PI;
  return theta2;
}

//////////////////////////////Fin modele geometrique //////////////////

void setup(){
  moteur1.attach(11); //Permet d'assigner le pin 10 à un moteur
  moteur2.attach(10);
  moteur3.attach(9);
  Serial.begin(9600);
  pinMode(buttondt,INPUT_PULLUP);
  pinMode(buttonct,INPUT_PULLUP);
  pinMode(buttongd,INPUT_PULLUP);
  pinMode(buttonlast,INPUT_PULLUP);
  pinMode(buttonswitch,INPUT_PULLUP);
  float theta2=find_theta2(6,10);
  float theta1=find_theta1(6,10);
  moteur2.write(theta2);
  moteur1.write(180-theta1);
  moteur3.write(90);
}

void loop(){
  //////////////////////////////////////Partie joystick/////////////////////////
  float theta2=find_theta2(posx,posy);
  float theta1=find_theta1(posx,posy);
  moteur2.write(theta2);
  delay(100);
  moteur1.write(180-theta1);
  delay(100);

 
  float X=analogRead(potValueX); // CHANGER LE DECALAGE
  float Y=analogRead(potValueY);

  double convx=map(X,0,1023,-4.5,4.5);
  convx=convx/10;
  Serial.println("posx=");
  Serial.println(posx);
  delay(100);
  double convy=map(Y,0,1023,-4.5,4.5);
  convy=((convy+1)/10);
  Serial.println("posy");
  Serial.println(posy);
  delay(100);
  
  if(posx>=7.9 && convx>=0){
    convx=0;
  }
  if(posx<=0 && convx<=0){
    convx=0;
  }
  if(posy>=13.5 && convy<=0){
    convy=0;
  }
  if(posy<=7.8 && convy>=0){
    convy=0;
  }

  posx=posx+convx;
  posy=posy-convy;

  //moteur2.write(find_theta2(posx,posy));
  //moteur1.write(find_theta1(posx,posy));

  if(!digitalRead(buttonswitch)){
    Serial.println("--------------BOUTON-------------- ");
    cpt++;
  }
  if(cpt%2==0){
    moteur3.write(90);
      delay(100);
  }
  else{
    moteur3.write(82);
    delay(100);
  }
  /////////////////////////////////////Fin joystick ////////////////
  
  delay(50);

  buttondt_stat=digitalRead(buttondt);
  buttonct_stat=digitalRead(buttonct);
  buttongd_stat=digitalRead(buttongd);
  buttonlast_stat=digitalRead(buttonlast);
  /////////////////////////////////////////Partie ligne pointiller ///////////////
  if(buttondt_stat==LOW){
    for (float x=4;x<=9;x=x+0.8){
      moteur3.write(90);
      delay(350);
      delay(350);
      float theta2=find_theta2(x,10);
      float theta1=find_theta1(x,10);
      moteur1.write(180-theta1);
      moteur2.write(theta2);
      delay(350);
      moteur3.write(84);
      delay(350);
    }
    moteur3.write(90);
    delay(100);
  delay(1000);
  }
  //////////////////////////////////////Partie cercle ///////////////////////////
  if(buttonct_stat==LOW){
    for (float x=-2;x<=2;x=x+0.1){
      moteur3.write(82);
      float theta2=find_theta2(x+6,10+sqrt(2*2-(x)*(x)));
      float theta1=find_theta1(x+6,10+sqrt(2*2-(x)*(x)));
      moteur1.write(180-theta1);
      moteur2.write(theta2);
      delay(120);
    }
    for (float x=2;x>=-2;x=x-0.1){
      float theta2=find_theta2(x+6,10-sqrt(2*2-(x)*(x)));
      float theta1=find_theta1(x+6,10-sqrt(2*2-(x)*(x)));
      moteur1.write(180-theta1);
      moteur2.write(theta2);
      delay(120);
   }
  float theta2=find_theta2(-2+6,10-sqrt(2*2-(-2)*(-2)));
  float theta1=find_theta1(-2+6,10-sqrt(2*2-(-2)*(-2)));
  moteur1.write(180-theta1);
  moteur2.write(theta2);
  delay(100);
  moteur3.write(90);
  }

  /////////////////////////////////////Partie ligne /////////////////////////
  if(buttongd_stat==LOW ){
    moteur3.write(85);
    delay(100);
    for (float x=4;x<=9;x=x+0.1){
      Serial.println(buttongd_stat);
      float theta2=find_theta2(x,10);
      float theta1=find_theta1(x,10);
      moteur1.write(180-theta1);
      moteur2.write(theta2);
      Serial.println(theta2);
      delay(200);
    }
    moteur3.write(90);
    delay(100);
  }
  ////////////////////////////////////////////////Partie cercle pointiller /////////////////
  if(buttonlast_stat==LOW){
    for (float x=-2;x<=2;x=x+0.5){
      delay(150);
      moteur3.write(90);
      delay(150);
      float theta2=find_theta2(x+6,10+sqrt(2*2-(x)*(x)));
      float theta1=find_theta1(x+6,10+sqrt(2*2-(x)*(x)));
      moteur1.write(180-theta1);
      moteur2.write(theta2);
      delay(150);
      moteur3.write(83);
      delay(150);
    }
    for (float x=2;x>=-2;x=x-0.5){
      delay(150);
      moteur3.write(90);
      delay(150);
      float theta2=find_theta2(x+6,10-sqrt(2*2-(x)*(x)));
      float theta1=find_theta1(x+6,10-sqrt(2*2-(x)*(x)));
      moteur1.write(180-theta1);
      moteur2.write(theta2);
      delay(130);
      moteur3.write(83);
      delay(130);
   }
  delay(100);
  moteur3.write(90);
  }
}