/*
  LabUI ESP32 Firmware v1.1
  ─────────────────────────────────────────────────────────────────────────
  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 procedure:
    Send  calibrate  and follow the prompts (recommended — pauses the data
    stream and walks through both points). Or drive it manually:
      1. Move sensor to known position #1, send  CAL:0:<actual_position>
         Board captures its current raw reading and pairs it with that position.
      2. Move sensor to known position #2, send  CAL:1:<actual_position>
         Board recomputes calibration_factor / calibration_offset and saves
         to EEPROM.
    Send  FILT:<cutoffHz>  to change the Butterworth low-pass cutoff
    (also saved to EEPROM). Send  config  at any time to print the current
    calibration and filter configuration.

  Serial commands (newline-terminated):
    calibrate       Guided calibration wizard (recommended)
    CAL:0:<pos>     Capture current raw reading as calibration point 0
    CAL:1:<pos>     Capture current raw reading as calibration point 1
    FILT:<hz>       Set Butterworth cutoff frequency
    config          Print current calibration + filter configuration
    help            Print this command list

  Replies:
    ACK:<command>\n   Command executed
    ERR:<message>\n   Command failed / not recognized

  ─────────────────────────────────────────────────────────────────────────
 */
#include <EEPROM.h>

// EEPROM-persisted configuration
#define EEPROM_SIZE 64
#define EEPROM_MAGIC 0xC1

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

// Calibration (mirrors cfg, kept as separate globals for fast access in loop())
float calibration_factor = 1.0;
float calibration_offset = 0.0;

// 2nd-order Butterworth low-pass (biquad, direct form II transposed)
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;

String serialLine;
bool streamingEnabled = true;
bool calibPointSet[2] = {false, false};

struct StoredConfig {
  uint8_t magic;
  float calibrationPosition[2];
  float calibrationRange[2];
  float butterworth_cutoffHz;
};

StoredConfig cfg = {
  EEPROM_MAGIC,
  {0, 150},
  {260.26, 421.3},
  0.5
};

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

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

  analogReadResolution(12);

  EEPROM.begin(EEPROM_SIZE);
  loadConfig();
  computeButterworthCoeffs(cfg.butterworth_cutoffHz, butterworth_sampleHz);
  computeCalibrationParameters(cfg.calibrationPosition, cfg.calibrationRange);
}

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

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

    float filtered_displacement = butterworthLowPass(readCalibratedPosition());
    float raw_displacement = readCalibratedPosition();
    if (streamingEnabled) {
      Serial.println(filtered_displacement, 2);
      //Serial.print(",  ");
      //Serial.println(raw_displacement, 2);
    }
  }

  readSerialCommands();
}

void readSerialCommands() {
  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\n') {
      serialLine.trim();
      if (serialLine.length() > 0) {
        handleCommand(serialLine);
      }
      serialLine = "";
    } else if (c != '\r') {
      serialLine += c;
    }
  }
}

void handleCommand(String line) {
  if (line.startsWith("CAL:0:") || line.startsWith("CAL:1:")) {
    int idx = line.charAt(4) - '0';
    float actualPos = line.substring(6).toFloat();
    float rawReading = readRawPosition();
    int otherIdx = 1 - idx;

    if (calibPointSet[otherIdx] && rawReading == cfg.calibrationRange[otherIdx]) {
      Serial.println("ERR:Calibration points cannot have equal raw reading");
      return;
    }

    cfg.calibrationPosition[idx] = actualPos;
    cfg.calibrationRange[idx] = rawReading;
    calibPointSet[idx] = true;

    if (!calibPointSet[otherIdx]) {
      Serial.println("ACK:" + line + " (waiting on CAL:" + String(otherIdx) + ")");
      return;
    }

    computeCalibrationParameters(cfg.calibrationPosition, cfg.calibrationRange);
    saveConfig();

    Serial.println("ACK:" + line);
  } else if (line.startsWith("FILT:")) {
    float hz = line.substring(5).toFloat();
    if (hz <= 0) {
      Serial.println("ERR:Invalid cutoff frequency");
      return;
    }
    cfg.butterworth_cutoffHz = hz;
    computeButterworthCoeffs(cfg.butterworth_cutoffHz, butterworth_sampleHz);
    saveConfig();

    Serial.println("ACK:" + line);
  } else if (line == "config") {
    printConfig();
  } else if (line == "help") {
    printHelp();
  } else if (line == "calibrate") {
    calibrationWizard();
  } else {
    Serial.println("ERR:Unknown command");
  }
}

