/*
 * ============================================================
 *  Motor-Propeller Characterisation Test Rig — Firmware v3
 *  Board  : Arduino Mega 2560 R3
 *  Author : Japs / Verocore
 *
 *  Pin map (verified against japesh.kicad_sch):
 *
 *  ─── Current sensors (ACS770ECB-220U — unidirectional) ───
 *    U1  VIOUT → A1   (Motor 1 / ESC on J6)
 *    U2  VIOUT → A2   (Motor 2 / ESC on J8)
 *    Quiescent V at 0 A ≈ 0.6 V  (NOT 2.5 V like ACS758)
 *    Sensitivity ≈ 10 mV/A (200–220 A range)
 *    IMPORTANT: connect main battery BEFORE powering Arduino
 *    or zero will be wrong → send 'zero1'/'zero2' to re-zero.
 *
 *  ─── Voltage dividers ────────────────────────────────────
 *    U4  Vout → A11  (Motor 1)   R1=30kΩ, R2=7.5kΩ  ×5
 *    U3  Vout → A12  (Motor 2)
 *
 *  ─── ESC PWM outputs ─────────────────────────────────────
 *    J6  → D12  Motor 1  PRIMARY — V/I/RPM monitored
 *    J8  → D15  Motor 2  PRIMARY — V/I monitored
 *    J5  → D13  SPARE (not soldered)
 *    J7  → D14  SPARE (not soldered)
 *
 *  ─── Load cells (HX711) ──────────────────────────────────
 *    LC1(U5) DT=D47, SCK=D53   → torque arm cell A
 *    LC2(U6) DT=D27, SCK=D25   → torque arm cell B
 *    LC3(U7) DT=D51, SCK=D49   → thrust rail cell A
 *    LC4(U8) DT=D31, SCK=D29   → thrust rail cell B
 *    Torque  = (LC1 − LC2) × g × arm/2     [N·mm]
 *    Thrust  = LC3 + LC4                    [g]
 *
 *  ─── RPM sensors (HW-006 line tracker, polled) ───────────
 *    RPM_MES_1 → D8  (J26)
 *    RPM_MES_2 → D9  (J27)
 *    D8/D9 are on PORTH — NO interrupt support on Mega 2560.
 *    Falling-edge detection is done by polling in loop().
 *    One reflective strip per propeller → marksPerRev = 1.
 *
 *  ─── Back-EMF (RC filtered via R7/R8 + C7/C8) ───────────
 *    BEMF_1 → A4   BEMF_2 → A3
 *    ⚠  DO NOT connect raw motor phase wires — max 5 V on ADC.
 *    Requires external 100kΩ/22kΩ voltage divider on phase wire.
 *
 *  ─── Temperature (100 kΩ NTC, B=3950) ───────────────────
 *    Temp1→A7  Temp2→A8  Temp3→A9  Temp4→A10
 *    PCB already has 10 kΩ pull-up to 5 V (R3–R6).
 *    Just plug NTC into J1–J4.  No external resistor needed.
 *    NTC is non-polarised — either pin to either terminal.
 *
 *  ─── Velocity (FS7 analog) ───────────────────────────────
 *    Vel1→A6   Vel2→A5
 *
 *  ─── IMU BNO055 (I2C, addr 0x28) ────────────────────────
 *    SDA=D20   SCL=D21  (Mega hardware I2C)
 *    Mode: AMG (raw accel + mag + gyro, no fusion)
 *    Vibration index = ||accel|| − 9.81  [m/s²]
 *
 * ============================================================
 *  Libraries (install via Library Manager):
 *    HX711_ADC        by Olav Kallhovd
 *    Adafruit BNO055
 *    Adafruit Unified Sensor
 *
 *  Serial: 115200 baud
 *    → CSV header on boot, then one CSV line per sample
 *    → Comment lines start with '#'
 *    ← Commands: see 'help'
 * ============================================================
 */

#include <Arduino.h>
#include <HX711_ADC.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>

// ─── IMU data struct ─────────────────────────────────────────
struct IMUData {
  float ax, ay, az;   // m/s²  (raw accelerometer, gravity included)
  float gx, gy, gz;   // rad/s (raw gyroscope)
};

unsigned long startTimeMs  = 0;
unsigned long lastSampleMs = 0;

// ════════════════════════════════════════════════════════════
//  USER-TUNABLE CONSTANTS
// ════════════════════════════════════════════════════════════

// Physical distance (mm) between the two torque-arm load-cell
// contact points on the motor mount.  MEASURE YOUR BUILD.
const float TORQUE_ARM_MM = 100.0f;

// ACS759LCB-100B bidirectional ±100 A current sensor.
//   Quiescent Vout at 0 A = VCC/2 = 2.5 V
//   Sensitivity = 20 mV/A → 0.020 V/A
//   Output range: 0.5 V (−100 A) … 4.5 V (+100 A)
const float SENSITIVITY   = 0.020f;  // V/A  (ACS759LCB-100B)

// NTC thermistor: 100 kΩ @ 25 °C, B = 3950
// (robu.in MF52 / generic 100K NTC — verify B with your datasheet)
const float NTC_R_NOMINAL = 100000.0f;
const float NTC_B         =   3950.0f;
const float NTC_R_PULLUP  =  10000.0f;   // 10 kΩ pull-up already on PCB
const float NTC_T_NOM_K   =    298.15f;  // 25 °C in Kelvin

// FS7 airspeed sensor linear mapping
const float FS7_V_ZERO = 0.5f;    // V at 0 m/s
const float FS7_V_FULL = 4.5f;    // V at FS7_V_MAX m/s
const float FS7_V_MAX  = 30.0f;   // m/s at full scale

// Back-EMF zero-cross threshold — tune with 'bemfth_X.X'
float BEMF_THRESHOLD_V = 2.5f;

