//Pin Declarations
#define Uss_Trig 27          //Sonic Trig Sensor 
#define Uss_Echo_FM 29        //Sonic Echo Sensor Front Middle

//Variable Declarations
//Loop
int sensorF_M;  //Front Middle sonic sensor current distance


//USonic sensors
unsigned long TimeNowMillis = 0;
unsigned long TimeNowMicros = 0;
unsigned long USonic_TimeSinceTrigHigh = 0;
unsigned long USonic_TimeSinceTrigLow = 0;

struct UsSensor
{
  float USonicSensor_FM;
}
ussNow;

void setup() {

  //Set up GPS
  Serial.begin(9600); // connect gps sensor
  
  //Set Trig pins for all sensors 
  pinMode(Uss_Trig, OUTPUT);

  //Set Echo pins for all sensors
  pinMode(Uss_Echo_FM, INPUT);
}


void loop() 
{ 
  //int sensorF_M = average_Filter();
  ussNow =  USonicSensors(); 
  Serial.println(ussNow.USonicSensor_FM);
  
  
}


/****************** Forward Facing Sensor Functions *******************/
//Function to get distance value of Front Middle Sensor
struct UsSensor USonicSensors()
{
  TimeNowMicros = micros();

  if(TimeNowMicros - USonic_TimeSinceTrigHigh > 10)
  {
    USonic_TimeSinceTrigHigh = micros();
    digitalWrite(Uss_Trig, HIGH);
  }
  if(TimeNowMicros - USonic_TimeSinceTrigLow > 20)
  {
    USonic_TimeSinceTrigLow = micros();
    digitalWrite(Uss_Trig, LOW);
  }
  
  long pulse_Time = pulseIn(Uss_Echo_FM, HIGH);
  ussNow.USonicSensor_FM = pulse_Time / 29 / 2;
  
}



/******************** Exponential Moving Average Filter ************************/
/*
const float factor = 0.25; //Weighting of the lastest value/reading
double previous_avg_FM = 0.0; //Used to get the previous value/reading of the ultrasonic sensor

int average_Filter()
{
  ussNow =  USonicSensors(); 
  
  Serial.print(ussNow.USonicSensor_FM); //Prints value of diatance before the Exponential Moving Average Filter
  Serial.print('\t');
  
  double current_avg_FM = 0.0; //Most recent value/reading will be stores in this variable
  current_avg_FM = (ussNow.USonicSensor_FM * factor) + ((previous_avg_FM) * (1 - factor)); //Value/reading is averaged
  Serial.println(current_avg_FM); //Prints the Exponentially Averaged Filter value/reading as a float
  previous_avg_FM = current_avg_FM; //The averaged value/reading is assigned to previous avergaed value/reading for the next loop
  delayMicroseconds(500);

  return current_avg_FM; //Returns the current avaerage value/reading
}*/