/* SR04_array_of_struct -- Ping multiple HC-SR04 sensors
  https://wokwi.com/projects/412101524873288705

  Modified from:
  https://wokwi.com/projects/399551802093377537 by https://wokwi.com/makers/xfpd
  found at https://forum.arduino.cc/t/need-help-with-autopilot-car-code/1311926/19?u=davex
*/
long duration, distance; // HC-SR04 measurements

// struct for holding table of sonar info
struct sonar_S {
  byte trigPin;
  byte echoPin;
  byte ledPin; // LEDs indicate active SR04
  const char * name;
};
sonar_S sonars[] = {  // Array of sonar structs
  {2, 3, 14, "FL"},
  {4, 5, 15, "FR"},
  {6, 7, 16, "L"},
  {8, 9, 17, "R"},
  {10, 11, 18, "Rear"},
};

byte sensors = sizeof(sonars) / sizeof(sonars[0]); // determine number of sensors
byte sensor = 0; // sensor counter

unsigned long timer, interval = 250; // timer between sequential pings

void setup() {
  Serial.begin (115200);
  for (auto & sonar : sonars)  { // interate through sonars
    pinMode(sonar.echoPin, INPUT);
    pinMode(sonar.trigPin, OUTPUT);
    pinMode(sonar.ledPin, OUTPUT);
  }
}

void loop() {
  if (millis() - timer > interval) { // waiting for "interval" to elapse
    timer = millis(); // store "now" time
    if (sensor == sensors) { // count equals sensors
      sensor = 0; // reset sensor counter
      Serial.println(); // format Serial Monitor output
    }
    myPing(sensor); // call myPing() function
    sensor++; // increment sensor counter
  }
}

void myPing(int SR04) {
  auto & sonar = sonars[SR04]; //  lookup the sonar in the array of structs
  digitalWrite(sonar.ledPin, HIGH); // indicate active SR04
  //Serial.print("D");
  Serial.print(sonar.name);
  Serial.print(": ");

  // ping
  // digitalWrite(sonar.trigPin, LOW);
  // delayMicroseconds(2);
  digitalWrite(sonar.trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(sonar.trigPin, LOW);
  float distance = pulseIn(sonar.echoPin, HIGH) / 58.2;
  if (distance < 100) Serial.print(" ");
  if (distance < 10) Serial.print(" ");
  Serial.print(distance);
  // Serial.print(pulseIn(sonar.echoPin, HIGH) / 58.2 ); // (time / 2) / 29.1
  // delay(500);
  // print/LED
  Serial.print(" ");
  digitalWrite(sonar.ledPin, LOW);
}