#include <Wire.h>
 #include <LiquidCrystal_I2C.h>

// Define ultrasonic sensor pins
#define trigPin 3
#define echoPin 2

// Define LCD parameters
#define LCD_COLS 20
#define LCD_ROWS 4

// Create LCD object
 LiquidCrystal_I2C lcd(0x27, LCD_COLS, LCD_ROWS);

// Buffer to store last three higher distances
const int bufferLength = 4;
float distanceBuffer[bufferLength] = {0};

float distance1 = 0.0;
float distance2 = 0.0;
float distance3 = 0.0;

void setup() {
  // Initialize serial communication
  Serial.begin(9600);

  // Initialize LCD
   lcd.init();
   lcd.backlight();

  // Initialize ultrasonic sensor pins
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}

void loop() {
  // Measure distance
  float distance = measureDistance();
  distanceBuffer[3] = distance;
  Serial.print("Current ");
  Serial.print(distance);
  if (distance>25){
    return;
  }


  if (distance > distanceBuffer[0]) {
    distance1 = distance;
    distanceBuffer[2] = distanceBuffer[1];
    distanceBuffer[1] = distanceBuffer[0];
    distanceBuffer[0] = distance;
  }
  else if (distance > distanceBuffer[1]) {
    distance2 = distance;
    distanceBuffer[2] = distanceBuffer[1];
    distanceBuffer[1] = distance;
  }
  else if (distance > distanceBuffer[2]) {
    distance3 = distance;
    distanceBuffer[2] = distance;
  }
  //  distanceBuffer[] = {distance1,distance2,distance3};
  // Update buffer with the latest distance
  // updateBuffer(distance);

  // Sort buffer in descending order
  //  sortBuffer();

  // Display the sorted distances on LCD
  displayDistances();

  delay(1000); // Delay for 1 second before next measurement
}

float measureDistance() {
  // Send pulse to trigger pin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Measure the duration of the echo pin
  float duration = pulseIn(echoPin, HIGH);

  // Convert the duration to distance in centimeters
  float distance = (duration * 0.034 / 2) + 5;

  return distance;
}

void updateBuffer(float newDistance) {
  // Shift elements in the buffer to make space for new distance
  for (int i = 0; i < bufferLength; i++) {
    if (newDistance > distanceBuffer[i]); {
      distanceBuffer[i] = newDistance;
    }
  }
  // Update the first element with the new distance
  // distanceBuffer[0] = newDistance;
}

void sortBuffer() {
  int temp, min, i , j;
  for (i = 0; i < bufferLength; i++) {
    min = i;
    for (j = i + 1; j < bufferLength; j++)
      if (distanceBuffer[j] < distanceBuffer[min])
        min = j;
    temp = distanceBuffer[i];
    distanceBuffer[i] = distanceBuffer[min];
    distanceBuffer[min] = temp;
  }
}

void displayDistances() {
  // Clear the LCD display
   lcd.clear();

  // Display the sorted distances on LCD
  for (int i = 0; i < bufferLength - 1; i++) {
    lcd.setCursor(0, i);
    lcd.print("Distance ");
    lcd.print(i + 1);
    lcd.print(": ");
    lcd.print(distanceBuffer[i]);
    lcd.print(" cm");
    Serial.print("Distance ");
    Serial.print(i + 1);
    Serial.print(": ");
    Serial.print(distanceBuffer[i]);
    Serial.println(" cm");
  }
  
  lcd.setCursor(0, 3);
  lcd.print("currentDis ");
  lcd.print(": ");
  lcd.print(distanceBuffer[3]);
  lcd.print(" cm");

}