// Voltage divider R1=30kΩ, R2=7.5kΩ → ×5
const float R1 = 30000.0f;
const float R2 =  7500.0f;

// Sample rate — 50 ms = 20 Hz
const unsigned long SAMPLE_INTERVAL_MS = 50;

// ════════════════════════════════════════════════════════════
//  FIXED HARDWARE CONSTANTS
// ════════════════════════════════════════════════════════════
const float VCC      = 5.0f;
const float DIV_GAIN = (R1 + R2) / R2;   // = 5.0

// ════════════════════════════════════════════════════════════
//  PIN ASSIGNMENTS  (from schematic japesh.kicad_sch)
// ════════════════════════════════════════════════════════════
const uint8_t LC_DOUT[4]      = {47, 27, 51, 31};
const uint8_t LC_SCK[4]       = {53, 25, 49, 29};  // LC4 SCK = D29

// Motor 1 = J6 = D12,  Motor 2 = J8 = D15
// J5(D13) and J7(D14) are spare (not soldered)
const uint8_t ESC_PIN[2]      = {12, 15};           // only 2 active ESCs

const uint8_t RPM_PIN[2]      = {8, 9};             // J26=D8, J27=D9
const uint8_t PIN_CURRENT[2]  = {A1, A2};
const uint8_t PIN_VOLTAGE[2]  = {A11, A12};
const uint8_t PIN_BEMF[2]     = {A4, A3};
const uint8_t PIN_TEMP[4]     = {A7, A8, A9, A10};
const uint8_t PIN_VELOCITY[2] = {A6, A5};

// ════════════════════════════════════════════════════════════
//  LOAD CELLS
// ════════════════════════════════════════════════════════════
HX711_ADC LoadCell[4] = {
  HX711_ADC(LC_DOUT[0], LC_SCK[0]),
  HX711_ADC(LC_DOUT[1], LC_SCK[1]),
  HX711_ADC(LC_DOUT[2], LC_SCK[2]),
  HX711_ADC(LC_DOUT[3], LC_SCK[3])
};
// LC1=torque-A, LC2=torque-B, LC3=thrust-A, LC4=thrust-B
// Run interactive calibration ('c1'…'c4') to update.
float calValues[4] = {207.0f, 210.0f, 121.5f, 115.0f};

// ════════════════════════════════════════════════════════════
//  ESC / SERVO  (only 2 active channels)
// ════════════════════════════════════════════════════════════
Servo esc[2];
int     throttle_us[2]   = {1000, 1000};
bool    motor_running[2] = {false, false};
int     min_throttle_us  = 1001;

// ESC arm gate — must ARM from dashboard before throttle works
bool    escsArmed = false;

// ════════════════════════════════════════════════════════════
//  RPM — HW-006 LINE TRACKER (polled, no interrupts)
//
//  D8 and D9 are on PORTH of the Mega 2560.
//  PORTH has NO external interrupt (INT) and NO pin-change
//  interrupt (PCINT) support on the Mega 2560.
//  attachInterrupt() silently does nothing on these pins.
//
//  Solution: fast falling-edge detection by polling in loop().
//  One reflective strip = 1 mark/rev.
//  marksPerRev[ch] is set to 1 by default and adjustable via
//  'marksX_N' command if you add more strips later.
// ════════════════════════════════════════════════════════════
uint8_t marksPerRev[2]             = {1, 1};
const unsigned long RPM_TIMEOUT_US = 500000UL;   // 0 RPM after 0.5 s

unsigned long lastPulse[2]     = {0, 0};
unsigned long pulseInterval[2] = {0, 0};
bool          lastRPMHigh[2]   = {true, true};

void pollOpticalRPM() {
  for (uint8_t ch = 0; ch < 2; ch++) {
    bool high = (digitalRead(RPM_PIN[ch]) == HIGH);
    if (!high && lastRPMHigh[ch]) {          // falling edge detected
      unsigned long t  = micros();
      unsigned long dt = t - lastPulse[ch];
      if (dt > 500UL) {                      // debounce: ignore < 500 µs (~120k RPM max)
        pulseInterval[ch] = dt;
        lastPulse[ch]     = t;
      }
    }
    lastRPMHigh[ch] = high;
  }
}

float getOpticalRPM(uint8_t ch) {
  unsigned long interval = pulseInterval[ch];
  unsigned long last     = lastPulse[ch];
  if (interval == 0) return 0.0f;
  if ((micros() - last) > RPM_TIMEOUT_US) return 0.0f;
  return 60.0e6f / ((float)interval * (float)marksPerRev[ch]);
}

// ════════════════════════════════════════════════════════════
//  RPM — BACK-EMF (analog zero-cross polling, backup method)
//
//  ⚠  Requires external resistor divider on motor phase wire.
//  DO NOT connect raw phase voltage directly to A3/A4.
//
//  Formula:
//    interval = time between two consecutive zero-crossings
//    elec_freq = 1 / (2 × interval_s)   [each crossing = half cycle]
//    mech_rpm  = elec_freq × 60 / pole_pairs
// ════════════════════════════════════════════════════════════
struct BEMFState {
  bool     lastAbove   = false;
  uint32_t lastCrossUs = 0;
  float    smoothedUs  = 0.0f;   // EMA-filtered half-cycle duration
  uint8_t  polePairs   = 7;      // adjust via 'polpairsX_N'
};
BEMFState bemf[2];

