#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*/