/*
 * IMU BLE Mouse - Seeed XIAO nRF52840 Sense  (v3.4)
 * ================================================================
 * MINIMUM (just working mouse, no BLE config):
 *   leave only FEATURE_BATTERY_MONITOR + FEATURE_BOOT_LOOP_DETECT
 *
 * RECOMMENDED first test:
 *   enable FEATURE_CONFIG_SERVICE, keep TAP + STREAM + TELEMETRY off
 *
 * Dependencies:
 *   FEATURE_TELEMETRY  requires FEATURE_CONFIG_SERVICE
 *   FEATURE_IMU_STREAM requires FEATURE_CONFIG_SERVICE
 * ================================================================
 */

#include "config.h"
#include "imu.h"
#include "motion.h"
#include "ble_config.h"
#include "battery.h"
#include "buttons.h"
#include <bluefruit.h>
#include <Adafruit_LittleFS.h>
#include <InternalFileSystem.h>
#include "Wire.h"
#include "sleep.h"

// Boot-loop detection 
#ifdef FEATURE_BOOT_LOOP_DETECT
  static uint32_t __attribute__((section(".noinit"))) bootCount;
  static uint32_t __attribute__((section(".noinit"))) bootMagic;
#endif

// BLE Standard Services 
BLEDis         bledis;
BLEHidAdafruit blehid;
#ifdef FEATURE_BATTERY_MONITOR
  BLEBas blebas;
#endif

// Persistence 
using namespace Adafruit_LittleFS_Namespace;
File cfgFile(InternalFS);

// Config definitions 
Config cfg;
const Config CFG_DEFAULTS = {
  CONFIG_MAGIC, 600.0f, 0.020f, 0.08f, CURVE_LINEAR, 0x00, CHARGE_SLOW,
  /*tapThreshold=*/12, /*tapAction=*/TAP_ACTION_KEY, /*tapKey=*/0x04, /*tapMod=*/0x03,  // Ctrl+Shift+A
  /*jerkThreshold=*/2000.0f, /*tapFreezeEnabled=*/1, /*featureFlags=*/FLAG_ALL_DEFAULT
};

// Telemetry definition 
#ifdef FEATURE_TELEMETRY
TelemetryPacket telem = {};
#endif

// Tuning constants
const int   IMU_POLL_MS        = 2;     // FIFO poll period; samples arrive at 208 Hz
const int   BIAS_SAMPLES       = 200;
const unsigned long IDLE_MS    = 1500;  // no motion for this long = idle (sleep countdown starts)
// Requested BLE connection interval (units of 1.25 ms). HID reports are paced to the
// negotiated interval, so the notify queue never fills and the loop never blocks.
// Slave latency lets the radio skip empty events while the cursor is still; reports
// are still sent at the next event, so it adds no pointer latency.
const uint16_t CONN_INTERVAL_MIN = 6;   // 7.5 ms
const uint16_t CONN_INTERVAL_MAX = 9;   // 11.25 ms
const uint16_t CONN_SLAVE_LATENCY = 4;
const unsigned long BATT_REPORT_MS       = 20000;
const unsigned long TELEMETRY_MS         = 1000;
const unsigned long HEARTBEAT_MS         = 10000;
const int           HEARTBEAT_DUR        = 30;
const unsigned long BOOT_SAFE_MS         = 5000;
#ifdef FEATURE_IMU_STREAM
  const unsigned long IMU_STREAM_RATE_MS = 20;
#endif
const float BATT_FULL                    = 4.20f;
const float BATT_EMPTY                   = 3.00f;
const float BATT_CRITICAL                = 3.10f;
#ifdef FEATURE_TAP_DETECTION
  const unsigned long CLICK_HOLD_MS = 60;
#endif
#ifdef FEATURE_TEMP_COMPENSATION
  const float TEMP_COMP_COEFF_DPS_C = 0.004f;
#endif

// Global state definitions
float calTempC    = 25.0f;
float cachedTempC = 25.0f;

#ifdef FEATURE_TAP_DETECTION
  bool    clickButtonDown = false;
  uint8_t clickButton     = 0;
  unsigned long clickDownMs= 0;
  uint32_t statLeftClicks  = 0;
  uint32_t statRightClicks = 0;
#endif