// updateBEMF — detect every rising/falling edge on the BEMF pin.
// Each edge is one half-cycle of the electrical frequency.
// An exponential moving average (α = 0.25) smooths interval noise,
// which eliminates the ±1000 RPM jitter from single-sample variation.
void updateBEMF(uint8_t ch) {
  int   raw   = analogRead(PIN_BEMF[ch]);
  float V     = raw * (VCC / 1023.0f);
  bool  above = (V > BEMF_THRESHOLD_V);

  if (above != bemf[ch].lastAbove) {
    uint32_t now = micros();
    uint32_t dt  = now - bemf[ch].lastCrossUs;
    // Accept edges in plausible range: 400 µs (>75k RPM) … 500 ms (120 RPM)
    if (dt > 400UL && dt < 500000UL) {
      if (bemf[ch].smoothedUs == 0.0f)
        bemf[ch].smoothedUs = (float)dt;          // seed on first edge
      else
        bemf[ch].smoothedUs = 0.75f * bemf[ch].smoothedUs + 0.25f * (float)dt;
      bemf[ch].lastCrossUs = now;
    }
    bemf[ch].lastAbove = above;
  }
}

float getBEMF_RPM(uint8_t ch) {
  if (bemf[ch].smoothedUs == 0.0f) return 0.0f;
  if ((micros() - bemf[ch].lastCrossUs) > RPM_TIMEOUT_US) {
    bemf[ch].smoothedUs = 0.0f;   // reset so it doesn't latch old speed
    return 0.0f;
  }
  // half-cycle → electrical Hz = 1 / (2 × interval_s) = 500000 / interval_µs
  float elec_Hz = 500000.0f / bemf[ch].smoothedUs;
  return elec_Hz * 60.0f / (float)bemf[ch].polePairs;
}

// RPM mode per channel: 0 = optical (default), 1 = BEMF
uint8_t rpmMode[2] = {0, 0};

float getRPM(uint8_t ch) {
  return (rpmMode[ch] == 0) ? getOpticalRPM(ch) : getBEMF_RPM(ch);
}

// ════════════════════════════════════════════════════════════
//  ADC HELPERS  (median-of-5 averaged over N windows)
//  With prescaler /16: each analogRead ≈ 26 µs
// ════════════════════════════════════════════════════════════
static void sort5(int *a) {
  #define SW(i,j) if(a[i]>a[j]){int t=a[i];a[i]=a[j];a[j]=t;}
  SW(0,1)SW(2,3)SW(0,2)SW(1,3)SW(1,2)SW(3,4)SW(2,3)SW(1,2)SW(0,1)SW(2,3)SW(1,2)
  #undef SW
}

int readADC(uint8_t pin, uint8_t windows = 8, uint8_t wsize = 5) {
  int  buf[5];
  long sum = 0;
  for (uint8_t w = 0; w < windows; w++) {
    for (uint8_t i = 0; i < wsize; i++) buf[i] = analogRead(pin);
    sort5(buf);
    sum += buf[wsize / 2];
  }
  return (int)(sum / windows);
}

// ════════════════════════════════════════════════════════════
//  CURRENT SENSING  (ACS759LCB-100B bidirectional ±100 A)
//
//  Quiescent Vout at 0 A = VCC/2 = 2.5 V
//  Sensitivity = 20 mV/A → I = (Vout − 2.5) / 0.020
//  Output: 0.5 V at −100 A, 4.5 V at +100 A
//
//  IMPORTANT: Calibrate zero BEFORE powering motors.
//  If the reading looks wrong, send 'zero1'/'zero2' with motors off.
// ════════════════════════════════════════════════════════════
float zeroCurrentV[2]     = {2.5f, 2.5f};   // VCC/2 for bidirectional
float currentOffset[2]    = {0.0f, 0.0f};
float filteredCurrent[2]  = {0.0f, 0.0f};
bool  firstCurrentRead[2] = {true, true};
const float CURRENT_ALPHA = 0.15f;

float readCurrent(uint8_t ch) {
  int   raw  = readADC(PIN_CURRENT[ch]);
  float Vpin = raw * (VCC / 1023.0f);

  // Slow zero-drift correction while motor is off
  if (!motor_running[ch]) {
    currentOffset[ch] = currentOffset[ch] * 0.99f
                      + (Vpin - zeroCurrentV[ch]) * 0.01f;
  }

  float A = (Vpin - zeroCurrentV[ch] - currentOffset[ch]) / SENSITIVITY;
  A = constrain(A, -100.0f, 100.0f);  // ACS759LCB-100B ±100 A

  if (firstCurrentRead[ch]) {
    filteredCurrent[ch]  = A;
    firstCurrentRead[ch] = false;
    return A;
  }
  filteredCurrent[ch] += CURRENT_ALPHA * (A - filteredCurrent[ch]);
  return filteredCurrent[ch];
}

void calibrateZeroCurrent(uint8_t ch) {
  Serial.print(F("# Zeroing current sensor ")); Serial.println(ch + 1);
  long sum = 0;
  for (int i = 0; i < 500; i++) {
    sum += analogRead(PIN_CURRENT[ch]);
    delay(2);
  }
  float measured = (sum / 500.0f) * (VCC / 1023.0f);

  // ACS759LCB-100B zero should be near 2.5 V (VCC/2).
  // Accept 1.5–3.5 V; outside that range something is wrong.
  if (measured < 1.5f || measured > 3.5f) {
    Serial.print(F("# WARNING: Vzero = "));
    Serial.print(measured, 4);
    Serial.println(F(" V — expected ~2.5 V for ACS759 at 0 A!"));
    Serial.println(F("#   Check VCC, GND, and VIOUT wiring."));
    Serial.println(F("#   With motors off send 'zero1'/'zero2' to retry."));
    return;
  }
  zeroCurrentV[ch]     = measured;
  currentOffset[ch]    = 0.0f;
  firstCurrentRead[ch] = true;
  Serial.print(F("#   Vzero = ")); Serial.print(zeroCurrentV[ch], 4);
  Serial.println(F(" V  ✓"));
}

