/*
  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

  Calibration:
    1. Flash board with calibrationMODE = true.
    2. Set to position #1 and record actual position (measuredRange[0]) and
       output position (readingRange[0]).
    3. Move to position #2 and record actual position (measuredRange[1])
       and output position (readingRange[1]).
    4. Flash board with calibrationMODE = false.

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

const int sensorPin = A0;
const unsigned long sampleInterval = 2000;
unsigned long lastSampleTime = 0;

//Calibration
float calibration_factor = 1.0;
float calibration_offset;
// 2 point calibration used for scaling factor
bool calibrationMODE = false;
float measuredRange[2] = {0, 150};
float readingRange[2] = {260.26, 421.3};


// 2nd-order Butterworth low-pass (biquad, direct form II transposed)
float butterworth_cutoffHz = 0.5;
const float butterworth_sampleHz = 1000000.0 / sampleInterval;
float butter_b0, butter_b1, butter_b2, butter_a1, butter_a2;
float butter_z1 = 0.0, butter_z2 = 0.0;

void setup() {
  Serial.begin(115200);

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

  analogReadResolution(12);
  computeButterworthCoeffs(butterworth_cutoffHz, butterworth_sampleHz);
  if (calibrationMODE == false){
    computeCalibrationParameters(measuredRange, readingRange);
  }
}

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

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

    uint32_t millivolts = analogReadMilliVolts(sensorPin);
    float displacement = convert_mV_to_mm(millivolts);
    float filtered_displacement = butterworthLowPass(displacement);
    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, 2);
    //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 volts = raw_val * (635.0 / 637.0);
  return calibration_factor * volts + calibration_offset;
}

void computeCalibrationParameters(float x[2], float y[2]){
  calibration_factor = (x[1] - x[0]) / (y[1] - y[0]);
  calibration_offset = x[0] - calibration_factor * y[0];
}

void computeButterworthCoeffs(float cutoffHz, float sampleHz) {
  // 2nd-order Butterworth LPF via bilinear transform
  float omega = tan(PI * cutoffHz / sampleHz);
  float omega2 = omega * omega;
  float sqrt2 = 1.41421356f;
  float a0 = omega2 + sqrt2 * omega + 1.0f;

  butter_b0 = omega2 / a0;
  butter_b1 = 2.0f * butter_b0;
  butter_b2 = butter_b0;
  butter_a1 = 2.0f * (omega2 - 1.0f) / a0;
  butter_a2 = (omega2 - sqrt2 * omega + 1.0f) / a0;
}

float butterworthLowPass(float newSample) {
  // Direct form II transposed biquad
  float output = butter_b0 * newSample + butter_z1;
  butter_z1 = butter_b1 * newSample - butter_a1 * output + butter_z2;
  butter_z2 = butter_b2 * newSample - butter_a2 * output;
  return output;
}