#include <Servo.h>
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16,2);
#define X_pin A0 
#define Y_pin A1 
#define sw 10 
#define Bb 2 //Bouton Bleu 
#define Bv 3 //Bouton Vert 
#define Br 4 //Bouton Rouge 
#define Bg 7 //Bouton Gris 
//Etat des boutons 
int bouttonb_stat = 0;
int bouttonv_stat = 0;
int bouttonr_stat = 0;
int bouttong_stat = 0;
float Rx;//Cordonnée Actuel selon x
float Ry;//Cordonnée Actuel selon y
float Rxx;//Cordonnée a atteindre selon x
float Ryy;//Cordonnée a atteindre selon y
int angle_effecteur=0;
//Definition des servos 
Servo droit;
Servo gauche;
Servo haut;
//Valeur de sauvgarde d'états 
int haut_s=50;
int bas_s=43;
int etat=bas_s;
float cpt=0;
//Parametres du modele geometrique inverse 
float l1=7;
float l4=8;
float l2=7;
float l3=8;
float l5=3.5;
float a=5;
int cpts=0;
int x_val; 
int y_val;
int sw_val;


//Modele geometrique inverse 
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;
  

}

//DEbut du programme 
void setup() {
  Serial.begin(9600);
  droit.attach(5);
  gauche.attach(6);
  haut.attach(9);
  pinMode(X_pin,INPUT);
  pinMode(Y_pin,INPUT);
  pinMode(sw,INPUT_PULLUP);
  pinMode (Bb,INPUT_PULLUP);
  pinMode (Bv,INPUT_PULLUP);
  pinMode (Br,INPUT_PULLUP);
  pinMode (Bg,INPUT_PULLUP);
  droit.write(90);
  gauche.write(120);
  haut.write(60);
  lcd.init();//initialisation de l'afficheur Lcd
  lcd.backlight();//Allumer le retro eclairage 
}