// ════════════════════════════════════════════════════════════
//  VOLTAGE
// ════════════════════════════════════════════════════════════
float readVoltage(uint8_t ch) {
  int raw = readADC(PIN_VOLTAGE[ch]);
  return (raw * VCC / 1023.0f) * DIV_GAIN;
}

// ════════════════════════════════════════════════════════════
//  TEMPERATURE  (100 kΩ NTC, B=3950, Steinhart-Hart)
//
//  Wiring based on standalone test code:
//  NTC is connected to 5V, 10 kΩ resistor is connected to GND
//  Therefore: R_ntc = 10k * (1023 / raw - 1)
// ════════════════════════════════════════════════════════════
float readTemperature(uint8_t pin) {
  int raw = analogRead(pin);
  if (raw <= 0 || raw >= 1023) return -273.15f;   // fault sentinel

  float ratio = raw / 1023.0f;
  float R_ntc = NTC_R_PULLUP * (1.0f / ratio - 1.0f);
  
  if (R_ntc <= 0.0f) return -273.15f;

  // Steinhart-Hart B-form: 1/T = 1/T0 + (1/B) × ln(R/R0)
  float tempK = 1.0f / (1.0f / NTC_T_NOM_K + logf(R_ntc / NTC_R_NOMINAL) / NTC_B);
  return tempK - 273.15f;
}

// ════════════════════════════════════════════════════════════
//  VELOCITY  (FS7 — linear V→m/s)
// ════════════════════════════════════════════════════════════
float readVelocity(uint8_t pin) {
  int   raw   = analogRead(pin);
  float V     = raw * (VCC / 1023.0f);
  float slope = FS7_V_MAX / (FS7_V_FULL - FS7_V_ZERO);
  return max(0.0f, (V - FS7_V_ZERO) * slope);
}

// ════════════════════════════════════════════════════════════
//  BNO055 IMU  (AMG mode — raw accel + gyro, no fusion)
//
//  Vibration index = ||accel_vector|| − g₀
//    At rest: ||a|| ≈ 9.81 m/s² → vibration ≈ 0
//    Structural vibration adds to magnitude → index rises
//    Useful to detect propeller imbalance or rig resonance.
//    For FFT analysis, log the raw ax/ay/az columns and
//    apply a DFT on the host.
// ════════════════════════════════════════════════════════════
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28);
bool bnoAvailable   = false;

IMUData readIMU() {
  IMUData d = {0, 0, 0, 0, 0, 0};
  if (!bnoAvailable) return d;
  imu::Vector<3> acc = bno.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
  imu::Vector<3> gyr = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
  d.ax = acc.x(); d.ay = acc.y(); d.az = acc.z();
  d.gx = gyr.x(); d.gy = gyr.y(); d.gz = gyr.z();
  return d;
}

// ════════════════════════════════════════════════════════════
//  ESC CONTROL  (2 active channels: Motor1=D12, Motor2=D15)
// ════════════════════════════════════════════════════════════
void updateESC(uint8_t ch) {
  if (!escsArmed) {
    motor_running[ch] = false;
    esc[ch].writeMicroseconds(1000);
    return;
  }
  motor_running[ch] = (throttle_us[ch] >= min_throttle_us);
  esc[ch].writeMicroseconds(throttle_us[ch]);
}

void killAllMotors(const __FlashStringHelper *reason) {
  escsArmed = false;
  for (uint8_t i = 0; i < 2; i++) {
    throttle_us[i]   = 1000;
    motor_running[i] = false;
    esc[i].writeMicroseconds(1000);
  }
  Serial.print(F("# MOTORS KILLED: ")); Serial.println(reason);
  Serial.println(F("# ESC_DISARMED"));
}

void escCalibrate(uint8_t ch) {
  Serial.print(F("# ESC ")); Serial.print(ch + 1);
  Serial.println(F(": REMOVE PROP → disconnect battery → reconnect when ready."));
  Serial.println(F("# Sending MAX (2000 µs) in 3 s…"));
  delay(3000);
  esc[ch].writeMicroseconds(2000);
  Serial.println(F("# Sending MIN (1000 µs) in 3 s…"));
  delay(3000);
  esc[ch].writeMicroseconds(1000);
  delay(1000);
  esc[ch].writeMicroseconds(throttle_us[ch]);
  Serial.println(F("# ESC calibration done."));
}

// ════════════════════════════════════════════════════════════
//  LOAD CELL CALIBRATION STATE MACHINE
// ════════════════════════════════════════════════════════════
enum CalState { CAL_IDLE, CAL_TARE, CAL_WEIGHT };
CalState calState     = CAL_IDLE;
int      calCellIndex = -1;

void startLCCalibration(int idx) {
  if (idx < 0 || idx > 3) return;
  calCellIndex = idx;
  calState     = CAL_TARE;
  Serial.print(F("\n# === Calibrating LoadCell ")); Serial.print(idx + 1); Serial.println(F(" ==="));
  Serial.print(F("# Current cal factor: ")); Serial.println(calValues[idx], 3);
  Serial.println(F("# Remove ALL load from cell, then send 't' to tare."));
}

