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

// Kalman filter variables
float estimatedADC_KF1 = 0; // Estimated ADC value for KF1
float errorEstimate_KF1 = 1; // Initial error estimate for KF1

// Process noise and measurement noise for KF1
float processNoise_KF1 = 1; // Process noise for KF1
float measurementNoise_KF1 = 1; // Measurement noise for KF1

// Function to apply Kalman filter to ADC readings for KF1
float kalmanFilter_KF1(float measurement) {
  // Prediction
  float prediction = estimatedADC_KF1;
  float predictionError = errorEstimate_KF1 + processNoise_KF1;

  // Update
  float kalmanGain = predictionError / (predictionError + measurementNoise_KF1);
  estimatedADC_KF1 = prediction + kalmanGain * (measurement - prediction);
  errorEstimate_KF1 = (1 - kalmanGain) * predictionError;

  return estimatedADC_KF1;
}

// Polynomial coefficients for converting voltage to turbidity level (NTU)
const float a = -2581.3;
const float b = 8731.2;
const float c = -4378.5;

// Function to convert voltage to turbidity level (NTU)
float voltageToNTU(float voltage) {
  // Apply the polynomial function
  float ntu = a * voltage * voltage + b * voltage + c;

  // Ensure NTU is within the range of 0 to 3000
  if (ntu > 3000) {
    ntu = 3000;
  } else if (ntu < 0) {
    ntu = 0;
  }

  return ntu;
}

// Initialize the LCD, set the I2C address to 0x27 for a 16x2 display
LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  Serial.begin(115200); // Initialize serial communication
  lcd.init();           // Initialize the LCD
  lcd.backlight();      // Turn on the backlight
  lcd.clear();          // Clear the display
  lcd.setCursor(0, 0);  // Set cursor to the first row
  lcd.print("Turbidity Sensor"); // Display title
}

void loop() {
  // Read the ADC value (0-4095) from pin 34
  int adcValue = analogRead(34);

  // Apply Kalman filter to ADC reading for KF1
  float filteredADC_KF1 = kalmanFilter_KF1(adcValue);

  // Convert ADC value to voltage (assuming 3.3V reference voltage)
  float voltage = filteredADC_KF1 * (3.3 / 4095.0);

  // Convert voltage to turbidity level (NTU)
  float ntu = voltageToNTU(voltage);

  // Print the results to the serial monitor
  Serial.print("ADC Value: ");
  Serial.println(adcValue);
  Serial.print("Filtered ADC Value: ");
  Serial.println(filteredADC_KF1); // Print the filtered ADC value
  Serial.print("Voltage (V): ");
  Serial.println(voltage, 4); // Print voltage with 4 decimal places
  Serial.print("Turbidity (NTU): ");
  Serial.println(ntu, 2); // Print NTU with 2 decimal places

  // Display the results on the LCD
  lcd.clear();
  lcd.setCursor(0, 0); // Set cursor to the first row
  lcd.print("Turbidity:");
  lcd.setCursor(0, 1); // Set cursor to the second row
  lcd.print(ntu, 2);   // Print NTU with 2 decimal places
  lcd.print(" NTU");

  delay(1000); // Delay for stability
}