#define trigFront 2
#define echoFront 3

#define trigLeft 4
#define echoLeft 5

#define trigRight 6
#define echoRight 7

#define ENA 9
#define IN1 8
#define IN2 10

#define ENB 11
#define IN3 12
#define IN4 13

int compteurAngle = 0;
int distanceMur = 20;

long lireDistance(int trig, int echo) {

  digitalWrite(trig, LOW);
  delayMicroseconds(2);

  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  long duree = pulseIn(echo, HIGH);
  long distance = duree * 0.034 / 2;

  return distance;
}

void avancer() {

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);

  analogWrite(ENA, 150);
  analogWrite(ENB, 150);
}

void stopRobot() {

  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

void tournerDroite() {

  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);

  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);

  analogWrite(ENA, 150);
  analogWrite(ENB, 150);

  delay(400);

  compteurAngle -= 90;
}

void tournerGauche() {

  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);

  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);

  analogWrite(ENA, 150);
  analogWrite(ENB, 150);

  delay(400);

  compteurAngle += 90;
}

void setup() {

  pinMode(trigFront, OUTPUT);
  pinMode(echoFront, INPUT);

  pinMode(trigLeft, OUTPUT);
  pinMode(echoLeft, INPUT);

  pinMode(trigRight, OUTPUT);
  pinMode(echoRight, INPUT);

  pinMode(ENA, OUTPUT);
  pinMode(ENB, OUTPUT);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  Serial.begin(9600);
}

void loop() {

  int front = lireDistance(trigFront, echoFront);
  int left = lireDistance(trigLeft, echoLeft);
  int right = lireDistance(trigRight, echoRight);

  bool murAvant = front < distanceMur;
  bool murGauche = left < distanceMur;
  bool murDroite = right < distanceMur;

  Serial.print("Angle: ");
  Serial.println(compteurAngle);

  // mode direction principale
  if (!murAvant && compteurAngle == 0) {

    avancer();
  }

  else {

    // suivi du mur

    if (!murDroite) {

      tournerDroite();

    }

    else if (!murAvant) {

      avancer();

    }

    else if (!murGauche) {

      tournerGauche();

    }

    else {

      tournerDroite();
      tournerDroite();
    }
  }

  delay(50);
}