void handleLCCalibration() {
  for (int i = 0; i < 4; i++) LoadCell[i].update();

  if (calState == CAL_TARE) {
    while (Serial.available()) {
      char c = (char)Serial.read();
      if (c == 't' || c == 'T') {
        LoadCell[calCellIndex].tareNoDelay();
        Serial.println(F("# Taring…"));
        delay(5);
        while (Serial.available()) Serial.read();
      }
    }
    if (LoadCell[calCellIndex].getTareStatus()) {
      calState = CAL_WEIGHT;
      Serial.println(F("# Tare done."));
      Serial.println(F("# Place KNOWN WEIGHT on cell and type its value in GRAMS, then Enter."));
      Serial.println(F("# Or type 'cancel'."));
    }

  } else if (calState == CAL_WEIGHT) {
    if (Serial.available()) {
      String inp = Serial.readStringUntil('\n');
      inp.trim();
      if (inp.length() == 0) return;
      if (inp.equalsIgnoreCase("cancel")) {
        Serial.println(F("# Calibration cancelled."));
        calState = CAL_IDLE; calCellIndex = -1; return;
      }
      float grams = inp.toFloat();
      if (grams <= 0.0f) {
        Serial.println(F("# Invalid. Enter positive grams or 'cancel'.")); return;
      }
      Serial.println(F("# Waiting for fresh HX711 reading…"));
      bool gotData = false;
      unsigned long pollStart = millis();
      while ((millis() - pollStart) < 1000UL) {
        if (LoadCell[calCellIndex].update()) { gotData = true; break; }
      }
      if (!gotData) Serial.println(F("# WARNING: HX711 slow; calibrating anyway."));
      LoadCell[calCellIndex].refreshDataSet();
      float newCal = LoadCell[calCellIndex].getNewCalibration(grams);
      if (newCal <= 0.0f) {
        Serial.println(F("# ERROR: Calibration failed — sensor fault?"));
      } else {
        calValues[calCellIndex] = newCal;
        LoadCell[calCellIndex].setCalFactor(newCal);
        Serial.print(F("# LC")); Serial.print(calCellIndex + 1);
        Serial.print(F(" new cal factor: ")); Serial.println(newCal, 4);
        Serial.println(F("# Calibration complete."));
      }
      calState = CAL_IDLE; calCellIndex = -1;
    }
  }
}

// ════════════════════════════════════════════════════════════
//  HELP TEXT
// ════════════════════════════════════════════════════════════
void printHelp() {
  Serial.println(F("\n# ─── Motor Test Rig v3 Commands ────────────────────────────"));
  Serial.println(F("# Motor control:"));
  Serial.println(F("#   mX_YYYY    Motor X (1-2) throttle µs   e.g. m1_1500"));
  Serial.println(F("#   allYYYY    Both ESCs same throttle      e.g. all1400"));
  Serial.println(F("#   stop       Emergency stop (disarms ESCs)"));
  Serial.println(F("#   arm        Enable throttle output (ESC ARMED)"));
  Serial.println(F("#   disarm     Lock throttle at 1000 µs (ESC DISARMED)"));
  Serial.println(F("#   minXXXX    Min throttle µs threshold    e.g. min1050"));
  Serial.println(F("# ESC setup:"));
  Serial.println(F("#   calX       ESC endpoint calibration motor X (REMOVE PROP first)"));
  Serial.println(F("# Current sensors:"));
  Serial.println(F("#   zeroX      Re-zero current sensor X (1-2), motor OFF + battery ON"));
  Serial.println(F("# Load cells:"));
  Serial.println(F("#   tX         Tare load cell X (1-4)"));
  Serial.println(F("#   tall       Tare all four load cells"));
  Serial.println(F("#   cX         Interactive calibration for load cell X"));
  Serial.println(F("#   cX-YYY     Direct-set cal factor  e.g. c2-185.0"));
  Serial.println(F("# RPM:"));
  Serial.println(F("#   rpmX_o     RPM channel X → optical (HW-006 line tracker, default)"));
  Serial.println(F("#   rpmX_b     RPM channel X → back-EMF mode"));
  Serial.println(F("#   marksX_N   Marks per revolution   e.g. marks1_1 (single strip)"));
  Serial.println(F("#   polpairsX_N  Pole pairs for BEMF   e.g. polpairs1_7"));
  Serial.println(F("#   bemfth_X.X  BEMF zero-cross threshold V  e.g. bemfth_0.5"));
  Serial.println(F("# System:"));
  Serial.println(F("#   bno        BNO055 calibration status"));
  Serial.println(F("#   srate_XX   Sample interval ms  e.g. srate_50 = 20 Hz"));
  Serial.println(F("#   diag       Raw ADC diagnostics for current & temperature"));
  Serial.println(F("#   help       This menu"));
  Serial.println(F("# ────────────────────────────────────────────────────────────"));
}

// ════════════════════════════════════════════════════════════
//  CSV HEADER
//  37 fields — must match use-serial.ts parser exactly
// ════════════════════════════════════════════════════════════
void printHeader() {
  Serial.println(F(
    "time_s,"
    "lc1_g,lc2_g,lc3_g,lc4_g,"
    "torque_Nmm,thrust_g,"
    "v1_V,i1_A,pwr1_W,"
    "v2_V,i2_A,pwr2_W,"
    "rpm1,rpm2,"
    "bemf_rpm1,bemf_rpm2,"
    "temp1_C,temp2_C,temp3_C,temp4_C,"
    "vel1_ms,vel2_ms,"
    "ax,ay,az,"
    "gx,gy,gz,"
    "thr1,thr2,thr3,thr4,"
    "run1,run2,run3,run4"
  ));
}

// ════════════════════════════════════════════════════════════
//  COMMAND PARSER
// ════════════════════════════════════════════════════════════
unsigned long sampleIntervalMs = SAMPLE_INTERVAL_MS;

