/* HCSR04UltrasonicDistanceSensorv4Function

   The goal of this sketch is to use multiple HC-SR04 modules triggered in Order

   pins used
   pin 7 to Trig Pin...sensor 1
   pin 6 to Echo pin...sensor 1
   pin 5 Trig pin...sensor 2
   pin 4 Echo pin...sensor 2
   pin 3 Trig pin...sensor 3
   pin 2 Echo pin...sensor 3
*/

//Global Variables
//const int is a constant integer that cannot have it's value changed.
const int triggerPin = 7;
const int echoPin = 6;
const int triggerPin2 = 5;
const int echoPin2 = 4;
const int triggerPin3 = 3;
const int echoPin3 = 2;

//nextsensor keeps a record of which sensor to trigger next
int nextSensor = 0;
unsigned long sensorTimer1;

//Function to trigger a sensor
//It receives the pins of the sensor and send back the distance
int getDistanceHCSSR04(int funcTriggerPin, int funcEchoPin) {
  //An unsigned long is needed because the numbers produced can be much larger
  //than an int can hold (max value 32,767)
  //Local Variables
  unsigned long duration;  //time taken for echo
  int myDistance;
  digitalWrite(funcTriggerPin, LOW);

  //A tiny delay 2 millionths of a second
  delayMicroseconds(2);

  // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
  //same as turing the LED on in the blink sketch
  digitalWrite(funcTriggerPin, HIGH);

  //pin stays HIGH (5v) for 10 microseconds to trigger a pulse
  delayMicroseconds(10);

  //Pin taken LOW (0v) after pulse triggered ready for next pulse
  digitalWrite(funcTriggerPin, LOW);

  duration = pulseIn(funcEchoPin, HIGH);

  myDistance = duration * 0.34 / 2;
  return myDistance;
}

void setup() {
  Serial.begin(9600);
  Serial.println("HCSR04UltrasonicDistanceSensorv4Function...");
  Serial.println(" ");//Blank line
  //pinMode set for OUTPUT...sending HIGH of LOW
  pinMode(triggerPin, OUTPUT);  //sends signal
  //pinMode set for INPUT, we will receive an incoming HIGH or LOW on this pin
  pinMode(echoPin, INPUT);      //receives signal
  //more pinModes for 2 more sensors
  pinMode(triggerPin2, OUTPUT);  //sends signal
  pinMode(echoPin2, INPUT);      //receives signal
  pinMode(triggerPin3, OUTPUT);  //sends signal
  pinMode(echoPin3, INPUT);      //receives signal
}

void loop() {
  //Local variable
  int distance;   //in mm
  //A single millis() timer loop
  if (millis() > sensorTimer1) {
    sensorTimer1 = millis() + 500;
    //A switch statement to decide which sensor to trigger next
    switch (nextSensor) {
      case 1:
        distance = getDistanceHCSSR04(triggerPin2, echoPin2);
        Serial.print(" Sensor 2 Distance: ");
        Serial.print(distance);
        Serial.println(" mm");
        break;
      case 2:
        distance = getDistanceHCSSR04(triggerPin3, echoPin3);
        Serial.print(" Sensor 3 Distance: ");
        Serial.print(distance);
        Serial.println(" mm");
        break;
      default:
        distance = getDistanceHCSSR04(triggerPin, echoPin);
        Serial.print(" Sensor 1 Distance: ");
        Serial.print(distance);
        Serial.println(" mm");
        break;
    }
    //increment the vale of nextSensor so that the next sensor will be selected next time round
    nextSensor++;
    //If the value is greater than 2 got back to sensor 0
    if (nextSensor > 2) {
      nextSensor = 0;
    }
  }


}