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

//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);
  if (calibrationMODE == false){
    computeCalibrationParameters(measuredRange, readingRange);
  }
  computeButterworthCoeffs(butterworth_cutoffHz, butterworth_sampleHz);
}

void loop() {
  handleSerialCommands();

  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;
}

void handleSerialCommands() {
  // "C:<cutoffHz>\n" sets Butterworth cutoff frequency, e.g. "C:15.0\n"
  if (Serial.available() > 0) {
    String line = Serial.readStringUntil('\n');
    line.trim();

    if (line.startsWith("C:")) {
      float newCutoff = line.substring(2).toFloat();
      if (newCutoff > 0.0 && newCutoff < butterworth_sampleHz / 2.0) {
        butterworth_cutoffHz = newCutoff;
        computeButterworthCoeffs(butterworth_cutoffHz, butterworth_sampleHz);
        butter_z1 = 0.0;
        butter_z2 = 0.0;
      }
    }
  }
}