#ifdef FEATURE_IMU_STREAM
  bool imuStreamEnabled = false;
  uint32_t streamNotifyFails = 0;
  uint32_t streamNotifyOk   = 0;
  unsigned long lastStreamDiag = 0;
  // Back-off state: after STREAM_BACKOFF_THRESH consecutive fails, skip notifies
  // for STREAM_BACKOFF_MS to let the SoftDevice HVN TX semaphore drain.
  // Without this, every notify() blocks for BLE_GENERIC_TIMEOUT (100ms).
  uint8_t       streamConsecFails = 0;
  unsigned long streamBackoffUntil = 0;
  const uint8_t        STREAM_BACKOFF_THRESH = 2;    // fails before backing off
  const unsigned long  STREAM_BACKOFF_MS     = 500;  // cooldown window
#endif

bool pendingCal   = false;
bool pendingReset = false;
#ifdef FEATURE_OTA
  bool pendingOTA = false;
#endif


ChargeStatus lastChargeStatus = CHGSTAT_DISCHARGING;

unsigned long lastMotionMs  = 0;
unsigned long lastImuPoll   = 0;
unsigned long lastBattTime  = 0;
unsigned long lastHeartbeat = 0;
unsigned long lastTelemetry = 0;
unsigned long bootStartMs   = 0;
#ifdef FEATURE_IMU_STREAM
  unsigned long lastImuStream = 0;
#endif

#ifdef FEATURE_TELEMETRY
  uint16_t statRecalCount = 0;
  float    statBiasRms    = 0.0f;
#endif

bool safeMode         = false;
bool bootCountCleared = false;

// Connection 
static volatile unsigned long connectedAtMs = 0;
static volatile bool          connParamPending = false;
static void onConnect(uint16_t connHdl) {
  (void)connHdl;
  connectedAtMs = millis();
  connParamPending = true;
}

// Advertising 
static void startAdvertising() {
  Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
  Bluefruit.Advertising.addTxPower();
  Bluefruit.Advertising.addAppearance(BLE_APPEARANCE_HID_MOUSE);
  Bluefruit.Advertising.addService(blehid);
  #ifdef FEATURE_BATTERY_MONITOR
    Bluefruit.Advertising.addService(blebas);
  #endif
  Bluefruit.Advertising.addName();
  Bluefruit.Advertising.restartOnDisconnect(true);
  Bluefruit.Advertising.setInterval(32, 244);
  Bluefruit.Advertising.setFastTimeout(30);
  Bluefruit.Advertising.start(0);
}

