#include <SoftwareSerial.h>

SoftwareSerial soft2(8,9);

int pwma = 10;
int pwmb = 11;
int ain1 = 7;
int ain2 = 6;
int bin1 = 5;
int bin2 = 4;
int stby = 3;
int TRIG = A1;
int ECHO = A0;
long cm;
long lect_echo;
 int envoi=0;
char inByte='a';

void setup() {
  Serial.begin(9600);
  soft2.begin(38400);
   pinMode(TRIG,OUTPUT);
digitalWrite(TRIG, LOW);
pinMode(ECHO,INPUT);
}

void loop() {
  String msg;
  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  

  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){
    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);
      }

     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);
      }

      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);
      }

      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++;
      }

      else if((msg.charAt(k) & 0xF0) ==0x50){
        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");
        }
      }

      else if((msg.charAt(k) & 0xF0) ==0x60){
        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++;
              }
            }
        }
        }

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