void loadConfig() {
  EEPROM.get(0, cfg);
  if (cfg.magic != EEPROM_MAGIC) {
    cfg = { EEPROM_MAGIC, {0, 150}, {260.26, 421.3}, 0.5 };
    saveConfig();
  }
}

void saveConfig() {
  EEPROM.put(0, cfg);
  EEPROM.commit();
}

// Fixed sensor scaling, pre calibration_factor/offset
float readRawPosition() {
  uint32_t millivolts = analogReadMilliVolts(sensorPin);
  float voltage = millivolts / 1000.0;
  float raw_val = voltage * (635.0 / 2.863);
  return raw_val * (635.0 / 637.0);
}

float readCalibratedPosition() {
  return calibration_factor * readRawPosition() + 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 printConfig() {
  Serial.println("Calibration:");
  Serial.print("\tPosition:\t");
  Serial.print(cfg.calibrationPosition[0], 3);
  Serial.print(",\t");
  Serial.println(cfg.calibrationPosition[1], 3);
  Serial.print("\tRaw values:\t");
  Serial.print(cfg.calibrationRange[0], 3);
  Serial.print(",\t");
  Serial.println(cfg.calibrationRange[1], 3);
  Serial.print("\tFactor:\t");
  Serial.println(calibration_factor, 6);
  Serial.print("\tOffset:\t");
  Serial.println(calibration_offset, 6);
  Serial.print("Butterworth cutoff (Hz):\t");
  Serial.println(cfg.butterworth_cutoffHz, 3);
  interruptibleDelay(10000);
}

void printHelp() {
  Serial.println("Commands:");
  Serial.println("\tcalibrate     Guided calibration wizard (recommended)");
  Serial.println("\tCAL:0:<pos>   Capture current raw reading as calibration point 0");
  Serial.println("\tCAL:1:<pos>   Capture current raw reading as calibration point 1");
  Serial.println("\tFILT:<hz>     Set Butterworth cutoff frequency");
  Serial.println("\tconfig        Print current calibration + filter configuration");
  Serial.println("\thelp          Print this command list");
  interruptibleDelay(10000);
}

// Waits up to ms, returns early (without consuming input) if a command starts arriving.
void interruptibleDelay(unsigned long ms) {
  unsigned long start = millis();
  while (millis() - start < ms) {
    if (Serial.available()) {
      return;
    }
  }
}

// Blocks until a newline-terminated line arrives; used only inside calibrationWizard(),
// where halting loop() to walk the user through steps is the point.
String blockingReadLine() {
  String buf = "";
  while (true) {
    if (Serial.available()) {
      char c = Serial.read();
      if (c == '\n') {
        buf.trim();
        return buf;
      } else if (c != '\r') {
        buf += c;
      }
    }
  }
}

void calibrationWizard() {
  bool wasStreaming = streamingEnabled;
  streamingEnabled = false;

  Serial.println("=== Calibration wizard (type 'cancel' anytime to abort) ===");

  float pos[2];
  float raw[2];

  for (int i = 0; i < 2; i++) {
    Serial.println("Move sensor to known position #" + String(i + 1) + ", enter actual position, press Enter:");
    String input = blockingReadLine();

    if (input.equalsIgnoreCase("cancel")) {
      Serial.println("Calibration cancelled. Previous calibration unchanged.");
      streamingEnabled = wasStreaming;
      return;
    }

    pos[i] = input.toFloat();
    raw[i] = readRawPosition();

    if (i == 1 && raw[1] == raw[0]) {
      Serial.println("ERR:Calibration points cannot have equal raw reading. Wizard aborted, previous calibration unchanged.");
      streamingEnabled = wasStreaming;
      return;
    }

    Serial.println("Point " + String(i) + " captured: position=" + String(pos[i], 3) + " raw=" + String(raw[i], 3));
  }

  cfg.calibrationPosition[0] = pos[0];
  cfg.calibrationPosition[1] = pos[1];
  cfg.calibrationRange[0] = raw[0];
  cfg.calibrationRange[1] = raw[1];
  calibPointSet[0] = true;
  calibPointSet[1] = true;

  computeCalibrationParameters(cfg.calibrationPosition, cfg.calibrationRange);
  saveConfig();

  Serial.println("Calibration saved.");
  printConfig();

  streamingEnabled = wasStreaming;
}