#include <SoftwareSerial.h>

//Déclaration des deux liaisons série. 
//Soft1 correspond au lien avec la première brique
//Soft2 correspond au lien avec le module bluetooth
SoftwareSerial soft1(10,11); //Rx,Tx
SoftwareSerial soft2(8,9); //Rx,Tx

//Pins pour le handshake de la liaison soft1
const int in1 = A0;
const int out1 = A1;

//Vérfie que le premier message à bien été envoyé
int envoi = 0;

//Message à envoyer au robot via bluetooth
String msg="";

//Permet de faire en sorte que le programme ne soit executé qu'une seule fois
int fin1=0;

void setup()
{

  //Liaison série avec l'ordinateur pour débuggage
  Serial.begin(9600);
  //Liaison avec la brique, fonctionnent toutes à 9600 baud
  soft1.begin(9600);
  //Liaison avec le bluetooth, le hc-05 fonctionne à 38400 baud
  soft2.begin(38400);

  //Mettre les pins du handshake en input/output
  pinMode(in1,INPUT);
  pinMode(out1,OUTPUT);

  //Mettre par défaut le handshake en low, pour pas de demande de communication
  digitalWrite(out1,LOW);
}

void loop()
{
  //Chaque message reçu est stocké temporaire dans cette chaine
  String msg_tmp="";

  //Premier message envoyé à la première brique
  String prim = "\n";

  //Variable où est stocké chaque octet reçu
  char inByte = 'a';

  //Pour les délais
  int tps,tps2;
  int timeout=0,timeout2=0;

  //Permet de déterminer quand arrêter d'écouter et envoyer le message en comptant le nombre de if
  int nb_if = 0;
  int occ2 =0;

  //En cas de premier envoi
  if(fin1==0){

  //Tant que la brique n'a pas accepté la communication, on tente d'envoyer un premier message
  while(envoi==0){

    //Début du protocole d'envoi via une liaison série
        digitalWrite(out1,HIGH);
        if(digitalRead(in1)==HIGH){
          soft1.print(prim);
          soft1.flush();
          envoi = 1;
        }
    //Fin du protocole d'envoi
        
  }

    digitalWrite(out1,LOW);
    tps =millis();

  //Tant que le message n'a pas été reçu en entier, on écoute sur le port série
  while(nb_if>=0){

  //Début du protocole d'écoute via la liaison série
    timeout = millis() - tps;
    if(digitalRead(in1)==HIGH){
    digitalWrite(out1,HIGH);
    soft1.listen();
    tps2=millis();
    if(soft1.available()>0){
    if(timeout2<50){
    inByte='a';
    while(inByte != '\n'){
      inByte = soft1.read();
      delay(10);
      if(inByte != '\n'){
        msg_tmp += inByte;
        if((inByte & 0x70) == 0x70){
          msg += msg_tmp;
          nb_if --;
          Serial.println("fin");
        }
        //Si on rencontre un if, on sait qu'on doit attendre un message de fin de circuit supplémentaire
        if((inByte & 0x60) == 0x60){
        if(occ2 == 0){
          nb_if ++;
          occ2=1;
          }
        else occ2 = 0;
        }
      }
      }
      Serial.print(msg_tmp);
      }
    }
    }
    //On a fini de recevoir une chaine, on reset alors msg_tmp, puis on attend la chaine suivante
      msg_tmp="";
      digitalWrite(out1, LOW);
  }
  //Fin de l'écoute

  //Le message a été reçu, et est envoyé via bluetooth
  msg += "\n\n\r";
  Serial.println("blorp");
  Serial.print(msg);
  soft2.print(msg);
  fin1=1;
 }
}