const int trigPinA = 13;
const int echoPinA = 12;
const int trigPinB = 14;
const int echoPinB = 27;
const int trigPinC = 26;
const int echoPinC = 25;
const int trigPinD = 33;
const int echoPinD = 32;

//define sound speed in cm/uS
#define SOUND_SPEED 0.034
//#define CM_TO_INCH 0.3937007874015748

long duration;
float distanceCm;
//float distanceInch;

float captureDuration = 50;
int movingAverageIteration = 25;

float calculateDistance(int trigPin, int echoPin){
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculate the distance
  distanceCm = duration * SOUND_SPEED/2;
  return distanceCm;
}

float movingAverage(int trigPin, int echoPin){
  float scanSensor = 0, result = 0, sumScanSensor = 0;
  for (int index = 0; index < movingAverageIteration; index++){
    scanSensor = calculateDistance(trigPin, echoPin);
    sumScanSensor += scanSensor;
    delay(captureDuration/movingAverageIteration);
  }
  result = sumScanSensor/movingAverageIteration;
  return result;
}

void setup() {
  Serial.begin(115200);
  pinMode(trigPinA, OUTPUT);
  pinMode(echoPinA, INPUT);
  pinMode(trigPinB, OUTPUT);
  pinMode(echoPinB, INPUT);
  pinMode(trigPinC, OUTPUT);
  pinMode(echoPinC, INPUT);
  pinMode(trigPinD, OUTPUT);
  pinMode(echoPinD, INPUT);
}

float distanceA, distanceB, distanceC, distanceD;
void loop() {
  Serial.println("");
  distanceA = movingAverage(trigPinA, echoPinA);
  distanceB = movingAverage(trigPinB, echoPinB);
  distanceC = movingAverage(trigPinC, echoPinC);
  distanceD = movingAverage(trigPinD, echoPinD);
  Serial.print("Distance A:");
  Serial.println(distanceA);
  Serial.print("Distance B:");
  Serial.println(distanceB);
  Serial.print("Distance C:");
  Serial.println(distanceC);
  Serial.print("Distance D:");
  Serial.println(distanceD);
}