/*
  Pada video kali ini kita akan belajar bagaimana menggunakan 3 sensor HCSR04
  untuk menyalakan beberapa lampu.
  kondisinya jika sensor1 bernilai kurang dari 100 cm dan sensor2 bernilai lebih dari 100 cm maka lampu LED kuning menyala
  jika sensor1 bernilai lebih dari 100 cm dan sensor2 bernilai kurang dari 100 cm maka lampu LED hijau menyala
  jika sensor1 bernilai kurang dari 100 cm dan sensor2 bernilai kurang dari 100 cm maka lampu LED biru menyala
  selain kondisi diatas maka lampu oranye menyala.

  Selain itu ada sensor3 yang akan digunakan untuk menyalakan lampu merah. jika jarak dari sensor3 kurang dari 50 cm maka lampu merah menyala. Jika jarak lebih dari 50 cm maka lampu mati.

  Mari kita mulai rekan-rekan
*/

byte ledMerah = 18; // pin D18
byte ledKuning = 15;
byte ledHijau = 2;
byte ledBiru = 4;
byte ledOranye = 5;

byte trigPin1 = 12;
byte echoPin1 = 34;
byte trigPin2 = 14;
byte echoPin2 = 39;
byte trigPin3 = 13;
byte echoPin3 = 36;

#define kecepatanSuara 0.034

float jarakcm1, jarakcm2, jarakcm3;

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

  //inisialisasi pin
  pinMode(ledMerah, OUTPUT);
  pinMode(ledKuning, OUTPUT);
  pinMode(ledHijau, OUTPUT);
  pinMode(ledBiru, OUTPUT);
  pinMode(ledOranye, OUTPUT);

  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);

  //atur lampu mati semua
  ledMati();

  Serial.println("Multi ultrasonic sensor dengan ESP32");
  Serial.println("Sensor 1(cm), Sensor 2 (cm), Sensor 3 (cm)");
}

void loop() {
  jarakcm1 = ambilJarak(trigPin1, echoPin1);
  jarakcm2 = ambilJarak(trigPin2, echoPin2);
  jarakcm3 = ambilJarak(trigPin3, echoPin3);

  Serial.print(jarakcm1, 2);
  Serial.print(" , ");
  Serial.print(jarakcm2, 2);
  Serial.print(" , ");
  Serial.println(jarakcm3);

  ledMati();

  //memilih kondisi
  //jika sensor1 bernilai kurang dari 100 cm dan sensor2 bernilai lebih dari 100 cm maka lampu LED kuning menyala
  if (jarakcm1 < 100 && jarakcm2 >= 100) {
    Serial.println("Lampu kuning menyala");
    digitalWrite(ledKuning, 1);
  }
  //jika sensor1 bernilai lebih dari 100 cm dan sensor2 bernilai kurang dari 100 cm maka lampu LED hijau menyala
  else if (jarakcm1 >= 100 && jarakcm2 < 100) {
    Serial.println("Lampu hijau menyala");
    digitalWrite(ledHijau, 1);
  }
  //jika sensor1 bernilai kurang dari 100 cm dan sensor2 bernilai kurang dari 100 cm maka lampu LED biru menyala
  else if (jarakcm1 < 100 && jarakcm2 < 100) {
    Serial.println("Lampu biru menyala");
    digitalWrite(ledBiru, 1);
  }
  //selain kondisi diatas maka lampu oranye menyala.
  else {
    Serial.println("Lampu oranye menyala");
    digitalWrite(ledOranye, 1);
  }

  //Jika jarak dari sensor3 kurang dari 50 cm maka lampu merah menyala.
  if (jarakcm3 < 50) {
    Serial.println("Lampu Merah menyala");
    digitalWrite(ledMerah, 1);
  }
  //Jika jarak lebih dari 50 cm maka lampu mati.
  else {
    Serial.println("Lampu Merah mati");
    digitalWrite(ledMerah, 0);
  }
  delay(500);
}

float ambilJarak(byte trigPin, byte echoPin) {
  float jarak = 0;
  unsigned long durasi = 0;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  durasi = pulseIn(echoPin, HIGH);

  // Calculate the distance
  jarak = durasi * kecepatanSuara / 2;
  return jarak;
}

void ledMati() {
  digitalWrite(ledMerah, 0);
  digitalWrite(ledKuning, 0);
  digitalWrite(ledHijau, 0);
  digitalWrite(ledBiru, 0);
  digitalWrite(ledOranye, 0);
}