const int LED1 = 18;
const int LED2 = 19;
const int LED3 = 27;
const int LED4 = 26;

#define ECHO_PIN1 15
#define TRIG_PIN1 2

#define ECHO_PIN2 4
#define TRIG_PIN2 5

#define ECHO_PIN3 34
#define TRIG_PIN3 32

#define ECHO_PIN4 33
#define TRIG_PIN4 25

void setup() {
  //Serial.begin(115200);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);
  pinMode(LED4, OUTPUT);
  pinMode(TRIG_PIN1, OUTPUT);
  pinMode(ECHO_PIN1, INPUT);
  pinMode(TRIG_PIN2, OUTPUT);
  pinMode(ECHO_PIN2, INPUT);
  pinMode(TRIG_PIN3, OUTPUT);
  pinMode(ECHO_PIN3, INPUT);
  pinMode(TRIG_PIN4, OUTPUT);
  pinMode(ECHO_PIN4, INPUT);
}

float readDistanceCM1() {
  digitalWrite(TRIG_PIN1, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN1, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN1, LOW);
  int duration = pulseIn(ECHO_PIN1, HIGH);
  return duration * 0.034 / 2;
}

float readDistanceCM2() {
  digitalWrite(TRIG_PIN2, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN2, LOW);
  int duration = pulseIn(ECHO_PIN2, HIGH);
  return duration * 0.034 / 2;
}

float readDistanceCM3() {
  digitalWrite(TRIG_PIN3, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN3, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN3, LOW);
  int duration = pulseIn(ECHO_PIN3, HIGH);
  return duration * 0.034 / 2;
}

float readDistanceCM4() {
  digitalWrite(TRIG_PIN4, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN4, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN4, LOW);
  int duration = pulseIn(ECHO_PIN4, HIGH);
  return duration * 0.034 / 2;
}

void loop() {
  float distance1 = readDistanceCM1();
  float distance2 = readDistanceCM2();
  float distance3 = readDistanceCM3();
  float distance4 = readDistanceCM4();

  if(distance1 < 200){
   digitalWrite(LED1, HIGH);
  } else {
   digitalWrite(LED1, LOW);
  }

  if(distance2 < 200){
   digitalWrite(LED2, HIGH);
  } else {
   digitalWrite(LED2, LOW);
  }
  
  if(distance3 < 200){
   digitalWrite(LED3, HIGH);
  } else {
   digitalWrite(LED3, LOW);
  }

  if(distance4 < 200){
   digitalWrite(LED4, HIGH);
  } else {
   digitalWrite(LED4, LOW);
  }

  // Serial.print("Jarak1: ");
  // Serial.println(distance1);
  // Serial.print("Jarak2: ");
  // Serial.println(distance2);

  delay(200);
}