// MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START * MACRO-START *
#define dbg(myFixedText, variableName) \
  Serial.print( F(#myFixedText " "  #variableName"=") ); \
  Serial.println(variableName);

#define dbgi(myFixedText, variableName,timeInterval) \
  { \
    static unsigned long intervalStartTime; \
    if ( millis() - intervalStartTime >= timeInterval ){ \
      intervalStartTime = millis(); \
      Serial.print( F(#myFixedText " "  #variableName"=") ); \
      Serial.println(variableName); \
    } \
  }

#define dbgc(myFixedText, variableName) \
  { \
    static long lastState; \
    if ( lastState != variableName ){ \
      Serial.print( F(#myFixedText " "  #variableName" changed from ") ); \
      Serial.print(lastState); \
      Serial.print( F(" to ") ); \
      Serial.println(variableName); \
      lastState = variableName; \
    } \
  }

#define dbgcf(myFixedText, variableName) \
  { \
    static float lastState; \
    if ( lastState != variableName ){ \
      Serial.print( F(#myFixedText " "  #variableName" changed from ") ); \
      Serial.print(lastState); \
      Serial.print( F(" to ") ); \
      Serial.println(variableName); \
      lastState = variableName; \
    } \
  }
// MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END * MACRO-END *

//Bonne chance :)

//Connection bouton1&2
const int bouton = 10;
int etatBouton;
const int bouton2 = 2;
int etatBouton2;
//Connection capteur distance
int sinal = 6;
float distance;
// Motor A connections
int enA = 9;
int in1 = 8;
int in2 = 7;
// Motor B connections
int enB = 3;
int in3 = 5;
int in4 = 4;

void checkLEDS() {
  digitalWrite(enA,HIGH);
  digitalWrite(enB,HIGH);

  digitalWrite(in1,HIGH);
  digitalWrite(in2,HIGH);
  digitalWrite(in3,HIGH);
  digitalWrite(in4,HIGH);
  delay(2000);
  digitalWrite(enA,LOW);
  digitalWrite(enB,LOW);

  digitalWrite(in1,LOW);
  digitalWrite(in2,LOW);
  digitalWrite(in3,LOW);
  digitalWrite(in4,LOW);

}

void setup() {
  // Set up du capteur
  Serial.begin(9600); //on sait pas
  Serial.println("Setup-Start");
  pinMode(sinal, OUTPUT);
  digitalWrite(sinal, LOW);
          
  pinMode(bouton, INPUT_PULLUP);
  pinMode(bouton2, INPUT_PULLUP);

  // Set all the motor control pins to outputs
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  checkLEDS();
  
  // Turn off motors - Initial state
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
  Serial.println("exiting Setup");
}


// This function lets you control spinning direction of motors
void directionControl() {
  // Set motors to maximum speed
  // For PWM maximum possible values are 0 to 255
  //analogWrite(enA, 0); // zero means no PWM at all
  //analogWrite(enB, 0);
  analogWrite(enA, 255);
  analogWrite(enB, 255);
}


void read() {
  pinMode(sinal, OUTPUT);
  digitalWrite(sinal, HIGH);
  delayMicroseconds(20);
  digitalWrite(sinal, LOW);

  pinMode(sinal, INPUT);
  distance = pulseIn(sinal, HIGH);
}

void loop() {
  etatBouton = digitalRead(10); //lire état bouton1
  dbgc("1:",etatBouton);
  
  etatBouton2 = digitalRead(2);
  dbgc("2:",etatBouton2);

  //Serial.println(etatBouton);
  //Serial.println(etatBouton2);

  //read(); //lire la distance
  //distance=distance/2;
  //distance=distance*0.034029;
  dbgcf("3:",distance);    

  directionControl();
  //delay(1000);

  //Serial.println(distance);
  //delay(100);


  //Conditions moteurs
  if (distance < 100 && etatBouton == LOW && etatBouton2 == LOW) { //boutons pas appuyés
    dbgi("bouton12 LOW ",1,1000);
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW); // les moteurs ne tournent pas
  }
  else if (distance < 100 && etatBouton == LOW && etatBouton2 == HIGH) { // bouton1 pas appuyé, bouton2 appuyé
    dbgi("B1 LOW B2 HIGH",2,1000);
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW); // Le moteur ne tourne pas
  }
  else if (distance < 100 && etatBouton == HIGH && etatBouton2 == LOW) { // bouton1 appuyé, bouton2 pas appuyé
    dbgi("B1 HIGH B2 LOW",3,1000);
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW); // Le moteur ne tourne pas
  }
  else if (distance > 100 && etatBouton == LOW && etatBouton2 == LOW) { // boutons pas appuyés
    dbgi("distance > 100 B1 LOW B2 LOW",4,1000);
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW); // Le moteur tourne pas
  }
  else if (distance > 100 && etatBouton == LOW && etatBouton2 == HIGH) { // bouton1 pas appuyé, bouton2 appuyé
    dbgi("distance > 100 B1 LOW B2 HIGH",5,1000);
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH); // Le moteur tourne en arrière

    analogWrite(enA, 255);
    analogWrite(enB, 255);
  }
  else if (distance > 100 && etatBouton == HIGH && etatBouton2 == LOW) { // bouton1 appuyé, bouton2 pas appuyé
    dbgi("distance > 100 B1 HIGH B2 LOW",6,1000);
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW); // Le moteur tourne en avant

    analogWrite(enA, 255);
    analogWrite(enB, 255);
  } 
  else {
    //dbgi("else",distance,1000);
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW); // Le moteur s'arrête de tourner
  }
}