/*
  
  Forum: https://forum.arduino.cc/t/how-to-program-3-ultrasonic-sensors/1178011
  Wokwi: https://wokwi.com/projects/378484791065958401

*/

constexpr byte noOfSensors = 3;
constexpr byte noOfLeds    = 3;

struct sensorType {
  byte no;
  byte triggerPin;
  byte echoPin;
  byte mode;
};

sensorType sensor[noOfSensors] = {
  {1, 2, 3, 255},
  {2, 4, 5, 255},
  {3, 7, 8, 255}
};

byte led[noOfLeds] = {9, 10, 11};

void setup() {
  Serial.begin(9600);
  for (int i = 0; i < noOfSensors; i++) {
    pinMode(sensor[i].triggerPin, OUTPUT);
    pinMode(sensor[i].echoPin, INPUT);
  }
  for (int i = 0; i < noOfLeds; i++) {
    pinMode(led[i], OUTPUT);
  }
}

void loop() {
  loop_ultrasonic(sensor[0]);
  loop_ultrasonic(sensor[1]);
  loop_ultrasonic(sensor[2]);
  controlLeds();
}

void loop_ultrasonic(sensorType &thisSensor) {
  unsigned long duration;
  float distance;
  digitalWrite(thisSensor.triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(thisSensor.triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(thisSensor.triggerPin, LOW);
  duration = pulseIn(thisSensor.echoPin, HIGH);
  distance = duration * 0.034 / 2;
  byte newMode = 0;
  if (distance <= 50) newMode = 1;
  if (distance <  20) newMode = 2;
  if (newMode == thisSensor.mode) { // This mode has already been handled!
    return;
  }
  thisSensor.mode = newMode;
  Serial.print("Sensor-No. ");
  Serial.print(thisSensor.no);
  Serial.print("\t");
  switch (newMode) {
    case 0 :  Serial.println("Jarak lebih dari 50cm");
      break;
    case 1 :  Serial.println("Jarak berada diantara 20cm hingga 50cm");
      break;
    case 2 : Serial.println("Jarak kurang dari 20cm");
      break;
  }
}

void controlLeds() {
  byte maxMode = 0;
  for (int i = 0; i < noOfSensors; i++) {
    if (sensor[i].mode != 255 && sensor[i].mode > maxMode) {
      maxMode = sensor[i].mode;
    }
  }
  switch (maxMode) {
    case 0 : 
      switchLeds(HIGH,LOW,LOW);
      break;
    case 1 :
      switchLeds(HIGH,HIGH,LOW);
      break;
    case 2 :
      switchLeds(HIGH,HIGH,HIGH);
      break;
  }
}

void switchLeds(byte state1, byte state2, byte state3) {
  digitalWrite(led[0], state1);
  digitalWrite(led[1], state2);
  digitalWrite(led[2], state3);
}