void parseCommand(String &inp) {
  inp.trim();
  if (inp.length() == 0) return;

  // ── mX_YYYY  motor throttle ──────────────────────────────
  if (inp.length() >= 6 && inp[0] == 'm' && isDigit(inp[1]) && inp[2] == '_') {
    int ch  = inp[1] - '1';
    int thr = inp.substring(3).toInt();
    if (ch >= 0 && ch < 2 && thr >= 1000 && thr <= 2000) {
      throttle_us[ch] = thr;
      updateESC(ch);
    } else {
      Serial.println(F("# ERR: motor 1-2, throttle 1000-2000"));
    }
    return;
  }

  // ── allYYYY  both ESCs ───────────────────────────────────
  if (inp.startsWith(F("all"))) {
    int thr = inp.substring(3).toInt();
    if (thr >= 1000 && thr <= 2000) {
      for (uint8_t i = 0; i < 2; i++) { throttle_us[i] = thr; updateESC(i); }
    } else Serial.println(F("# ERR: throttle 1000-2000"));
    return;
  }

  // ── stop ─────────────────────────────────────────────────
  if (inp == F("stop")) { killAllMotors(F("stop command")); return; }

  // ── arm ──────────────────────────────────────────────────
  if (inp == F("arm")) {
    escsArmed = true;
    Serial.println(F("# ESCs ARMED — throttle commands active."));
    Serial.println(F("# ESC_ARMED"));
    return;
  }

  // ── disarm ───────────────────────────────────────────────
  if (inp == F("disarm")) {
    for (uint8_t i = 0; i < 2; i++) {
      throttle_us[i]   = 1000;
      motor_running[i] = false;
      esc[i].writeMicroseconds(1000);
    }
    escsArmed = false;
    Serial.println(F("# ESCs DISARMED — outputs held at 1000 µs."));
    Serial.println(F("# ESC_DISARMED"));
    return;
  }

  // ── minXXXX ──────────────────────────────────────────────
  if (inp.startsWith(F("min"))) {
    int v = inp.substring(3).toInt();
    if (v >= 1000 && v <= 2000) {
      min_throttle_us = v;
      Serial.print(F("# min_thr = ")); Serial.println(v);
    }
    return;
  }

  // ── calX  ESC calibration ────────────────────────────────
  if (inp.startsWith(F("cal")) && inp.length() == 4) {
    int ch = inp[3] - '1';
    if (ch >= 0 && ch < 2) escCalibrate(ch);
    return;
  }

  // ── zeroX  current zero ──────────────────────────────────
  if (inp.startsWith(F("zero")) && inp.length() == 5) {
    int ch = inp[4] - '1';
    if (ch == 0 || ch == 1) calibrateZeroCurrent(ch);
    return;
  }

  // ── tall  tare all ───────────────────────────────────────
  if (inp == F("tall")) {
    for (int i = 0; i < 4; i++) LoadCell[i].tareNoDelay();
    Serial.println(F("# Taring all load cells…")); return;
  }

  // ── tX  tare single ──────────────────────────────────────
  if (inp[0] == 't' && inp.length() == 2 && isDigit(inp[1])) {
    int idx = inp[1] - '1';
    if (idx >= 0 && idx < 4) {
      LoadCell[idx].tareNoDelay();
      Serial.print(F("# LC")); Serial.print(idx+1); Serial.println(F(" taring…"));
    }
    return;
  }

  // ── cX-YYY  direct cal factor ────────────────────────────
  if (inp[0] == 'c' && isDigit(inp[1]) && inp.length() > 2 && inp[2] == '-') {
    int   idx = inp[1] - '1';
    float v   = inp.substring(3).toFloat();
    if (idx >= 0 && idx < 4 && v > 0.0f) {
      calValues[idx] = v;
      LoadCell[idx].setCalFactor(v);
      Serial.print(F("# LC")); Serial.print(idx+1);
      Serial.print(F(" cal = ")); Serial.println(v, 4);
    } else Serial.println(F("# ERR: e.g. c1-185.0"));
    return;
  }

  // ── cX  interactive calibration ──────────────────────────
  if (inp[0] == 'c' && inp.length() == 2 && isDigit(inp[1])) {
    startLCCalibration(inp[1] - '1'); return;
  }

  // ── rpmX_o / rpmX_b  RPM mode ────────────────────────────
  if (inp.startsWith(F("rpm")) && inp.length() >= 6 && isDigit(inp[3]) && inp[4] == '_') {
    int  ch = inp[3] - '1';
    char m  = inp[5];
    if (ch == 0 || ch == 1) {
      rpmMode[ch] = (m == 'b') ? 1 : 0;
      Serial.print(F("# RPM ch")); Serial.print(ch+1);
      Serial.println(rpmMode[ch] ? F(": BEMF mode") : F(": Optical mode"));
    }
    return;
  }

  // ── marksX_N ─────────────────────────────────────────────
  if (inp.startsWith(F("marks")) && inp.length() >= 7) {
    int ch = inp[5] - '1';
    int n  = inp.substring(7).toInt();
    if ((ch == 0 || ch == 1) && n >= 1 && n <= 20) {
      marksPerRev[ch] = n;
      Serial.print(F("# marks_per_rev ch")); Serial.print(ch+1);
      Serial.print(F(" = ")); Serial.println(n);
    }
    return;
  }

  // ── polpairsX_N ──────────────────────────────────────────
  if (inp.startsWith(F("polpairs")) && inp.length() >= 10) {
    int ch = inp[8] - '1';
    int pp = inp.substring(10).toInt();
    if ((ch == 0 || ch == 1) && pp >= 1 && pp <= 50) {
      bemf[ch].polePairs = pp;
      Serial.print(F("# polePairs ch")); Serial.print(ch+1);
      Serial.print(F(" = ")); Serial.println(pp);
    }
    return;
  }

  // ── bemfth_X.X ───────────────────────────────────────────
  if (inp.startsWith(F("bemfth_"))) {
    float v = inp.substring(7).toFloat();
    if (v >= 0.1f && v <= 4.9f) {
      BEMF_THRESHOLD_V = v;
      Serial.print(F("# BEMF threshold = ")); Serial.print(v,2); Serial.println(F(" V"));
    }
    return;
  }

  // ── srate_XX  sample interval ────────────────────────────
  if (inp.startsWith(F("srate_"))) {
    int ms = inp.substring(6).toInt();
    if (ms >= 20 && ms <= 5000) {
      sampleIntervalMs = ms;
      Serial.print(F("# Sample interval = ")); Serial.print(ms); Serial.println(F(" ms"));
    } else Serial.println(F("# ERR: srate 20-5000"));
    return;
  }

  // ── bno  IMU status ──────────────────────────────────────
  if (inp == F("bno")) {
    Serial.print(F("# BNO055: ")); Serial.println(bnoAvailable ? F("OK") : F("NOT FOUND"));
    if (bnoAvailable) {
      uint8_t sys, gyro, accel, mag;
      bno.getCalibration(&sys, &gyro, &accel, &mag);
      Serial.print(F("#   sys=")); Serial.print(sys);
      Serial.print(F(" gyro=")); Serial.print(gyro);
      Serial.print(F(" accel=")); Serial.print(accel);
      Serial.print(F(" mag=")); Serial.println(mag);
      Serial.println(F("# In AMG mode gyro+accel should be 3; mag can be 0."));
    }
    return;
  }

  // ── diag  raw sensor diagnostics ─────────────────────────
  if (inp == F("diag")) {
    Serial.println(F("# ─── Sensor Diagnostics ────────────────────────────────"));
    for (uint8_t ch = 0; ch < 2; ch++) {
      int   raw  = analogRead(PIN_CURRENT[ch]);
      float Vpin = raw * (VCC / 1023.0f);
      float A    = (Vpin - zeroCurrentV[ch] - currentOffset[ch]) / SENSITIVITY;
      Serial.print(F("#  Curr ch")); Serial.print(ch+1);
      Serial.print(F(": raw=")); Serial.print(raw);
      Serial.print(F("  Vpin=")); Serial.print(Vpin,4);
      Serial.print(F("  Vzero=")); Serial.print(zeroCurrentV[ch],4);
      Serial.print(F("  offset=")); Serial.print(currentOffset[ch],4);
      Serial.print(F("  → ")); Serial.print(A,2); Serial.println(F(" A"));
    }
    for (uint8_t i = 0; i < 4; i++) {
      int   raw   = analogRead(PIN_TEMP[i]);
      float Vpin  = raw * (VCC / 1023.0f);
      float denom = VCC - Vpin;
      float R_ntc = (denom > 0.001f) ? (NTC_R_PULLUP * Vpin / denom) : -1.0f;
      Serial.print(F("#  Temp ")); Serial.print(i+1);
      Serial.print(F(": raw=")); Serial.print(raw);
      Serial.print(F("  Vpin=")); Serial.print(Vpin,4);
      Serial.print(F("  R_ntc=")); Serial.print(R_ntc,0);
      Serial.println(F(" Ω  (expect ~100000 at 25°C)"));
    }
    Serial.println(F("# ───────────────────────────────────────────────────────"));
    return;
  }

  // ── help ─────────────────────────────────────────────────
  if (inp == F("help")) { printHelp(); return; }

  Serial.print(F("# Unknown command: ")); Serial.println(inp);
}

