/* The 12-bit resolution of the ADC maps the input voltage from 0 to 3.3 Vdc to byte values of 0 to 4,095.
  Unfortunately, the ADC converters exhibit non-linear characteristics.
  There is a lower threshold voltage of about 0.13 volts before the ADC
  starts to register and the volts per bit ratio above about 2.5 volts deviates from the nearly
  linear ratio between these two regions.

  1: https://w4krl.com/esp32-analog-to-digital-conversion-accuracy/
  2: https://github.com/G6EJD/ESP32-ADC-Accuracy-Improvement-function/blob/master/ESP32_ADC_Read_Voltage_Accurate.ino
*/

#include <Arduino.h>

const int analogPin = 14;   // Analog input pin
const int numSamples = 10;  // Number of samples for averaging
const float V_REF = 3.14;    // Reference voltage normally 3.3v
const float ADC_MAX = 4095; // 12-bit ADC

// Linear calibration equation parameters (derived from measurements)
const float slope = 0.0008;
const float intercept = 0.1372;

long sum = 0;
int sampleCount = 0;
unsigned long lastSampleTime = 0;
const unsigned long sampleInterval = 10; // Interval between samples in milliseconds

void setup() {
  Serial.begin(115200);
  analogReadResolution(12);  // Set ADC resolution to 12 bits
}

void readAnalogFilteredNonBlocking(int pin) {
  if (millis() - lastSampleTime >= sampleInterval) {
    sum += analogRead(pin);
    sampleCount++;
    lastSampleTime = millis();
  }
}

bool isSampleComplete() {
  return sampleCount >= numSamples;
}

float getFilteredADCValue() {
  float average = (float)sum / sampleCount;
  sum = 0;
  sampleCount = 0;
  return average;
}

float calibrateADC(float adcValue) {
  // Apply the linear calibration equation
  return (slope * adcValue) + intercept;
}

float mapVoltageToDistance(float voltage) {
  // Map the voltage to distance in mm
  // Assuming the sensor has a linear response from 0V = 0mm to 3.3V = 100mm
  return (voltage / V_REF) * 100.0;
}

double polynomialCalibrateADC(double reading) {
  // Apply the polynomial calibration equation
  if(reading < 1 || reading > 4095) return 0;
  return -0.000000000000016 * pow(reading, 4) + 0.000000000118171 * pow(reading, 3) 
         - 0.000000301211691 * pow(reading, 2) + 0.001109019271794 * reading + 0.034143524634089;
}

void loop() {
  // Non-blocking ADC sampling
  readAnalogFilteredNonBlocking(analogPin);

  if (isSampleComplete()) {
    float rawADC = getFilteredADCValue();     // Get the filtered ADC value
    float voltageLinear = calibrateADC(rawADC);     // Calibrate the ADC value to voltage using linear method
    float voltagePolynomial = polynomialCalibrateADC(rawADC); // Calibrate the ADC value to voltage using polynomial method
    float distanceLinear = mapVoltageToDistance(voltageLinear); // Map the linear calibrated voltage to distance
    float distancePolynomial = mapVoltageToDistance(voltagePolynomial); // Map the polynomial calibrated voltage to distance

    // Print the results
    Serial.print("Raw ADC: ");
    Serial.print(rawADC);
    
    Serial.print(" | Voltage (Linear): ");
    Serial.print(voltageLinear, 4);  // Print linear calibrated voltage with 4 decimal places
    Serial.print("V | Distance (Linear): ");
    Serial.print(distanceLinear, 2); // Print linear calibrated distance with 2 decimal places
    
    Serial.print("mm | Voltage (Polynomial): ");
    Serial.print(voltagePolynomial, 4);  // Print polynomial calibrated voltage with 4 decimal places
    Serial.print("V | Distance (Polynomial): ");
    Serial.print(distancePolynomial, 2); // Print polynomial calibrated distance with 2 decimal places
    
    Serial.println("mm");
  }
}