void loop() {

 //initialisation des bouttons 
  bouttong_stat = digitalRead(Bg);
  bouttonb_stat = digitalRead(Bb);
  bouttonr_stat = digitalRead(Br);
  bouttonv_stat = digitalRead(Bv);

//Ligne continue 
if(bouttonb_stat==LOW){
  haut.write(60);
  lcd.setCursor(1,0);//Position d'écriture dans l'afficheur 
  lcd.print("Mode : Ligne ");//Ecriture dans l'afficheur 
  haut.write(45);
  //Dessin d'une ligne de 5cm
    for(float i=-2.5;i<=2.5;i=i+0.0015){
       droit.write(180-theta1(i,12));
       gauche.write(180-theta2(i,12));
       }
  delay(50);
  haut.write(60);
  droit.write(180-theta1(-2.5,12));
  gauche.write(180-theta2(-2.5,12));
  delay(1000);
  lcd.clear();//Effacer
}

//Ligne discontinue 
if(bouttonv_stat==LOW){
  lcd.setCursor(1,0);//Position d'écriture dans l'afficheur 
  lcd.print("Mode : Ligne ");//Ecriture dans l'afficheur 
  lcd.setCursor(1,1);//Position d'écriture dans l'afficheur 
  lcd.print("Discontinue ");//Ecriture dans l'afficheur
   
//Dessin d'une ligne de 5cm 
   for(float i=-2.5;i<=2.5;i=i+0.1){
      droit.write(180-theta1(i,12));
      gauche.write(180-theta2(i,12));
      cpt=cpt+0.1;
//Sauvegarde de l'état 
      if(cpt>=0.1 && etat==bas_s ){
        etat=haut_s;
        haut.write(haut_s);
        delay(700);
     
          }
      else if(cpt>=0.1 && etat==haut_s)  {
        etat=bas_s;
        haut.write(bas_s);
        delay(700);
        
      }
     
    }
    
  delay(50);
  droit.write(180-theta1(-2.5,12));
  gauche.write(180-theta2(-2.5,12));
  delay(1000);
  lcd.clear();//Effacer
}

//Cercle
if(bouttonr_stat==LOW){
  lcd.setCursor(1,0);//Position d'écriture dans l'afficheur 
  lcd.print("Mode : Cercle ");//Ecriture dans l'afficheur
  haut.write(40);
//Dessin du demi cercle superieur  
for(float x=-2;x<=2;x=x+0.009){
  gauche.write(180-theta2(x,12+sqrt((2*2)-(x*x))));
 droit.write(180-theta1(x,12+sqrt((2*2)-(x*x))));
  delay(10);
}
//Dessin du demi cercle inferieur 
for(float x=2;x>=-2;x=x-0.009){
 gauche.write(180-theta2(x,12-sqrt((2*2)-(x*x))));
  droit.write(180-theta1(x,12-sqrt((2*2)-(x*x))));
  delay(10);
}

delay(2000);
lcd.clear();//Effacer
 }

//Cercle discontinue 
if(bouttong_stat==LOW){
 lcd.setCursor(1,0);//Position d'écriture dans l'afficheur 
  lcd.print("Mode : Cercle ");//Ecriture dans l'afficheur 
  lcd.setCursor(1,1);//Position d'écriture dans l'afficheur 
  lcd.print("Discontinue ");//Ecriture dans l'afficheur 
  cpt=0;
  etat=haut_s;
 //Dessin du demi cercle superieur 
for(float x=-2.5;x<=2.5;x=x+0.2){
 gauche.write(180-theta2(x,12+sqrt((2.5*2.5)-(x*x))));
 droit.write(180-theta1(x,12+sqrt((2.5*2.5)-(x*x))));
  delay(10);
    cpt=cpt+0.1;
      //Sauvgarde de l'etat actuel
      if(cpt>=0.1 && etat==bas_s ){
        etat=haut_s;
        cpt=0;
        haut.write(haut_s);
        delay(700);
     
          }
      else if(cpt>=0.1 && etat==haut_s)  {
        etat=bas_s;
        cpt=0;
        haut.write(bas_s);
        delay(700);
        
      }
}



 //Dessin du demi cercle inferieur  
for(float x=2.5;x>=-2.5;x=x-0.2){
 gauche.write(180-theta2(x,12-sqrt((2.5*2.5)-(x*x))));
  droit.write(180-theta1(x,12-sqrt((2.5*2.5)-(x*x))));
  delay(10);
    cpt=cpt+0.1;
    //Sauvgarde de l'etat actuele 
      if(cpt>=0.1 && etat==bas_s ){
        etat=haut_s;
        cpt=0;
        haut.write(haut_s);
        delay(700);
     
          }
      else if(cpt>=0.1 && etat==haut_s)  {
        etat=bas_s;
        haut.write(bas_s);
        delay(700);
        
      }
}

delay(2000);
lcd.clear();//Effacer
  }
 //Mode manuel
  if (bouttonr_stat == HIGH && bouttonb_stat == HIGH && bouttonv_stat == HIGH && bouttong_stat == HIGH) {
 lcd.setCursor(1,0);//Position d'écriture dans l'afficheur 
  lcd.print("Mode : Manuel  ");//Ecriture dans l'afficheur 

   // Lecture des valeurs du joystick
  sw_val=digitalRead(sw);
  Serial.println(sw_val);
  x_val = analogRead(X_pin);
  y_val = analogRead(Y_pin);
  //conversion
  Rx=map(x_val,0,1023,3,-3);
  Ry=map(y_val,0,1023,13.8,7);
  //Commande en vitesse 
  if (Rxx < Rx) {
    Rxx += 0.01;
    if (Rxx > Rx) {
      Rxx = Rx;
    }
  } else if (Rxx > Rx) {
    Rxx -= 0.01;
    if (Rxx < Rx) {
      Rxx = Rx;
    }
  }
  if (Ryy < Ry) {
    Ryy += 0.01;
    if (Ryy > Ry) {
      Ryy = Ry;
    }
  } else if (Ryy > Ry) {
    Ryy -= 0.01;
    if (Ryy < Ry) {
      Ryy = Ry;
    }
  }
 droit.write(180-theta1(Rxx,Ryy));
 gauche.write(180-theta2(Rxx,Ryy));
 
 //Lever le stylo 
 if(!digitalRead(sw) && etat==haut_s){
    haut.write(45);
    etat=bas_s;
    delay(300);
  }
  else if (!digitalRead(sw) && etat==bas_s){
    haut.write(60);
    etat=haut_s;
    delay(300);
  }

  
  /*if(cpts==1 &&  sw_val==0 ){
    haut.write(haut_s);
  }
  else{
    haut.write(bas_s);
  }*/
  delay(10);

}
}