// ════════════════════════════════════════════════════════════
//  SETUP
// ════════════════════════════════════════════════════════════
void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10);
  startTimeMs = millis();
  Serial.setTimeout(100);   // don't block on readStringUntil

  // ADC prescaler /16 → ~26 µs per conversion (default /128 = 104 µs)
  ADCSRA = (ADCSRA & 0xF8) | 0x04;

  // ── Load cells ─────────────────────────────────────────
  Serial.println(F("# Initialising load cells…"));
  for (int i = 0; i < 4; i++) {
    LoadCell[i].begin();
    LoadCell[i].start(1000, true);
    if (LoadCell[i].getTareTimeoutFlag()) {
      Serial.print(F("# WARNING: LC")); Serial.print(i+1);
      Serial.println(F(" tare timeout — check wiring. Continuing."));
    } else {
      Serial.print(F("#   LC")); Serial.print(i+1); Serial.println(F(" OK"));
    }
    LoadCell[i].setCalFactor(calValues[i]);
  }

  // ── Current sensor zeroing ─────────────────────────────
  // Requires main battery connected.  If battery is not connected
  // a warning is printed and the default 0.6 V is kept.
  Serial.println(F("# Zeroing current sensors — battery must be connected & motors off"));
  calibrateZeroCurrent(0);
  calibrateZeroCurrent(1);

  // ── ESC init (2 channels: D12=M1, D15=M2) ─────────────
  Serial.println(F("# Initialising ESCs (3 s at 1000 µs) — DISARMED"));
  esc[0].attach(ESC_PIN[0]);   // D12 → J6 → Motor 1
  esc[1].attach(ESC_PIN[1]);   // D15 → J8 → Motor 2
  esc[0].writeMicroseconds(1000);
  esc[1].writeMicroseconds(1000);
  delay(3000);
  Serial.println(F("# ESCs initialised. Press ARM in dashboard before running motors."));
  Serial.println(F("# ESC_DISARMED"));

  // ── RPM (HW-006 line tracker — polled, no interrupts) ──
  // D8/D9 are on PORTH — no interrupt support on Mega 2560.
  pinMode(RPM_PIN[0], INPUT_PULLUP);   // D8 → J26
  pinMode(RPM_PIN[1], INPUT_PULLUP);   // D9 → J27
  lastRPMHigh[0] = (digitalRead(RPM_PIN[0]) == HIGH);
  lastRPMHigh[1] = (digitalRead(RPM_PIN[1]) == HIGH);
  lastPulse[0] = lastPulse[1] = micros();
  Serial.println(F("# RPM: polled edge-detect on D8/D9 — marks=1 per rev (single strip)"));

  // ── BNO055 ─────────────────────────────────────────────
  Wire.begin();
  bnoAvailable = bno.begin();
  if (bnoAvailable) {
    bno.setExtCrystalUse(true);
    bno.setMode(OPERATION_MODE_AMG);   // raw accel+gyro, no fusion
    Serial.println(F("# BNO055 OK (AMG mode — vibration via ||accel|| − 9.81)"));
  } else {
    Serial.println(F("# WARNING: BNO055 not found. IMU columns = 0."));
    Serial.println(F("#   Check SDA=D20, SCL=D21, addr 0x28, 3.3V power."));
  }

  printHelp();
  printHeader();
}

