/*------------------INITIALISATION DES VARIABLES MECANIQUE DE NOTRE ROBOT ---------------*/
float angle_effecteur = 180-105;
float l1 = 5;
float l4 = 6;
float l2 = 5;
float l3 = 6;
float l5 = 1.96;
float a = 6;
 
int axeX = A0;
int axeY = A1;


float cpt = 0;

/*Valeur du joystick*/
float posx = 0; 
float posy = 0;

/*Valeur des angles des servo moteur*/
float haut = 70;
float bas = 110;
float an = haut; 


#include <Servo.h>

Servo monServo1; //Servo de droite au PIN 10
Servo monServo2; //Servo de gauche au PIN 9
Servo monServo3; //Servo pointillé au PIN 11


int BOUTON = 7; // Bouton du Joystick

int boutonpoint = 6; //Bouton VERT
int etatpoint;
int cptpoint = 0;

int boutonjoystick = 5; //Bouton ROUGE
int etatjoystick; 
int cptjoystick = 0;

int boutoncercle = 4; // Bouton JAUNE
int etatcercle;

int boutonligne = 3; //Bouton BLEU
int etatligne;




  
int ledJ = 13; //LED du cercle
int ledB = 8; //LED de la ligne
int ledV = 12; //LED du mode pointillé
int ledR = 2; //LED du mode Joystick


void setup() {
  
Serial.begin(9600);

monServo1.attach(10);
monServo2.attach(9);
monServo3.attach(11);

pinMode (axeX, INPUT);
pinMode (axeY, INPUT);

pinMode (BOUTON, INPUT_PULLUP);
pinMode(boutoncercle, INPUT_PULLUP);
pinMode(boutonligne, INPUT_PULLUP);
pinMode(boutonpoint, INPUT_PULLUP);
pinMode(boutonjoystick, INPUT_PULLUP);

pinMode(ledJ, OUTPUT);
pinMode(ledB, OUTPUT);
pinMode(ledV, OUTPUT);
pinMode(ledR, OUTPUT);


}

float theta1(float xp,float yp){
  float O1P=sqrt((-a+xp)*(-a+xp)+(yp)*(yp));
  float b=(((l1*l1)-(l5*l5 +l4*l4 -2*l4*l5*cos((180-angle_effecteur)*M_PI/180.0) )+(O1P*O1P))/(2*l1*O1P));
  float alpha=acos(b)*180/M_PI;
  float beta=atan(((yp)/(a-xp)))*180/M_PI;
  float theta=180-(beta+alpha);
  return theta;

}

float theta2(float xp,float yp){
  float O1P=sqrt((-a+xp)*(-a+xp)+(yp)*(yp));
  float omega=(acos(((O1P*O1P)-(l5*l5 +l4*l4 -2*l4*l5*cos((180-angle_effecteur)*M_PI/180.0))-(l1*l1))/(-2*(sqrt(l5*l5 +l4*l4 -2*l4*l5*cos((180-angle_effecteur)*M_PI/180.0)))*l1))*180.0/M_PI)+acos((-(l5*l5 +l4*l4 -2*l4*l5*cos((180-angle_effecteur)*M_PI/180.0))+(l5*l5)-(l4*l4))/(-2*l4*(sqrt(l5*l5 +l4*l4 -2*l4*l5*cos((180-angle_effecteur)*M_PI/180.0)))))*180.0/M_PI;
  float omega_bis=omega-theta1(xp,yp)-angle_effecteur;
  float O2C=sqrt((a+(xp+l5*(cos(omega_bis*M_PI/180.0))))*(a+(xp+l5*cos(-omega_bis*M_PI/180.0)))+(yp+l5*sin(-omega_bis*M_PI/180.0))*(yp+l5*sin(-omega_bis*M_PI/180.0)));
  float alpha2=acos((((l2*l2)-(l3*l3)+(O2C*O2C))/(2*l2*O2C)))*180/M_PI;
  float beta2=atan(((yp+l5*sin(-omega_bis*M_PI/180.0))/(a+xp+l5*cos(-omega_bis*M_PI/180.0))))*180.0/M_PI;
  float theta2=beta2+alpha2;
  return theta2;
}

