#define COUNTMAX 24

float myArray[COUNTMAX];  // Correctly initialize array
bool detected = false;    // Boolean to track if a blade was already detected
int count = 0;            // Count for blades detected
int arrayIndex = 0;       // Index to store velocities in myArray
float currentSensorValue = 500.0;  // Current sensor value
const float alpha = 0.9;
int ledPin = 13;
long deltatime = 0;
long time = 0;
const float bladeDistance = 0.08755;  // Distance between two blades (adjust this value as needed)

#include <LiquidCrystal.h>  // Include the library code

LiquidCrystal lcd(4, 6, 10, 11, 12, 13);  // Initialize the library with the numbers of the interface pins
float velocity = 0;         // Averaged velocity to be displayed
float velocity2blades = 0;
float velocitywhole = 0;   // Accumulator for velocities
float zero = 0;

void setup() {
  Serial.begin(115200);    // Initialize serial communication at 115200 bits per second
  pinMode(ledPin, OUTPUT);
  lcd.begin(16, 2);        // Set up the LCD's number of columns and rows
  lcd.clear();             // Clear the LCD screen
}

void loop() {
  // Read the current value from the sensor (Hall sensor) with exponential smoothing:
  currentSensorValue = alpha * currentSensorValue + (1 - alpha) * (float) analogRead(A2);
  
  // Detect a transition from >= 498 to < 480 (blade passing)
  if (currentSensorValue < 480.0 && !detected) {  
    detected = true;  // Blade detected, set detected to true
    count++;          // Increment blade count
    digitalWrite(ledPin, detected);

    // Capture the time difference between detections (in microseconds)
    long currentTime = micros();
    deltatime = currentTime - time;
    time = currentTime;  // Update time for next detection

    // Ensure deltatime is valid before calculating velocity
    if (deltatime > 0) {
      // Calculate velocity for 2 blades based on deltatime (Convert micros to seconds)
      velocity2blades = (bladeDistance * 1) / (deltatime / 1000000.0);  
      velocitywhole += velocity2blades;  // Add to the cumulative velocity

      // Store the velocity in the array
      myArray[arrayIndex] = velocity2blades;
      arrayIndex = (arrayIndex + 1) % COUNTMAX;  // Circular array to prevent overflow

      // Debugging print to check deltatime and velocity2blades
      Serial.print("deltatime: ");
      Serial.println(deltatime);
      Serial.print("velocity2blades: ");
      Serial.println(velocity2blades);
    } else {
      // In case deltatime is invalid
      Serial.println("Invalid deltatime detected.");
    }
  }

  // Reset detection when the sensor value goes back above 480
  if (currentSensorValue >= 480.0 && detected) {
    detected = false;  // Reset detection flag for the next blade
    digitalWrite(ledPin, detected);
  }

  // When enough blades have been detected, calculate and display the average velocity
  if (count >= COUNTMAX) {
    // Reset the count for the next cycle
    count = 0;

    // Variables for storing min, max, and sum of velocities
    float valmin = 1024.0;
    float valmax = 0.0;
    int indmin = 0;
    int indmax = 0;
    float total = 0.0;

    // Find the indices of min and max values in myArray
    for (int i = 0; i < COUNTMAX; i++) {
      if (myArray[i] < valmin) {
        indmin = i;
        valmin = myArray[i];
      }
      if (myArray[i] > valmax) {
        indmax = i;
        valmax = myArray[i];
      }
    }

    // Calculate total excluding min and max values
    for (int i = 0; i < COUNTMAX; i++) {
      if (i != indmin && i != indmax) {
        total += myArray[i];
      }
    }

    // Calculate the average velocity, excluding the min and max values
    float VelocityAverage = total / (COUNTMAX - 2);

    // Display the averaged velocity on the LCD
    lcd.clear();  // Clear the LCD before displaying new data
    lcd.setCursor(0, 0);  
    lcd.print("The velocity is:");

    lcd.setCursor(0, 1);  
    lcd.print(VelocityAverage, 2);  // Print velocity with 2 decimal places
    lcd.print(" m/s");

    // Optionally print to serial for debugging
    Serial.print("Average velocity: ");
    Serial.print(VelocityAverage, 2);
    Serial.println(" m/s");

    // Reset variables for next cycle
    velocitywhole = 0;
    deltatime = 0;
  }

}