#include <Arduino.h>
#include <driver/adc.h>

const int hallEffectPin = 34;
const int numReadings = 10; // Number of readings to use for filtering
int sensorReadings[numReadings]; // Buffer for sensor readings
int readingIndex = 0; // Index to keep track of the current reading
int medianFilteredValue = 0; // Filtered value using median filter
float emaFilteredValue = 0; // Filtered value using exponential moving average
float emaAlpha = 0.2; // Smoothing factor for EMA (adjust as needed)

unsigned long startTime = 0; // Timestamp for the start of a revolution
bool peakDetected = false;   // Flag to track peak detection
const int threshold = 1000; // Adjust the threshold as needed
const unsigned long minRevolutionTime = 1000000; // Minimum time for one revolution in microseconds (adjust as needed)

void setup() {
  Serial.begin(115200);
  adc1_config_width(ADC_WIDTH_BIT_12);
  adc1_config_channel_atten(ADC1_CHANNEL_6, ADC_ATTEN_DB_11);

  // Initialize the sensor readings buffer
  for (int i = 0; i < numReadings; i++) {
    sensorReadings[i] = 0;
  }
}

void loop() {
  // Read the raw sensor value
  uint32_t hallEffectADC = analogReadMilliVolts(34); // Use the correct ADC channel

  // Update the sensor readings buffer
  sensorReadings[readingIndex] = hallEffectADC;
  readingIndex = (readingIndex + 1) % numReadings;

  // Calculate the median filtered value
  medianFilteredValue = calculateMedianFilter();

  // Calculate the exponential moving average filtered value
  emaFilteredValue = calculateEMAFilter(hallEffectADC);

  // Check for peak detection (adjust threshold as needed)
  if (hallEffectADC > (medianFilteredValue + threshold)) {
    if (!peakDetected) {
      peakDetected = true;
      startTime = micros(); // Record the start time of a revolution
    }
  } else {
    peakDetected = false;
  }

  // Check if a complete revolution has occurred (RPM calculation)
  unsigned long currentTime = micros();
  if (peakDetected && (currentTime - startTime) >= minRevolutionTime) {
    // Calculate RPM
    unsigned long timeForOneRevolution = currentTime - startTime;
    float rpm = 60000000 / timeForOneRevolution;

    // Print or store the RPM value
    Serial.print("RPM: ");
    Serial.println(rpm, 2); // Display RPM with 2 decimal places
  }

  delay(50); // Adjust the delay as needed
}

int calculateMedianFilter() {
  // Sort the sensor readings buffer
  int sortedReadings[numReadings];
  for (int i = 0; i < numReadings; i++) {
    sortedReadings[i] = sensorReadings[i];
  }
  for (int i = 0; i < numReadings - 1; i++) {
    for (int j = i + 1; j < numReadings; j++) {
      if (sortedReadings[i] > sortedReadings[j]) {
        int temp = sortedReadings[i];
        sortedReadings[i] = sortedReadings[j];
        sortedReadings[j] = temp;
      }
    }
  }

  // Return the median value
  return sortedReadings[numReadings / 2];
}

float calculateEMAFilter(uint32_t currentValue) {
  static float emaValue = 0;
  emaValue = (1 - emaAlpha) * emaValue + emaAlpha * currentValue;
  return emaValue;
}