/*
  LabUI ESP32 Firmware v1.0
  ─────────────────────────────────────────────────────────────────────────
  Author: Adam Shatila
  Co-Author: Christian Kolset

  Upload this sketch to your ESP32 with a TE SP1 spring potensiometer.

  Required library: esp32 by Espressif Systems

  ─────────────────────────────────────────────────────────────────────────
 */

const int sensorPin = A0;
const unsigned long sampleInterval = 2000;
unsigned long lastSampleTime = 0;
const double lowPassFilter_alpha = 0.00675;
const unsigned long calibration_factor = 1.07517;
const long calibration_offset = -267.25;
float filtered_displacement = 0.0;

void setup() {

  Serial.begin(115200);

  while (!Serial) {
    delay(10);
  }

  analogReadResolution(12);
}

void loop() {
  unsigned long currentTime = micros();

  if (currentTime - lastSampleTime >= sampleInterval) {
    lastSampleTime = currentTime;

    uint32_t millivolts = analogReadMilliVolts(sensorPin);
    float displacement = convert_mV_to_mm(millivolts);
    filtered_displacement = lowPassFilter(displacement, filtered_displacement, lowPassFilter_alpha);
    float volts = filtered_displacement;

    // Output the voltage to the serial monitor with 3 decimal places
    //Serial.print(displacement, 3);
    //Serial.print(",   ");
    Serial.println(filtered_displacement, 3);
    //Serial.println(millivolts);
  }
}

float convert_mV_to_mm(float mVolts){
  // Reads mV from sensor and output in mm from lower limit
  float voltage = mVolts / 1000.0;
  float raw_val = voltage * (635.0 / 2.863);
  //float inverted_val = 650.0 - raw_val;
  //float volts = inverted_val * (635.0 / 637.0);
  float volts = raw_val * (635.0 / 637.0);

  return calibration_factor * volts + calibration_offset;
}

float lowPassFilter(float newSample, float prevOutput, float alpha) {
  // Simple exponential low-pass filter (EMA) formula
  return alpha * newSample + (1.0 - alpha) * prevOutput;
}