#include <SoftwareSerial.h>
  

  //Liaison série pour le module bluetooth

  SoftwareSerial soft2(8,9);
  

  //Tous les pins nécessaires à l'utilisation du contrôleur moteur

  int pwma = 10;
  int pwmb = 11;
  int ain1 = 7;
  int ain2 = 6;
  int bin1 = 5;
  int bin2 = 4;
  int stby = 3;

  
  //Pins pour le détecteur ultrasons de l'arduino

  int TRIG = A1;
  int ECHO = A0;

  
  //Distance mesurée par le robot

  long cm;

  //Valeur renvoyée par le capteur ultrasons

  long lect_echo;

  //Variable indiquant si le chemin à déjà été reçu ou non
  int envoi=0;
  //Variable pour stocker chaque octet reçu

  char inByte='a';

  
  void setup() {

    //Initialiser les liaisons série, 9600 hard pour le debug, 38400 soft2 pour le module bluetooth

    Serial.begin(9600);
    soft2.begin(38400);

  
    //Setup par défaut des pins du capteur ultrasons
    pinMode(TRIG,OUTPUT);
    digitalWrite(TRIG, LOW);
    pinMode(ECHO,INPUT);

  }
  
  void loop() {

    //Stocker le message reçu via bluetooth

    String msg;

  
    //Variables diverses utilisées par la suite

    int i = 0;
    int nb_if = 0;
    char id_sauv;
    int test_if;
    int nb_boucles[6];
    char id_boucles[6];
    int boucle_rec = 0;
    int capteur;

    //boucle dans une boucle gérée  
  

    //écouter sur le port série du module bluetooth, jusqu'à recevoir un chemin

    soft2.listen();

    while(soft2.available()>0 && inByte !='\n'){
      inByte=soft2.read();

      msg+=inByte;
      delay(10);
      i++;  
      envoi=1;

    }

    //++a01z++a++z   test if
    //++B 1Q+z test while (executé 32 fois)
    //++B 1B +Q1Q+z test while dans un while
    if(envoi==1){

  
      //Aller tout droit

      for(int k=0;k<i;k++){
        if((msg.charAt(k) & 0xF0) ==0x10){
        Serial.print("tout droit\n\r");
         digitalWrite(stby,HIGH);
          analogWrite(pwma,100);
          analogWrite(pwmb,100);
          digitalWrite(ain1,HIGH);
          digitalWrite(bin1,HIGH);
          digitalWrite(ain2,LOW);
          digitalWrite(bin2,LOW);
          delay(1000);
        }
  

      //Tourner à gauche

       else if((msg.charAt(k) & 0xF0) ==0x20){
        Serial.print("à gauche\n\r");
        digitalWrite(stby,HIGH);
        analogWrite(pwma,100);
        analogWrite(pwmb,100);
        digitalWrite(ain1,HIGH);
        digitalWrite(bin1,LOW);
        digitalWrite(ain2,LOW);
        digitalWrite(bin2,HIGH);
        delay(900);
        }
  

      //Tourner à droite

        else if((msg.charAt(k) & 0xF0) ==0x30){
           Serial.print("à droite\n\r");
           digitalWrite(stby,HIGH);
          analogWrite(pwma,100);
          analogWrite(pwmb,100);
          digitalWrite(ain1,LOW);
          digitalWrite(bin1,HIGH);
          digitalWrite(ain2,HIGH);
          digitalWrite(bin2,LOW);
          delay(900);
        }
  

        //Début boucle while

        else if((msg.charAt(k) & 0xF0) ==0x40){
          Serial.print("début while\n\r");
          k++;
          nb_boucles[boucle_rec] = (int)msg.charAt(k);
          id_boucles[boucle_rec] = msg.charAt(k-1);
          boucle_rec++;
        }
  

      //Fin boucle while

        else if((msg.charAt(k) & 0xF0) ==0x50){

          //LEs deux tableaux suivants stockent les différentes id des instructions while, et permettent donc de gérer le cas d'un while dans un while

          nb_boucles[boucle_rec-1]--;
          if(nb_boucles[boucle_rec-1]!=0){
            k--;
            while(msg.charAt(k)!= id_boucles[boucle_rec-1]){
              k--;
            }
            k++;
          }
          else{
            boucle_rec--;
            Serial.print("fin while\n\r");
          }
        }
  

        //IF

        else if((msg.charAt(k) & 0xF0) ==0x60){

          //Pour le if, on compare la valeur du deuxième octet à la valeur lue par la capteur, puis soit on continue le chemin actuel, soit on saute jusqu'au prochain if ayant la même id que le if actuel.

          Serial.print("if\n\r");
          digitalWrite(TRIG, HIGH);
          delayMicroseconds(50); 
          digitalWrite(TRIG, LOW);
          lect_echo = pulseIn(ECHO, HIGH); 
          cm = lect_echo / 58; 
          Serial.println(cm);
          delay(10);
          if((msg.charAt(k) & 0x01) == 0x01){
              if(msg.charAt(++k) > cm){
                nb_if++;
                test_if=nb_if;
                id_sauv = msg.charAt(k+1);
                while(((msg.charAt(k)) & 0xF0) != 0x70 && k<i){
                  k++;
                }
                while(test_if!=0){
                  if(((msg.charAt(k) & 0xF0) == 0x60)){
                    k++;
                    test_if--;
                  }
                  else k++;
                }
              }
          }
          else if((msg.charAt(k) & 0x01) == 0x00){
              if(msg.charAt(++k) < cm){
                nb_if++;
                test_if=nb_if;
                id_sauv = msg.charAt(k+1);
                while(((msg.charAt(k)) & 0xF0) != 0x70 && k<i){
                  k++;
                }
                while(test_if!=0){
                  if(((msg.charAt(k) & 0xF0) == 0x60)){
                    k++;
                    test_if--;
                  }
                  else k++;
                }
              }
          }
          }
  

      //Fin de parcours

        else if((msg.charAt(k) & 0xF0) ==0x70){
          Serial.print("fin parcours\n\r");
          envoi=2;
          digitalWrite(stby,LOW);
          analogWrite(pwma,0);
          analogWrite(pwmb,0);
          digitalWrite(ain1,LOW);
          digitalWrite(bin1,LOW);
          digitalWrite(ain2,LOW);
          digitalWrite(bin2,LOW);
          k=i;
        }
  
      }
    }
  }
  
  
  /*- Tout droit: 0001
  - Virage à gauche: 0010
  - Virage à droite: 0011
  - Début while: 0100
  - Fin while: 0101
  - If: 0110
  - Fin parcours: 0111*/