// ════════════════════════════════════════════════════════════
//  MAIN LOOP
// ════════════════════════════════════════════════════════════
void loop() {

  // ── Calibration state machine (pauses CSV output) ────────
  if (calState != CAL_IDLE) {
    handleLCCalibration();
    return;
  }

  // ── Incoming serial commands ──────────────────────────────
  if (Serial.available()) {
    String inp = Serial.readStringUntil('\n');
    parseCommand(inp);
  }

  // ── Optical RPM polling — called 3× per loop for resolution
  pollOpticalRPM();
  pollOpticalRPM();

  // ── Back-EMF zero-cross polling ───────────────────────────
  updateBEMF(0);
  updateBEMF(1);

  // One more RPM poll after BEMF reads
  pollOpticalRPM();

  // ── HX711 non-blocking update ─────────────────────────────
  for (int i = 0; i < 4; i++) LoadCell[i].update();

  // ── Tare completion notifications ─────────────────────────
  for (int i = 0; i < 4; i++) {
    if (LoadCell[i].getTareStatus()) {
      Serial.print(F("# LC")); Serial.print(i+1); Serial.println(F(" tare OK"));
    }
  }

  // ── Timed CSV output ─────────────────────────────────────
  unsigned long now = millis();
  if ((now - lastSampleMs) < sampleIntervalMs) return;
  lastSampleMs = now;

  // Load cells
  float lc[4];
  for (int i = 0; i < 4; i++) lc[i] = LoadCell[i].getData();

  // Mechanics
  float torque_Nmm = (lc[0] - lc[1]) * 9.80665e-3f * (TORQUE_ARM_MM * 0.5f);
  float thrust_g   = lc[2] + lc[3];

  // Electrical (2 channels)
  float I[2], V[2], P[2];
  for (int ch = 0; ch < 2; ch++) {
    I[ch] = readCurrent(ch);
    V[ch] = readVoltage(ch);
    P[ch] = V[ch] * fabsf(I[ch]);  // power is always positive; I can go negative (regen)
  }

  // RPM
  float rpm_opt[2]  = {getOpticalRPM(0),  getOpticalRPM(1)};
  float rpm_bemf[2] = {getBEMF_RPM(0),    getBEMF_RPM(1)};

  // Temperature
  float temp[4];
  for (int i = 0; i < 4; i++) temp[i] = readTemperature(PIN_TEMP[i]);

  // Velocity
  float vel[2] = {readVelocity(PIN_VELOCITY[0]), readVelocity(PIN_VELOCITY[1])};

  // IMU
  IMUData imu = readIMU();

  // ── Emit CSV  ─────────────────────────────────────────────
  // 37 fields, matches use-serial.ts field-index table exactly.
  // ESC3/ESC4 are spare (not soldered) — report 1000 µs / not running.
  float t = (now - startTimeMs) / 1000.0f;

  Serial.print(t, 4);

  // [1-4] load cells
  for (int i = 0; i < 4; i++) { Serial.print(','); Serial.print(lc[i], 2); }

  // [5] torque  [6] thrust
  Serial.print(','); Serial.print(torque_Nmm, 3);
  Serial.print(','); Serial.print(thrust_g,   2);

  // [7-9] motor 1 V/I/P
  Serial.print(','); Serial.print(V[0], 2);
  Serial.print(','); Serial.print(I[0], 2);
  Serial.print(','); Serial.print(P[0], 2);

  // [10-12] motor 2 V/I/P
  Serial.print(','); Serial.print(V[1], 2);
  Serial.print(','); Serial.print(I[1], 2);
  Serial.print(','); Serial.print(P[1], 2);

  // [13-14] optical RPM
  Serial.print(','); Serial.print(rpm_opt[0],  1);
  Serial.print(','); Serial.print(rpm_opt[1],  1);

  // [15-16] BEMF RPM
  Serial.print(','); Serial.print(rpm_bemf[0], 1);
  Serial.print(','); Serial.print(rpm_bemf[1], 1);

  // [17-20] temperatures
  for (int i = 0; i < 4; i++) { Serial.print(','); Serial.print(temp[i], 1); }

  // [21-22] velocity
  Serial.print(','); Serial.print(vel[0], 2);
  Serial.print(','); Serial.print(vel[1], 2);

  // [23-28] IMU
  Serial.print(','); Serial.print(imu.ax, 3);
  Serial.print(','); Serial.print(imu.ay, 3);
  Serial.print(','); Serial.print(imu.az, 3);
  Serial.print(','); Serial.print(imu.gx, 3);
  Serial.print(','); Serial.print(imu.gy, 3);
  Serial.print(','); Serial.print(imu.gz, 3);

  // [29-32] throttle: M1, M2, spare=1000, spare=1000
  Serial.print(','); Serial.print(throttle_us[0]);
  Serial.print(','); Serial.print(throttle_us[1]);
  Serial.print(','); Serial.print(1000);   // J5 spare
  Serial.print(','); Serial.print(1000);   // J7 spare

  // [33-36] motor running flags: M1, M2, spare=0, spare=0
  Serial.print(','); Serial.print(motor_running[0] ? '1' : '0');
  Serial.print(','); Serial.print(motor_running[1] ? '1' : '0');
  Serial.print(','); Serial.print('0');    // J5 spare
  Serial.print(','); Serial.print('0');    // J7 spare

  Serial.println();
}