/*
  Multi HC-SR04 Demonstration
  Uses a CD74HC4067 multiplexer to select up to 16 sensors.
  2/1/26
  AnonEngineering
*/

const int NUM_SENSORS = 4;
const int ADDR_PINS[] = {27, 26, 25, 33};
const int ECHO_PIN = 32;
const int TRIG_PIN = 13;

int getDistance() {
  // send trigger
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  // calculate distance
  unsigned long duration = pulseIn(ECHO_PIN, HIGH);
  float distance = (duration * .034) / 2;
  // return distance as an integer
  return (int)distance;
}

void setChannel(int channel)  {
  for (int i = 0; i < 4; i++) {
    digitalWrite(ADDR_PINS[i], (channel >> i) & 0x01);
  }
}

void showDistance(int channel, int distance)  {
  Serial.print("Channel ");
  Serial.print(channel);
  Serial.print(":\t");
  Serial.print(distance);
  Serial.println(" cm");
}

void showSplash() {
  Serial.println("\nMulti HC-SR04 Sensor Demo");
  Serial.print("Displays distances from ");
  Serial.print(NUM_SENSORS);
  Serial.println(" sensors\n");
}

void setup() {
  Serial.begin(115200);
  for (int i = 0; i < 4; i++) {
    pinMode(ADDR_PINS[i], OUTPUT);
  }
  pinMode(ECHO_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  showSplash();
}

void loop() {
  for (int channel = 0; channel < NUM_SENSORS; channel++) {
    setChannel(channel);
    int dist = getDistance();
    showDistance(channel, dist);
    delay(1000);
  }
}