void loop() {

/*Initialisation des Servo à 90°*/
// monServo2.write(90);
// monServo1.write(90);
// delay(1000);

etatcercle = digitalRead(boutoncercle);
etatligne = digitalRead(boutonligne);
etatpoint = digitalRead(boutonpoint);
etatjoystick = digitalRead(boutonjoystick);

if (etatpoint == LOW){
  cptpoint++;
  delay(100);
}

if (etatjoystick == LOW){
  cptjoystick++;
  delay(100);
}
  
if (cptjoystick %2 == 0){ //Le mode JOYSTICK est éteint
 // Serial.println("Joystick désactivé");
  //delay(500);
  digitalWrite(ledR, LOW);
  cptjoystick == 0;
}

if (cptpoint%2 == 1){ //Le mode POINTILLE est activé (mais on doit utiliser un autre bouton pour faire une action)
  digitalWrite(ledV, HIGH);
 // Serial.println("\t mode Pointillé activé");
  //delay(500);
 
}

if (cptpoint%2 == 0){ //Le mode POINTILLE est activé (mais on doit utiliser un autre bouton pour faire une action)
  digitalWrite(ledV, LOW);
  //Serial.println("\t mode Pointillé désactivé");
  cptpoint = 0;
 // delay(500);
  
}
   
/* ----------------JOYSTICK-------------------------*/

if (cptjoystick % 2 == 1){
  Serial.println("Joystick activé");
  digitalWrite(ledR, HIGH);
 
  float X=analogRead(axeX)+15; // CHANGER LE DECALAGE
  float Y=analogRead(axeY)+8;

  double convx=((X*0.2)/1023)-0.1;
  double convy=((Y*0.2)/1023)-0.1;
  
  if(X <= 511.5+60 && X >= 511.5-60){
    X = 511.5;
  }
  if(Y <= 511.5+60 && Y >= 511.5-60){
    Y = 511.5;
  }

  if(posx>=2.5 && convx>=0){
    convx=0;
  }
  if(posx<=-2.5 && convx<=0){
    convx=0;
  }
  if(posy>=2.5 && convy<=0){
    convy=0;
  }
  if(posy<=-2.5 && convy>=0){
    convy=0;
  }

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

  monServo2.write(180-theta2(posx,posy-1.5));
  monServo1.write(180-theta1(posx,posy-1.5));

  if(!digitalRead(BOUTON) && an==haut){
    Serial.println("--------------BOUTON-------------- ");
    monServo3.write(bas);
    delay(100);
    monServo3.write(90);
    an=bas;
    delay(350);
  }

  else if(!digitalRead(BOUTON) && an==bas){
    Serial.println("--------------BOUTON-------------- ");
    monServo3.write(haut);
    delay(150);
    monServo3.write(90);
    an=haut;
    delay(350);
  }
  delay(50);
}



/* ---------------CERCLE------------------------*/

if (cptpoint%2 == 0 && etatcercle == LOW){
  digitalWrite(ledJ, HIGH);

  if(an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    delay(500);
    }
    
monServo2.write(180-theta2(3,0-1));
monServo1.write(180-theta1(3,0-1));

delay(50);

if(an==bas){
      monServo3.write(haut);
      delay(200);
      monServo3.write(90);

      an=haut;
      cpt=0;
      delay(500);
  }

delay(50);
for(float x=2.5;x>=0;x=x-0.015){
  Serial.print(theta1(x,x));Serial.print("   ");
  Serial.println(x);
  monServo2.write(180-theta2(x+0.5,sqrt((2.5*2.5)-(x*x))-1.5));
  monServo1.write(180-theta1(x+0.5,sqrt((2.5*2.5)-(x*x))-1.5));
  delay(10);
}

for(float x=0;x>=-2.5;x=x-0.015){
  Serial.print(theta1(x,x));Serial.print("   ");
  Serial.println(x);
  monServo2.write(180-theta2(x+0.5,sqrt((2.5*2.5)-(x*x))-1.5));
  monServo1.write(180-theta1(x+0.5,sqrt((2.5*2.5)-(x*x))-1.5));
  delay(10);
}

  for(float x=-2.5;x<=0;x=x+0.015){
  Serial.print(theta1(x,x));Serial.print("   ");
  Serial.println(x);
  monServo2.write(180-theta2(x+0.5,-1.5-sqrt((2.5*2.5)-(x*x))));
  monServo1.write(180-theta1(x+0.5,-1.5-sqrt((2.5*2.5)-(x*x))));
  delay(10);
}

for(float x=0;x<=2.5;x=x+0.015){
  Serial.print(theta1(x,x));Serial.print("   ");
  Serial.println(x);
  monServo2.write(180-theta2(x+0.5,-1.5-sqrt((2.5*2.5)-(x*x))));
  monServo1.write(180-theta1(x+0.5,-1.5-sqrt((2.5*2.5)-(x*x))));
  delay(10);
}


//monServo2.write(180-theta2(-2.5,-1.5));
 // monServo1.write(180-theta1(-2.5,-1.5));
  delay(200);
  if(an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    delay(500);
    }
  digitalWrite(ledJ, LOW);
}

/* ---------------CERCLE POINTILLE ------------------------*/

if (cptpoint%2 == 1 && etatcercle == LOW){
  digitalWrite(ledJ, HIGH);

  cpt=0;

  if(an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    delay(500);
    }
    
monServo2.write(180-theta2(-2.5+0.5,0-1.5));
monServo1.write(180-theta1(-2.5+0.5,0-1.5));

delay(200);

if(an==bas){
      monServo3.write(haut);
      delay(200);
      monServo3.write(90);

      an=haut;
      cpt=0;
      delay(500);
  }

delay(100);


if(an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    delay(500);
    }


  for(float x=-2.4;x<=0;x=x+0.015){
    Serial.print(theta1(x,x));Serial.print("   ");
    Serial.println(x);
    monServo2.write(180-theta2(x+0.5,sqrt((2.5*2.5)-(x*x))-1.5));
    monServo1.write(180-theta1(x+0.5,sqrt((2.5*2.5)-(x*x))-1.5));
      cpt=cpt+0.1;
    if(cpt>=0.2 && an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);

      an=bas;
      cpt=0;
    delay(50);
    }

    

    if(cpt>=5 && an==bas){
      monServo3.write(haut);
      delay(200);
      monServo3.write(90);

      an=haut;
      cpt=0;
      delay(50);
    }
    delay(10);
  }


for(float x=0;x<=2.5;x=x+0.015){
    Serial.print(theta1(x,x));Serial.print("   ");
    Serial.println(x);
    monServo2.write(180-theta2(x+0.5,sqrt((2.5*2.5)-(x*x))-1.5));
    monServo1.write(180-theta1(x+0.5,sqrt((2.5*2.5)-(x*x))-1.5));
      cpt=cpt+0.1;
    if(cpt>=0.2 && an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);

      an=bas;
      cpt=0;
    delay(50);
    }

    

    if(cpt>=5 && an==bas){
      monServo3.write(haut);
      delay(200);
      monServo3.write(90);

      an=haut;
      cpt=0;
      delay(50);
    }
    delay(10);
  }



