#define TF 5
#define EF 18

#define TL 17
#define EL 19

#define TR 16
#define ER 21

#define TB 4
#define EB 22

#define LED_FAST 25
#define LED_MED 26
#define LED_SLOW 27
#define LED_STOP 14

long getDistance(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo, HIGH, 30000);
  return duration * 0.034 / 2;
}

void setup() {
  Serial.begin(9600);

  pinMode(TF, OUTPUT); pinMode(EF, INPUT);
  pinMode(TL, OUTPUT); pinMode(EL, INPUT);
  pinMode(TR, OUTPUT); pinMode(ER, INPUT);
  pinMode(TB, OUTPUT); pinMode(EB, INPUT);

  pinMode(LED_FAST, OUTPUT);
  pinMode(LED_MED, OUTPUT);
  pinMode(LED_SLOW, OUTPUT);
  pinMode(LED_STOP, OUTPUT);
}

void loop() {
  long front = getDistance(TF, EF);
  long left  = getDistance(TL, EL);
  long right = getDistance(TR, ER);
  long back  = getDistance(TB, EB);
  
  Serial.print("Front: "); Serial.print(front);
  Serial.print(" | Left: "); Serial.print(left);
  Serial.print(" | Right: "); Serial.print(right);
  Serial.print(" | Back: "); Serial.println(back);

  digitalWrite(LED_FAST, LOW);
  digitalWrite(LED_MED, LOW);
  digitalWrite(LED_SLOW, LOW);
  digitalWrite(LED_STOP, LOW);

  if (front > 80) {
    digitalWrite(LED_FAST, HIGH);
    Serial.println("MOVE_FORWARD_FAST");
  }
  else if (front > 50) {
    digitalWrite(LED_MED, HIGH);
    Serial.println("MOVE_FORWARD_MEDIUM");
  }
  else if (front > 30) {
    digitalWrite(LED_SLOW, HIGH);
    Serial.println("MOVE_FORWARD_SLOW");
  }
  else {
    digitalWrite(LED_STOP, HIGH);
    Serial.println("STOP");
  }

  delay(200);
}