//Hardware
#define distSensor A0 //Define what analog pin the distance sensor is on
int ledPin = 13; //Define what pin the warning LED is connected to

//Arrays
int distanceArray[9];
int medianDistanceArray[9];
int maxValuesArray[4];
int maxIndexArray[4];

//Counters
int i;
int j;
int k;

//Math variables
int distance;
float voltage;
int medianDistance;
float medianVoltage;
int arraySum;
int runningAverage;
int maxIndex;
int maxIndexTwo;
int medianFilterFinal;

void setup(){
  Serial.begin(9600); //Default Serial baud rate
  pinMode(ledPin, OUTPUT); //Set LED pin as input
  pinMode(distSensor, INPUT); //Define the distance sensor as an input
}

void loop(){
  int maxValue = 0;
  int maxValueTwo = 0;

  arraySum = 0; //Clear arraySum
  runningAverage = 0; //Clear runningAverage


  //Running Average loop
  for (i=0; i<=8; i++){
    delay(100); //Slow down Arduino to check sensor every 100ms (10hz, as specified in lab assignment)
    voltage = analogRead(distSensor) * 0.0048828125; //Read the data from the sensor, multiply it by a conversion factor then assign to a variable
    distance = 27.726 * pow(voltage, -1.2045); //Conversion from voltage to distance
    distanceArray[i] = distance; //Input data into the array according to the counter
    arraySum = arraySum + distanceArray[i]; //Sum the data of the array
    runningAverage = arraySum / 9; //Average out the data in the array
      }

  //Median Filtering loop
  for (j=0; j<=3; j++){
    for (k=0; k<=8; k++){
        delay(100); //Slow down Arduino to check sensor every 100ms (10hz, as specified in lab assignment)
        medianVoltage = analogRead(distSensor) * 0.0048828125; //Read the data from the sensor, multiply it by a conversion factor then assign to a variable
        medianDistance = 27.726 * pow(medianVoltage, -1.2045); //Conversion from voltage to distance
        medianDistanceArray[k] = medianDistance; //Input data into the array according to the counter

        if(medianDistanceArray[k] > maxValue){
          maxValue = medianDistanceArray[k];
          maxIndex = k;
          maxValuesArray[j] = maxValue;
          maxIndexArray[j] = maxIndex;
          }

        if(k=8){
          distanceArray[maxValue] = 0;
          }
        }
  if(maxValuesArray[j] > maxValueTwo){
   maxValueTwo = maxValuesArray[j];
   maxIndexTwo = j;
   if(j=3){
    medianFilterFinal = maxValueTwo;
    }
   }
  }

  //Out of Range check
  if(distance > 80){ //If statement for detecting if the sensor is beyond its max range
    digitalWrite(ledPin, HIGH); //If the sensor is beyond max range, turn on the warning LED
    Serial.println("Out of functioning range"); //Print warning for beyond max range
    }
  else if(distance < 80){ //Else if for if sensor is within max range
    digitalWrite(ledPin, LOW); //Turn led off if within range

    //Printing code
    Serial.print("Running Average: ");
    Serial.print(runningAverage);
    Serial.print("cm");
    Serial.println(" ");
    Serial.print("Median Filter: ");
    Serial.print(medianFilterFinal);
    Serial.print("cm");
    Serial.println(" ");
    Serial.println("-");
    }
}