// Setup 
void setup() {
  Serial.begin(115200);
  unsigned long serialWait = millis();
  while (!Serial && (millis() - serialWait < 2000)) { delay(10); }

  pinMode(PIN_CHG,  INPUT_PULLUP);
  pinMode(LED_RED,   OUTPUT); digitalWrite(LED_RED,   HIGH);
  pinMode(LED_GREEN, OUTPUT); digitalWrite(LED_GREEN, HIGH);
  pinMode(LED_BLUE,  OUTPUT); digitalWrite(LED_BLUE,  HIGH);

  // Boot-loop detection 
  #ifdef FEATURE_BOOT_LOOP_DETECT
    if (bootMagic != 0xCAFEBABE) { bootMagic = 0xCAFEBABE; bootCount = 0; }
    bootCount++;
    Serial.print("[BOOT] count="); Serial.println(bootCount);
    if (bootCount >= 3) {
      bootCount = 0; safeMode = true;
      Serial.println("[BOOT] Boot loop - safe mode (no config service)");
      InternalFS.begin(); InternalFS.remove(CONFIG_FILENAME);
      for (int i=0; i<3; i++) { digitalWrite(LED_RED,LOW); delay(150); digitalWrite(LED_RED,HIGH); delay(150); }  // fault: red
    }
  #endif

  loadConfig();
  applyChargeMode(cfg.chargeMode);

  // configAttrTableSize MUST be called before Bluefruit.begin().
  // Value must be >= what HID+DIS+BAS+custom services actually need.
  // Too small crashes just as hard as too large. Floor is 1536.
  Serial.print("[BLE] ATT table: "); Serial.print(ATT_TABLE_SIZE); Serial.println(" bytes");
  Bluefruit.configAttrTableSize(ATT_TABLE_SIZE);
  Bluefruit.configPrphBandwidth(BANDWIDTH_MAX);  // Raise MTU so 24-byte telemetry fits in one notify

  Bluefruit.begin(1, 0);
  Bluefruit.setTxPower(4);
  Bluefruit.setName(safeMode ? "IMU Mouse (safe)" : "IMU Mouse");
  Bluefruit.Periph.setConnInterval(CONN_INTERVAL_MIN, CONN_INTERVAL_MAX);
  Bluefruit.Periph.setConnSlaveLatency(CONN_SLAVE_LATENCY);
  Bluefruit.Periph.setConnectCallback(onConnect);

  Wire1.begin();  // LSM6DS3 is on internal I2C bus (Wire1), must init before imu.begin()
  // Keep the library's scaling in sync with imuConfigure() (sleep.cpp uses readFloatGyro*)
  imu.settings.gyroRange       = 1000;
  imu.settings.gyroSampleRate  = 208;
  imu.settings.accelRange      = 4;
  imu.settings.accelSampleRate = 208;
  imu.settings.accelODROff     = 0;
  if (imu.begin() != 0) {
    Serial.println("[ERROR] IMU init failed");
    while(1) { digitalWrite(LED_RED, !digitalRead(LED_RED)); delay(100); }  // fault: red rapid blink
  }
  imuConfigure();
  Serial.println("[OK] IMU ready");

  #ifdef FEATURE_TAP_DETECTION
    if (cfg.featureFlags & FLAG_TAP_ENABLED) setupTapDetection();
  #endif

  #ifdef FEATURE_PHYSICAL_BUTTONS
    setupPhysicalButtons();
  #endif

  cachedTempC = readIMUTemp();

  #ifdef FEATURE_BATTERY_MONITOR
    initBatteryADC();
    updateBattery();
  #endif

  calibrateGyroBias();

  sleepManagerInit();

  bledis.setManufacturer("Seeed Studio");
  bledis.setModel("XIAO nRF52840 Sense");
  bledis.begin();

  blehid.begin();

  #ifdef FEATURE_BATTERY_MONITOR
    blebas.begin(); blebas.write(batteryPercent(readBatteryVoltage()));
  #endif

  #ifdef FEATURE_CONFIG_SERVICE
    if (!safeMode) {
      setupConfigService();
      Serial.println("[OK] Config service started");
    } else {
      Serial.println("[SAFE] Config service skipped");
    }
  #endif

  startAdvertising();
  Serial.print("[OK] Advertising - features:");
  #ifdef FEATURE_CONFIG_SERVICE
    Serial.print(" CFG");
  #endif
  #ifdef FEATURE_TELEMETRY
    Serial.print(" TELEM");
  #endif
  #ifdef FEATURE_IMU_STREAM
    Serial.print(" STREAM");
  #endif
  #ifdef FEATURE_TAP_DETECTION
    Serial.print(" TAP");
  #endif
  #ifdef FEATURE_TEMP_COMPENSATION
    Serial.print(" TEMPCOMP");
  #endif
  #ifdef FEATURE_AUTO_RECAL
    Serial.print(" AUTORECAL");
  #endif
  #ifdef FEATURE_BATTERY_MONITOR
    Serial.print(" BATT");
  #endif
  #ifdef FEATURE_BOOT_LOOP_DETECT
    Serial.print(" BOOTDET");
  #endif
  #ifdef FEATURE_PHYSICAL_BUTTONS
    Serial.print(" PHYSBTN");
  #endif
  Serial.print(" SLEEP");
  Serial.println();

  bootStartMs = millis();
  lastMotionMs = lastImuPoll = lastBattTime = lastHeartbeat = lastTelemetry = millis();
}


// Debug state (serial commands)
static bool          statsEnabled = false;   // 's' - 1 Hz pipeline stats
static bool          rawLogEnabled = false;  // 'l' - every FIFO sample as CSV
static unsigned long diagUntil = 0;          // 'd' - 10 s of yaw/pitch/roll
static uint32_t statHidReports = 0, statHidFails = 0, statSamples = 0;
static int32_t  statSentX = 0, statSentY = 0;
static uint32_t statMaxHidGapUs = 0;
// 't' - synthetic sweep, totals reported when it ends
static bool     testRunning = false;
static int32_t  testPosX = 0, testNegX = 0, testPosY = 0, testNegY = 0;
static uint32_t testReports = 0, testFails = 0;

