#define echoPinDepanKiri  5
#define trigPinDepanKiri 4

#define echoPinDepanKanan  7
#define trigPinDepanKanan 6

#define echoPinBelakangKiri 9
#define trigPinBelakangKiri 8

#define echoPinBelakangKanan 11
#define trigPinBelakangKanan 10

#define echoPinKiriSatu 13
#define trigPinKiriSatu 12

#define echoPinKiriDua 3
#define trigPinKiriDua 2

#define echoPinKananSatu 1
#define trigPinKananSatu 0

// #define echoPinKananDua 4
// #define trigPinKananDua 3

const int lampuDepan = A3; // Lampu Depan
const int lampuBelakang = A2; // Lampu Belakang
const int lampuKanan = A0; // Lampu Kanan
const int lampuKiri = A1; // Lampu Kiri

void setup() {
  Serial.begin(115200);
  pinMode(lampuDepan, OUTPUT);
  pinMode(lampuBelakang, OUTPUT);
  pinMode(lampuKiri, OUTPUT);
  pinMode(lampuKanan, OUTPUT);

  pinMode(trigPinDepanKiri, OUTPUT);
  pinMode(echoPinDepanKiri, INPUT);
  pinMode(trigPinDepanKanan, OUTPUT);
  pinMode(echoPinDepanKanan, INPUT);

  pinMode(trigPinBelakangKiri, OUTPUT);
  pinMode(echoPinBelakangKiri, INPUT);
  pinMode(trigPinBelakangKanan, OUTPUT);
  pinMode(echoPinBelakangKanan, INPUT);

  pinMode(trigPinKiriSatu, OUTPUT);
  pinMode(echoPinKiriSatu, INPUT);
  pinMode(trigPinKiriDua, OUTPUT);
  pinMode(echoPinKiriDua, INPUT);

  pinMode(trigPinKananSatu, OUTPUT);
  pinMode(echoPinKananSatu, INPUT);
}

float readDistanceCM() {
  digitalWrite(trigPinDepanKiri, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinDepanKiri, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinDepanKiri, LOW);
  int duration = pulseIn(echoPinDepanKiri, HIGH);
  return duration * 0.034 / 2;
}

float bacaDepanKanan() {
  digitalWrite(trigPinDepanKanan, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinDepanKanan, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinDepanKanan, LOW);
  int duration = pulseIn(echoPinDepanKanan, HIGH);
  return duration * 0.034 / 2;
}

float bacaBelakangKiri() {
  digitalWrite(trigPinBelakangKiri, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinBelakangKiri, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinBelakangKiri, LOW);
  int duration = pulseIn(echoPinBelakangKiri, HIGH);
  return duration * 0.034 / 2;
}

float bacaBelakangKanan() {
  digitalWrite(trigPinBelakangKanan, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinBelakangKanan, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinBelakangKanan, LOW);
  int duration = pulseIn(echoPinBelakangKanan, HIGH);
  return duration * 0.034 / 2;
}

float bacaKiriSatu() {
  digitalWrite(trigPinKiriSatu, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinKiriSatu, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinKiriSatu, LOW);
  int duration = pulseIn(echoPinKiriSatu, HIGH);
  return duration * 0.034 / 2;
}

float bacaKiriDua() {
  digitalWrite(trigPinKiriDua, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinKiriDua, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinKiriDua, LOW);
  int duration = pulseIn(echoPinKiriDua, HIGH);
  return duration * 0.034 / 2;
}

float bacaKananSatu() {
  digitalWrite(trigPinKananSatu, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPinKananSatu, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPinKananSatu, LOW);
  int duration = pulseIn(echoPinKananSatu, HIGH);
  return duration * 0.034 / 2;
}

void loop() {
  float distance = readDistanceCM();
  float distance1 = bacaDepanKanan();
  if(distance < 40){
    digitalWrite(lampuDepan, HIGH);
    Serial.print("LAMPU DEPAN KIRI NYALA\n");
  }else if(distance1 < 40){
    digitalWrite(lampuDepan, HIGH);
    Serial.print("LAMPU DEPAN KANAN NYALA\n");
  } else {
    digitalWrite(lampuDepan, LOW);
  }

  float distance2 = bacaBelakangKiri();
  float distance3 = bacaBelakangKanan();
  if(distance2 < 20){
    digitalWrite(lampuBelakang, HIGH);
    Serial.print("LAMPU BELAKANG KIRI NYALA\n");
  } else if(distance3 < 20){
    digitalWrite(lampuBelakang, HIGH);
    Serial.print("LAMPU BELAKANG KANAN NYALA\n");
  } else {
    digitalWrite(lampuBelakang, LOW);
  }

  float distance4 = bacaKiriSatu();
  float distance5 = bacaKiriDua();
  if(distance4 < 30){
    digitalWrite(lampuKiri, HIGH);
    Serial.print("LAMPU KIRI NYALA\n");
  } else if(distance5 < 30){
    digitalWrite(lampuKiri, HIGH);
    Serial.print("LAMPU KIRI NYALA\n");
  } else {
    digitalWrite(lampuKiri, LOW);
  }

  float distance6 = bacaKananSatu();
  if(distance6 < 30){
    digitalWrite(lampuKanan, HIGH);
    Serial.print("LAMPU KANAN NYALA\n");
  } else {
    digitalWrite(lampuKanan, LOW);
  }

  delay(100);
}