if(an==bas){
      monServo3.write(haut);
      delay(200);
      monServo3.write(90);

      an=haut;
      cpt=0;
      
  }



if(an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    
    }







  for(float x=2.5;x>=0;x=x-0.015){
    Serial.print(theta1(x,x));Serial.print("   ");
    Serial.println(x);
    monServo2.write(180-theta2(x+0.5,-sqrt((2.5*2.5)-(x*x))-1.5));
    monServo1.write(180-theta1(x+0.5,-sqrt((2.5*2.5)-(x*x))-1.5));
      cpt=cpt+0.1;

    if(cpt>=0.2 && an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    delay(50);
    }

    if(cpt>=5 && an==bas){
      monServo3.write(haut);
      delay(200);
      monServo3.write(90);

      an=haut;
      cpt=0;
      delay(50);
    }
    delay(10);
  }


for(float x=0;x>=-2.5;x=x-0.015){
    Serial.print(theta1(x,x));Serial.print("   ");
    Serial.println(x);
    monServo2.write(180-theta2(x+0.5,-sqrt((2.5*2.5)-(x*x))-1.5));
    monServo1.write(180-theta1(x+0.5,-sqrt((2.5*2.5)-(x*x))-1.5));
      cpt=cpt+0.1;

    if(cpt>=0.2 && an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    delay(50);
    }

    if(cpt>=5 && an==bas){
      monServo3.write(haut);
      delay(200);
      monServo3.write(90);

      an=haut;
      cpt=0;
      delay(50);
    }
    delay(10);
  }



  delay(100);
  if(an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    delay(500);
    }
  digitalWrite(ledJ, LOW);
}


/*---------------LIGNE------------------------*/

if (cptpoint%2 == 0 && etatligne == LOW ){
    digitalWrite(ledB, HIGH);

if(an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    delay(500);
    }

monServo2.write(180-theta2(-2.1,0-3));
monServo1.write(180-theta1(-2.1,0-3));

delay(300);

if(an==bas){
      monServo3.write(haut);
      delay(150);
      monServo3.write(90);

      an=haut;
      cpt=0;
      delay(500);
  }

delay(50);

for(float x=-2.1;x<=2.9;x=x+0.027){
  monServo2.write(180-theta2(x,0-3));
  monServo1.write(180-theta1(x,0-3));
  delay(50);
}

  

  
  delay(100);
  if(an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    delay(500);
    }
  digitalWrite(ledB, LOW);
}

/* ---------------LIGNE POINTILLE------------------------*/

if (cptpoint%2 == 1 && etatligne == LOW){
    digitalWrite(ledB, HIGH);
  
  cpt=0;
  if(an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    delay(500);
    }
    
monServo2.write(180-theta2(-2.5,0-2));
monServo1.write(180-theta1(-2.5,0-2));

delay(300);

if(an==bas){
      monServo3.write(haut);
      delay(150);
      monServo3.write(90);

      an=haut;
      cpt=0;
      delay(500);
  }

delay(50);

  for(float x=-2.5;x<=2.5;x=x+0.027){
    monServo2.write(180-theta2(x,0-2));
    monServo1.write(180-theta1(x,0-2));

    cpt=cpt+0.1;
    Serial.println(cpt);
    if(cpt>=0.2 && an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
      delay(300);
    }

    if(cpt>=2 && an==bas){
      monServo3.write(haut);
      delay(150);
      monServo3.write(90);
      an=haut;
      cpt=0;
      delay(300);
    }
    delay(10);
  }
  delay(100);
  if(an==haut){
      monServo3.write(bas);
      delay(100);
      monServo3.write(90);
      an=bas;
      cpt=0;
    delay(500);
    }
  digitalWrite(ledB, LOW);
}


}