static void printStats(unsigned long now) {
  const MotionDebug& m = motionDebug();
  BLEConnection* c = Bluefruit.connected() ? Bluefruit.Connection(Bluefruit.connHandle()) : nullptr;
  Serial.print("[STAT] smp/s="); Serial.print(statSamples);
  Serial.print(" ovr="); Serial.print(imuFifoOverruns);
  Serial.print(" i2cErr="); Serial.print(imuI2cErrors);
  Serial.print(" hid/s="); Serial.print(statHidReports);
  Serial.print(" fail="); Serial.print(statHidFails);
  Serial.print(" sent="); Serial.print(statSentX); Serial.print(","); Serial.print(statSentY);
  Serial.print(" maxGap="); Serial.print(statMaxHidGapUs / 1000.0f, 1); Serial.print("ms");
  Serial.print(" ci="); if (c) { Serial.print(c->getConnectionInterval() * 1.25f, 2); Serial.print("ms"); } else Serial.print("-");
  Serial.print(" bias(mdps)=");
  for (int i = 0; i < 3; i++) { Serial.print(m.bias[i] * (180.f / PI) * 1000.f, 1); if (i < 2) Serial.print(","); }
  Serial.print(m.biasValid ? " valid" : " UNCAL");
  Serial.print(m.still ? " still" : " moving");
  Serial.print(" upd="); Serial.print(m.biasUpdates);
  Serial.print(" roll="); Serial.print(m.rollDeg, 1);
  Serial.print(" up="); Serial.print(m.up[0], 3); Serial.print(","); Serial.print(m.up[1], 3); Serial.print(","); Serial.print(m.up[2], 3);
  Serial.print(" T="); Serial.println(cachedTempC, 2);
  statSamples = statHidReports = statHidFails = 0;
  statMaxHidGapUs = 0;
  (void)now;
}

