#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

const int trigPin = 9; // Trigger pin for HC-SR04
const int echoPin = 10; // Echo pin for HC-SR04

const int numReadings = 5; // Number of readings for moving average
long readings[numReadings]; // Array to store readings
int readIndex = 0; // Index for storing new readings
long total = 0; // Sum of readings

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // Initialize OLED display
  display.display();
  delay(2000); // Wait for display to initialize
  display.clearDisplay();
  
  // Initialize readings array
  for (int i = 0; i < numReadings; i++) {
    readings[i] = 0;
  }
}

void loop() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  long duration = pulseIn(echoPin, HIGH);
  float distance = duration * 0.034 / 2; // Calculate distance in centimeters
  
  // Update readings array
  total = total - readings[readIndex];
  readings[readIndex] = distance;
  total = total + readings[readIndex];
  readIndex = (readIndex + 1) % numReadings;
  
  // Calculate moving average
  float movingAverage = total / numReadings;
  
  // Display moving average on OLED
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("Distance:");
  display.setCursor(0, 10);
  display.print(movingAverage);
  display.print(" cm");
  display.display();
  
  delay(1000); // Update every second
}