// Loop 
void loop() {
  unsigned long now = millis();

  // Clear boot counter after BOOT_SAFE_MS of stable running
  #ifdef FEATURE_BOOT_LOOP_DETECT
    if (!bootCountCleared && (now - bootStartMs >= BOOT_SAFE_MS)) {
      bootCount = 0; bootCountCleared = true;
      Serial.println("[BOOT] Stable - counter cleared");
    }
  #endif

  // Serial commands: 'c' calibrate, 'r' factory reset, 'd' axis diagnostic,
  // 's' toggle stats, 'l' toggle raw sample log, 't' synthetic sweep test
  while (Serial.available()) {
    char cmd = Serial.read();
    if (cmd == 'c') { Serial.println("[SERIAL] Calibrate"); pendingCal   = true; }
    if (cmd == 'r') { Serial.println("[SERIAL] Reset");     pendingReset = true; }
    if (cmd == 'd') { Serial.println("[DIAG] yaw/pitch (rad/s, roll-compensated) + roll for 10s"); diagUntil = now + 10000; }
    if (cmd == 's') { statsEnabled = !statsEnabled; Serial.println(statsEnabled ? "[STAT] on" : "[STAT] off"); }
    if (cmd == 'l') { rawLogEnabled = !rawLogEnabled; Serial.println(rawLogEnabled ? "[RAW] gx,gy,gz,ax,ay,az (LSB),roll (0.1 deg)" : "[RAW] off"); }
    if (cmd == 't' && !testRunning) {
      Serial.println("[TEST] Synthetic sweep: right, left, down, up (1 s each)");
      testPosX = testNegX = testPosY = testNegY = 0; testReports = testFails = 0;
      testRunning = true; motionStartTest();
    }
    #ifdef FEATURE_OTA
      if (cmd == 'o') { Serial.println("[SERIAL] OTA DFU"); pendingOTA = true; }
    #endif
  }

  if (pendingCal)   { pendingCal = false; calibrateGyroBias(); }
  if (pendingReset) { pendingReset = false; factoryReset(); }
  #ifdef FEATURE_OTA
    if (pendingOTA) {
      pendingOTA = false;
      Serial.println("[OTA] Disconnecting BLE and entering bootloader DFU mode...");
      Serial.flush();
      // Gracefully close the BLE connection first so the host can detect the
      // disconnect and be ready to see DfuTarg advertise after the reboot.
      if (Bluefruit.connected()) {
        Bluefruit.disconnect(0);
        delay(300);
      }
      delay(200);
      enterOTADfu();  // Adafruit nRF52 core: sets GPREGRET correctly and resets into bootloader OTA mode
    }
  #endif

  // Ask the host for a short connection interval once the link has settled.
  if (connParamPending && Bluefruit.connected() && now - connectedAtMs >= 2000) {
    connParamPending = false;
    uint16_t h = Bluefruit.connHandle();
    BLEConnection* c = Bluefruit.Connection(h);
    if (c) {
      Serial.print("[BLE] Connection interval "); Serial.print(c->getConnectionInterval() * 1.25f, 2); Serial.println("ms");
      if (c->getConnectionInterval() > CONN_INTERVAL_MAX) {
        ble_gap_conn_params_t p = { CONN_INTERVAL_MIN, CONN_INTERVAL_MAX, CONN_SLAVE_LATENCY, BLE_GAP_CONN_SUPERVISION_TIMEOUT_MS / 10 };
        uint32_t err = sd_ble_gap_conn_param_update(h, &p);
        Serial.print("[BLE] Requested 7.5-11.25ms interval (err="); Serial.print(err); Serial.println(")");
      }
    }
  }

  // Heartbeat LED (non-blocking, so it can't delay HID reports)
  static int heartbeatLed = -1;
  if (now - lastHeartbeat >= HEARTBEAT_MS) {
    lastHeartbeat = now;
    heartbeatLed = Bluefruit.connected() ? LED_BLUE : LED_GREEN;  // blue=BT connected, green=advertising
    digitalWrite(heartbeatLed, LOW);
  }
  if (heartbeatLed >= 0 && now - lastHeartbeat >= (unsigned long)HEARTBEAT_DUR) {
    digitalWrite(heartbeatLed, HIGH); heartbeatLed = -1;
  }

  #ifdef FEATURE_BATTERY_MONITOR
    if (now - lastBattTime >= BATT_REPORT_MS) { lastBattTime = now; updateBattery(); }
  #endif

  #ifdef FEATURE_TAP_DETECTION
    if (cfg.featureFlags & FLAG_TAP_ENABLED) processTaps(now);
  #endif

  #ifdef FEATURE_PHYSICAL_BUTTONS
    processPhysicalButtons();
  #endif

  // Debug modes keep the device awake so it can be measured lying still.
  bool debugAwake = statsEnabled || rawLogEnabled || testRunning;
  if (debugAwake && sleepStage == SLEEP_IMU_LP) sleepManagerWakeIMU();
  bool idle = !debugAwake && (now - lastMotionMs >= IDLE_MS);
  {
    bool idle_for_sleep = (sleepStage == SLEEP_IMU_LP) ? true : idle;
    if (sleepManagerUpdate(now, idle_for_sleep, Bluefruit.connected())) return;
  }

  static ImuRaw lastSample = {};
  if (now - lastImuPoll >= (unsigned long)IMU_POLL_MS) {
    lastImuPoll = now;

    static unsigned long lastTempMs = 0;
    if (now - lastTempMs >= 1000) { lastTempMs = now; cachedTempC = readIMUTemp(); }

    #ifdef FEATURE_TELEMETRY
      if (!safeMode && (now - lastTelemetry >= TELEMETRY_MS)) {
        lastTelemetry = now; pushTelemetry(now);
      }
    #endif

    // Run the tracking pipeline on every sample the IMU produced since the last poll.
    ImuRaw samples[32];
    int n = imuReadFifo(samples, 32);
    for (int i = 0; i < n; i++) {
      motionProcess(samples[i], now);
      if (rawLogEnabled) {
        const ImuRaw& r = samples[i];
        Serial.print(r.gx); Serial.print(','); Serial.print(r.gy); Serial.print(','); Serial.print(r.gz); Serial.print(',');
        Serial.print(r.ax); Serial.print(','); Serial.print(r.ay); Serial.print(','); Serial.print(r.az); Serial.print(',');
        Serial.println((int)lroundf(motionDebug().rollDeg * 10.0f));
      }
    }
    if (n > 0) lastSample = samples[n - 1];
    statSamples += n;
    if (motionConsumeMoved()) lastMotionMs = now;
  }

  // HID report, paced just under the connection interval: every radio event gets
  // a report (an occasional event carries two), none goes out empty. Counts that
  // could not be sent stay in the accumulator, so no motion is ever dropped.
  static int32_t  streamMoveX = 0, streamMoveY = 0;
  static uint32_t lastHidUs = 0;
  {
    bool conn = Bluefruit.connected();
    uint32_t intervalUs = 10000;
    if (conn) {
      BLEConnection* c = Bluefruit.Connection(Bluefruit.connHandle());
      if (c) intervalUs = c->getConnectionInterval() * 1250UL;
    }
    intervalUs = intervalUs * 7 / 8;
    uint32_t nowUs = micros();
    int8_t dx, dy;
    if (nowUs - lastHidUs >= intervalUs && motionTakeCounts(dx, dy)) {
      uint32_t gap = nowUs - lastHidUs;
      lastHidUs = nowUs;
      bool ok = !conn || blehid.mouseMove(dx, dy);  // not connected: counts are discarded
      if (ok) {
        statHidReports++; statSentX += dx; statSentY += dy;
        if (gap > statMaxHidGapUs && gap < 1000000UL) statMaxHidGapUs = gap;
        streamMoveX += dx; streamMoveY += dy;
        if (testRunning) {
          testReports++;
          if (dx > 0) testPosX += dx; else testNegX += dx;
          if (dy > 0) testPosY += dy; else testNegY += dy;
        }
      } else {
        motionReturnCounts(dx, dy); statHidFails++; testFails++;
      }
    }
  }

  if (testRunning && !motionTestActive()) {
    testRunning = false;
    Serial.print("[TEST] done: +X="); Serial.print(testPosX); Serial.print(" -X="); Serial.print(testNegX);
    Serial.print(" +Y="); Serial.print(testPosY); Serial.print(" -Y="); Serial.print(testNegY);
    Serial.print(" reports="); Serial.print(testReports); Serial.print(" fails="); Serial.println(testFails);
  }

  if (statsEnabled) {
    static unsigned long lastStat = 0;
    if (now - lastStat >= 1000) { lastStat = now; printStats(now); }
  }

  // Axis diagnostic
  if (diagUntil && now < diagUntil) {
    static unsigned long lastDiagPrint = 0;
    if (now - lastDiagPrint >= 100) { lastDiagPrint = now;
      const MotionDebug& m = motionDebug();
      Serial.print("[DIAG] yaw="); Serial.print(m.yaw, 3);
      Serial.print(" pitch="); Serial.print(m.pitch, 3);
      Serial.print(" roll="); Serial.print(m.rollDeg, 1);
      Serial.print(" speed="); Serial.println(m.speed, 4);
    }
  } else if (diagUntil) { diagUntil = 0; Serial.println("[DIAG] Done"); }

  #ifdef FEATURE_IMU_STREAM
    if (!safeMode && imuStreamEnabled && Bluefruit.connected()
        && (now - lastImuStream >= IMU_STREAM_RATE_MS)) {
      lastImuStream = now;

      if (now < streamBackoffUntil) {
        // Backing off - host TX buffer congested, skip to avoid 100ms block
      } else {
        const MotionDebug& m = motionDebug();
        ImuPacket pkt;
        int8_t mx = (int8_t)constrain(streamMoveX, -127, 127);
        int8_t my = (int8_t)constrain(streamMoveY, -127, 127);
        pkt.gyroX_mDPS = (int16_t)constrain(m.pitch * (180.f/PI) * 1000.f, -32000, 32000);
        pkt.gyroZ_mDPS = (int16_t)constrain(m.yaw   * (180.f/PI) * 1000.f, -32000, 32000);
        pkt.accelX_mg  = (int16_t)constrain(lastSample.ax * ACCEL_G_PER_LSB * 1000.f, -32000, 32000);
        pkt.accelY_mg  = (int16_t)constrain(lastSample.ay * ACCEL_G_PER_LSB * 1000.f, -32000, 32000);
        pkt.accelZ_mg  = (int16_t)constrain(lastSample.az * ACCEL_G_PER_LSB * 1000.f, -32000, 32000);
        pkt.moveX      = mx;
        pkt.moveY      = my;
        pkt.flags      = idle ? 0x01 : 0x00;
        pkt._pad       = 0;
        if (cfgImuStream.notify((uint8_t*)&pkt, sizeof(pkt))) {
          streamMoveX -= mx; streamMoveY -= my;
          streamNotifyOk++;
          streamConsecFails = 0;
        } else {
          streamNotifyFails++;
          streamConsecFails++;
          if (streamConsecFails >= STREAM_BACKOFF_THRESH) {
            streamBackoffUntil = now + STREAM_BACKOFF_MS;
            streamConsecFails  = 0;
            Serial.print("[STREAM] TX congested - backing off ");
            Serial.print(STREAM_BACKOFF_MS); Serial.println("ms");
          }
        }
      }

      // Periodic stream health report every 10 seconds
      if (now - lastStreamDiag >= 10000) {
        lastStreamDiag = now;
        Serial.print("[STREAM] ok="); Serial.print(streamNotifyOk);
        Serial.print(" fail="); Serial.print(streamNotifyFails);
        Serial.print(" rate="); Serial.print((streamNotifyOk * 1000UL) / 10000); Serial.println("pkt/s");
        streamNotifyOk = 0; streamNotifyFails = 0;
      }
    }
    if (!imuStreamEnabled) streamMoveX = streamMoveY = 0;
  #endif
}