/*
 * IMU BLE Mouse — Seeed XIAO nRF52840 Sense  (v3.4)
 * ================================================================
 * Feature flags — comment out any line to disable that feature.
 * ATT table size is computed automatically from enabled features.
 * Start with minimal flags to isolate the SoftDevice RAM issue,
 * then re-enable one at a time.
 *
 * 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
 *
 * ── Feature flag index ───────────────────────────────────────────
 *   FEATURE_CONFIG_SERVICE    Custom GATT service (ConfigBlob + Command)
 *   FEATURE_TELEMETRY         +24-byte notify characteristic, 1 Hz
 *   FEATURE_IMU_STREAM        +14-byte notify characteristic, ~100 Hz
 *   FEATURE_TAP_DETECTION     LSM6DS3 hardware tap engine → L/R clicks
 *   FEATURE_TEMP_COMPENSATION Gyro drift correction by temperature delta
 *   FEATURE_AUTO_RECAL        Recalibrate after AUTO_RECAL_MS idle
 *   FEATURE_BATTERY_MONITOR   ADC battery read + BLE Battery Service
 *   FEATURE_BOOT_LOOP_DETECT  .noinit crash counter → safe mode
 *
 * Dependencies:
 *   FEATURE_TELEMETRY  requires FEATURE_CONFIG_SERVICE
 *   FEATURE_IMU_STREAM requires FEATURE_CONFIG_SERVICE
 * ================================================================
 */

// ─── Feature Flags ────────────────────────────────────────────────────────────
#define FEATURE_CONFIG_SERVICE
#define FEATURE_TELEMETRY
#define FEATURE_IMU_STREAM
#define FEATURE_TAP_DETECTION
#define FEATURE_TEMP_COMPENSATION
#define FEATURE_AUTO_RECAL
#define FEATURE_BATTERY_MONITOR
#define FEATURE_BOOT_LOOP_DETECT

// ─── Debug ────────────────────────────────────────────────────────────────────
// #define DEBUG

// ─── ATT table size ───────────────────────────────────────────────────────────
// Must be passed to configAttrTableSize() BEFORE Bluefruit.begin().
// Too small → SoftDevice panics. Too large → SoftDevice claims RAM the linker
// already assigned to the app and also panics. Sweet spot is 1500-2000.
//
// HID service alone costs ~700 B (report map + 6 characteristics + CCCDs).
// Add DIS (~100 B), BAS (~50 B), then our custom characteristics.
// Use 1536 as the safe base; add 256 per notify characteristic.
//
// Feature cost breakdown:
//   HID + DIS + BAS baseline : ~900 B (always present)
//   CONFIG_SERVICE (blob+cmd) :  ~80 B
//   TELEMETRY (R/N 24 bytes)  :  ~40 B
//   IMU_STREAM (N 14 bytes)   :  ~30 B
//
#define _ATT_BASE 900
#ifdef FEATURE_CONFIG_SERVICE
  #define _ATT_CFG 80
#else
  #define _ATT_CFG 0
#endif
#ifdef FEATURE_TELEMETRY
  #define _ATT_TELEM 40
#else
  #define _ATT_TELEM 0
#endif
#ifdef FEATURE_IMU_STREAM
  #define _ATT_STREAM 30
#else
  #define _ATT_STREAM 0
#endif
// Floor of 1536 so we never go below what HID actually needs
#define ATT_TABLE_SIZE_CALC (_ATT_BASE + _ATT_CFG + _ATT_TELEM + _ATT_STREAM)
#define ATT_TABLE_SIZE (ATT_TABLE_SIZE_CALC < 1536 ? 1536 : ATT_TABLE_SIZE_CALC)

// ─── Includes ─────────────────────────────────────────────────────────────────
#include <bluefruit.h>
#include <Adafruit_LittleFS.h>
#include <InternalFileSystem.h>
#include "LSM6DS3.h"
#include "Wire.h"

// ─── Boot-loop detection ──────────────────────────────────────────────────────
#ifdef FEATURE_BOOT_LOOP_DETECT
  static uint32_t __attribute__((section(".noinit"))) bootCount;
  static uint32_t __attribute__((section(".noinit"))) bootMagic;
#endif
static bool safeMode         = false;
static bool bootCountCleared = false;

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

// ─── BLE Config Service ───────────────────────────────────────────────────────
#ifdef FEATURE_CONFIG_SERVICE
  BLEService        cfgService   (0x1234);
  BLECharacteristic cfgBlob      (0x1235);  // ConfigBlob  R/W  16 bytes
  BLECharacteristic cfgCommand   (0x1236);  // Command       W   1 byte
  #ifdef FEATURE_TELEMETRY
    BLECharacteristic cfgTelemetry(0x1237); // Telemetry   R/N  24 bytes
  #endif
  #ifdef FEATURE_IMU_STREAM
    BLECharacteristic cfgImuStream(0x1238); // ImuStream     N  14 bytes
  #endif
#endif

// ─── IMU ──────────────────────────────────────────────────────────────────────
LSM6DS3 imu(I2C_MODE, 0x6A);

#define REG_CTRL1_XL    0x10
#define REG_TAP_CFG     0x58
#define REG_TAP_THS_6D  0x59
#define REG_INT_DUR2    0x5A
#define REG_WAKE_UP_THS 0x5B
#define REG_MD1_CFG     0x5E
#define REG_TAP_SRC     0x1C
#define REG_OUT_TEMP_L  0x20
#define REG_OUT_TEMP_H  0x21

// ─── Pins ─────────────────────────────────────────────────────────────────────
#define PIN_VBAT_ENABLE (14)
#define PIN_VBAT_READ   (32)
#define PIN_CHG         (23)
#define PIN_HICHG       (22)

// ─── Persistence ──────────────────────────────────────────────────────────────
#define CONFIG_FILENAME  "/imu_mouse_cfg.bin"
#define CONFIG_MAGIC     0xDEAD1239UL

using namespace Adafruit_LittleFS_Namespace;
File cfgFile(InternalFS);

// ─── Enums ────────────────────────────────────────────────────────────────────
enum CurveType   : uint8_t { CURVE_LINEAR=0, CURVE_SQUARE=1, CURVE_SQRT=2 };
enum ChargeMode  : uint8_t { CHARGE_OFF=0,   CHARGE_SLOW=1,  CHARGE_FAST=2 };
enum ChargeStatus: uint8_t { CHGSTAT_DISCHARGING=0, CHGSTAT_CHARGING=1, CHGSTAT_FULL=2 };

// ─── Config (stored in flash) ─────────────────────────────────────────────────
struct Config {
  uint32_t   magic;
  float      sensitivity;
  float      deadZone;
  float      accelStrength;
  CurveType  curve;
  uint8_t    axisFlip;
  ChargeMode chargeMode;
};
Config cfg;
const Config CFG_DEFAULTS = {
  CONFIG_MAGIC, 600.0f, 0.060f, 0.08f, CURVE_LINEAR, 0x00, CHARGE_SLOW
};

// ─── ConfigBlob (over BLE, no magic) ─────────────────────────────────────────
struct __attribute__((packed)) ConfigBlob {
  float   sensitivity;    // [0]
  float   deadZone;       // [4]
  float   accelStrength;  // [8]
  uint8_t curve;          // [12]
  uint8_t axisFlip;       // [13]
  uint8_t chargeMode;     // [14]
  uint8_t _pad;           // [15]
};
static_assert(sizeof(ConfigBlob) == 16, "ConfigBlob must be 16 bytes");

// ─── TelemetryPacket ──────────────────────────────────────────────────────────
#ifdef FEATURE_TELEMETRY
struct __attribute__((packed)) TelemetryPacket {
  uint32_t uptimeSeconds;   // [0]
  uint32_t leftClicks;      // [4]
  uint32_t rightClicks;     // [8]
  float    tempCelsius;     // [12]
  float    biasRmsRadS;     // [16]
  uint16_t recalCount;      // [20]
  uint8_t  chargeStatus;    // [22]
  uint8_t  _pad;            // [23]
};
static_assert(sizeof(TelemetryPacket) == 24, "TelemetryPacket must be 24 bytes");
TelemetryPacket telem = {};
#endif

// ─── ImuPacket ────────────────────────────────────────────────────────────────
#ifdef FEATURE_IMU_STREAM
struct __attribute__((packed)) ImuPacket {
  int16_t  gyroY_mDPS;  // [0]
  int16_t  gyroZ_mDPS;  // [2]
  int16_t  accelX_mg;   // [4]
  int16_t  accelY_mg;   // [6]
  int16_t  accelZ_mg;   // [8]
  int8_t   moveX;       // [10]
  int8_t   moveY;       // [11]
  uint8_t  flags;       // [12]  bit0=idle bit1=singleTap bit2=doubleTap
  uint8_t  _pad;        // [13]
};
static_assert(sizeof(ImuPacket) == 14, "ImuPacket must be 14 bytes");
#endif

// ─── Tuning constants ─────────────────────────────────────────────────────────
const float ALPHA              = 0.96f;
const int   LOOP_RATE_MS       = 10;
const int   BIAS_SAMPLES       = 200;
const int   IDLE_FRAMES        = 150;
const unsigned long BATT_REPORT_MS       = 10000;
const unsigned long TELEMETRY_MS         = 1000;
const unsigned long HEARTBEAT_MS         = 2000;
const int           HEARTBEAT_DUR        = 30;
const unsigned long BOOT_SAFE_MS         = 5000;
#ifdef FEATURE_IMU_STREAM
  const unsigned long IMU_STREAM_RATE_MS = 50;  // 20 Hz max — 100 Hz overwhelms BLE conn interval
#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
#ifdef FEATURE_AUTO_RECAL
  const unsigned long AUTO_RECAL_MS        = 5UL * 60UL * 1000UL;
#endif

// ─── State ────────────────────────────────────────────────────────────────────
float angleX = 0, angleY = 0;
float accumX = 0, accumY = 0;
float biasGX = 0, biasGY = 0, biasGZ = 0;
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;
#endif

bool pendingCal   = false;
bool pendingReset = false;

ChargeStatus lastChargeStatus = CHGSTAT_DISCHARGING;

int           idleFrames  = 0;
unsigned long idleStartMs = 0;
unsigned long lastTime      = 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

// ─── I2C helpers ──────────────────────────────────────────────────────────────
void imuWriteReg(uint8_t reg, uint8_t val) {
  // LSM6DS3 is on Wire1 (internal I2C, SDA=P0.17, SCL=P0.16), NOT Wire (external pins 4/5)
  Wire1.beginTransmission(0x6A); Wire1.write(reg); Wire1.write(val); Wire1.endTransmission();
}
uint8_t imuReadReg(uint8_t reg) {
  Wire1.beginTransmission(0x6A); Wire1.write(reg); Wire1.endTransmission(false);
  Wire1.requestFrom((uint8_t)0x6A, (uint8_t)1);
  return Wire1.available() ? Wire1.read() : 0;
}

// ─── Temperature ──────────────────────────────────────────────────────────────
float readIMUTemp() {
  int16_t raw = (int16_t)((imuReadReg(REG_OUT_TEMP_H) << 8) | imuReadReg(REG_OUT_TEMP_L));
  return 25.0f + (float)raw / 256.0f;
}

// ─── Tap detection ────────────────────────────────────────────────────────────
#ifdef FEATURE_TAP_DETECTION
void setupTapDetection() {
  imuWriteReg(REG_CTRL1_XL,    0x60);  // ODR=416Hz, FS=±2g
  imuWriteReg(REG_TAP_CFG,     0x8E);  // INT_EN + LIR + TAP_Z/Y/X
  imuWriteReg(REG_TAP_THS_6D,  0x0C);  // threshold 750 mg (was 500 mg — too easy to false-trigger)
  imuWriteReg(REG_INT_DUR2,    0x7A);  // DUR=7(538ms), QUIET=2(19ms), SHOCK=2(38ms)
  imuWriteReg(REG_WAKE_UP_THS, 0x80);  // enable double-tap
  imuWriteReg(REG_MD1_CFG,     0x48);  // route taps to INT1
  Serial.println("[TAP] Engine configured — single=LEFT, double=RIGHT");
}

void processTaps(unsigned long now) {
  // Release held button after CLICK_HOLD_MS
  if (clickButtonDown && (now - clickDownMs >= CLICK_HOLD_MS)) {
    blehid.mouseButtonPress(clickButton, false);
    clickButtonDown = false; clickButton = 0;
  }
  if (clickButtonDown) return;  // Don't start a new click while one is held

  // The LSM6DS3 (with D_TAP_EN) already disambiguates at hardware level:
  //   SINGLE_TAP is only set after the DUR window expires with no second tap.
  //   DOUBLE_TAP is set immediately when the second tap arrives within DUR.
  // We trust this directly — no software delay needed.
  uint8_t tapSrc = imuReadReg(REG_TAP_SRC);
  if (!(tapSrc & 0x40)) return;  // TAP_IA not set — no event

  if (tapSrc & 0x10) {           // DOUBLE_TAP → right click
    Serial.println("[TAP] Double → RIGHT");
    blehid.mouseButtonPress(MOUSE_BUTTON_RIGHT, true);
    clickButton = MOUSE_BUTTON_RIGHT; clickButtonDown = true; clickDownMs = now;
    statRightClicks++;
  } else if (tapSrc & 0x20) {   // SINGLE_TAP → left click
    Serial.println("[TAP] Single → LEFT");
    blehid.mouseButtonPress(MOUSE_BUTTON_LEFT, true);
    clickButton = MOUSE_BUTTON_LEFT; clickButtonDown = true; clickDownMs = now;
    statLeftClicks++;
  }
}
#endif // FEATURE_TAP_DETECTION

// ─── Charge mode ──────────────────────────────────────────────────────────────
void applyChargeMode(ChargeMode mode) {
  switch (mode) {
    case CHARGE_OFF:  pinMode(PIN_HICHG, INPUT_PULLUP);                          break;
    case CHARGE_SLOW: pinMode(PIN_HICHG, OUTPUT); digitalWrite(PIN_HICHG, HIGH); break;
    case CHARGE_FAST: pinMode(PIN_HICHG, OUTPUT); digitalWrite(PIN_HICHG, LOW);  break;
  }
  const char* n[] = {"OFF (~0mA)", "SLOW (50mA)", "FAST (100mA)"};
  Serial.print("[CHG] "); Serial.println(n[mode]);
}

// ─── Config persistence ───────────────────────────────────────────────────────
void loadConfig() {
  InternalFS.begin();
  cfgFile.open(CONFIG_FILENAME, FILE_O_READ);
  if (cfgFile) {
    cfgFile.read(&cfg, sizeof(cfg)); cfgFile.close();
    if (cfg.magic != CONFIG_MAGIC) {
      cfg = CFG_DEFAULTS; Serial.println("[CFG] Defaults (bad magic)");
    } else {
      Serial.println("[CFG] Loaded from flash");
    }
  } else { cfg = CFG_DEFAULTS; Serial.println("[CFG] Defaults (no file)"); }
}

void saveConfig() {
  InternalFS.remove(CONFIG_FILENAME);
  cfgFile.open(CONFIG_FILENAME, FILE_O_WRITE);
  if (cfgFile) { cfgFile.write((uint8_t*)&cfg, sizeof(cfg)); cfgFile.close(); Serial.println("[CFG] Saved"); }
  else          { Serial.println("[CFG] ERROR: write failed"); }
}

// ─── ConfigBlob push ─────────────────────────────────────────────────────────
#ifdef FEATURE_CONFIG_SERVICE
void pushConfigBlob() {
  ConfigBlob b;
  b.sensitivity = cfg.sensitivity; b.deadZone = cfg.deadZone;
  b.accelStrength = cfg.accelStrength; b.curve = (uint8_t)cfg.curve;
  b.axisFlip = cfg.axisFlip; b.chargeMode = (uint8_t)cfg.chargeMode; b._pad = 0;
  cfgBlob.write((uint8_t*)&b, sizeof(b));
}
#endif

void factoryReset() {
  cfg = CFG_DEFAULTS; saveConfig();
  applyChargeMode(cfg.chargeMode);
  #ifdef FEATURE_CONFIG_SERVICE
    if (!safeMode) pushConfigBlob();
  #endif
  #ifdef FEATURE_TELEMETRY
    telem = {};
  #endif
  #ifdef FEATURE_TAP_DETECTION
    statLeftClicks = statRightClicks = 0;
  #endif
  Serial.println("[CFG] Factory reset complete");
}

// ─── BLE callbacks ────────────────────────────────────────────────────────────
#ifdef FEATURE_CONFIG_SERVICE
void onConfigBlobWrite(uint16_t h, BLECharacteristic* c, uint8_t* d, uint16_t l) {
  if (l != sizeof(ConfigBlob)) { Serial.println("[CFG] Bad blob length"); return; }
  ConfigBlob* b = (ConfigBlob*)d;
  cfg.sensitivity   = b->sensitivity;
  cfg.deadZone      = b->deadZone;
  cfg.accelStrength = b->accelStrength;
  if (b->curve     <= 2) cfg.curve      = (CurveType)b->curve;
  cfg.axisFlip = b->axisFlip;
  if (b->chargeMode <= 2) { cfg.chargeMode = (ChargeMode)b->chargeMode; applyChargeMode(cfg.chargeMode); }
  saveConfig();
  Serial.print("[CFG] Written — sens="); Serial.print(cfg.sensitivity,0);
  Serial.print(" dz="); Serial.print(cfg.deadZone,3);
  Serial.print(" curve="); Serial.print(cfg.curve);
  Serial.print(" chg="); Serial.println(cfg.chargeMode);
}

void onCommandWrite(uint16_t h, BLECharacteristic* c, uint8_t* d, uint16_t l) {
  if (l < 1) return;
  if (d[0] == 0x01) pendingCal   = true;
  if (d[0] == 0xFF) pendingReset = true;
}

#ifdef FEATURE_IMU_STREAM
void onImuStreamCccd(uint16_t conn_hdl, BLECharacteristic* chr, uint16_t value) {
  imuStreamEnabled = (value == BLE_GATT_HVX_NOTIFICATION);
  Serial.print("[STREAM] "); Serial.println(imuStreamEnabled ? "ON" : "OFF");
}
#endif
#endif // FEATURE_CONFIG_SERVICE

// ─── BLE service setup ────────────────────────────────────────────────────────
#ifdef FEATURE_CONFIG_SERVICE
void setupConfigService() {
  cfgService.begin();

  cfgBlob.setProperties(CHR_PROPS_READ | CHR_PROPS_WRITE);
  cfgBlob.setPermission(SECMODE_OPEN, SECMODE_OPEN);
  cfgBlob.setFixedLen(sizeof(ConfigBlob));
  cfgBlob.setWriteCallback(onConfigBlobWrite);
  cfgBlob.begin();
  pushConfigBlob();

  cfgCommand.setProperties(CHR_PROPS_WRITE);
  cfgCommand.setPermission(SECMODE_OPEN, SECMODE_OPEN);
  cfgCommand.setFixedLen(1);
  cfgCommand.setWriteCallback(onCommandWrite);
  cfgCommand.begin();

  #ifdef FEATURE_TELEMETRY
    cfgTelemetry.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
    cfgTelemetry.setPermission(SECMODE_OPEN, SECMODE_NO_ACCESS);
    cfgTelemetry.setFixedLen(sizeof(TelemetryPacket));
    cfgTelemetry.begin();
    cfgTelemetry.write((uint8_t*)&telem, sizeof(telem));
  #endif

  #ifdef FEATURE_IMU_STREAM
    cfgImuStream.setProperties(CHR_PROPS_NOTIFY);
    cfgImuStream.setPermission(SECMODE_OPEN, SECMODE_NO_ACCESS);
    cfgImuStream.setFixedLen(sizeof(ImuPacket));
    cfgImuStream.setCccdWriteCallback(onImuStreamCccd);
    cfgImuStream.begin();
  #endif

  // Print actual ATT table budget at runtime
  Serial.print("[BLE] ATT_TABLE_SIZE="); Serial.print(ATT_TABLE_SIZE);
  Serial.print(" | chars=2");
  #ifdef FEATURE_TELEMETRY
    Serial.print("+TELEM");
  #endif
  #ifdef FEATURE_IMU_STREAM
    Serial.print("+STREAM");
  #endif
  Serial.println();
}
#endif // FEATURE_CONFIG_SERVICE

// ─── Battery ──────────────────────────────────────────────────────────────────
#ifdef FEATURE_BATTERY_MONITOR
float readBatteryVoltage() {
  pinMode(PIN_VBAT_ENABLE, OUTPUT); digitalWrite(PIN_VBAT_ENABLE, LOW); delay(1);
  pinMode(PIN_VBAT_READ, INPUT);
  analogReference(AR_INTERNAL_3_0); analogReadResolution(12);
  for (int i=0; i<5; i++) { analogRead(PIN_VBAT_READ); delay(1); }
  int32_t raw=0; for (int i=0; i<16; i++) raw += analogRead(PIN_VBAT_READ); raw /= 16;
  digitalWrite(PIN_VBAT_ENABLE, HIGH);
  analogReference(AR_DEFAULT); analogReadResolution(10);
  return (raw / 4096.0f) * 3.0f * 2.0f;
}
int batteryPercent(float v) {
  return (int)constrain((v - BATT_EMPTY) / (BATT_FULL - BATT_EMPTY) * 100.f, 0, 100);
}

void updateBattery() {
  float v = readBatteryVoltage(); int pct = batteryPercent(v);
  bool chg = (digitalRead(PIN_CHG) == LOW);
  ChargeStatus status = chg ? (pct >= 99 ? CHGSTAT_FULL : CHGSTAT_CHARGING) : CHGSTAT_DISCHARGING;
  blebas.write(pct);
  lastChargeStatus = status;
  #ifdef FEATURE_TELEMETRY
    telem.chargeStatus = (uint8_t)status;
  #endif
  const char* st[] = {"discharging","charging","full"};
  Serial.print("[BATT] "); Serial.print(v,2); Serial.print("V ");
  Serial.print(pct); Serial.print("% "); Serial.println(st[status]);
  if (status == CHGSTAT_DISCHARGING && v < BATT_CRITICAL)
    for (int i=0; i<6; i++) { digitalWrite(LED_RED,LOW); delay(80); digitalWrite(LED_RED,HIGH); delay(80); }
}
#endif // FEATURE_BATTERY_MONITOR

// ─── Calibration ─────────────────────────────────────────────────────────────
void calibrateGyroBias() {
  Serial.println("[CAL] Hold still...");
  double sx=0, sy=0, sz=0;
  for (int i=0; i<BIAS_SAMPLES; i++) {
    sx += imu.readFloatGyroX(); sy += imu.readFloatGyroY(); sz += imu.readFloatGyroZ();
    digitalWrite(LED_BLUE, (i%20 < 10)); delay(5);
  }
  biasGX = (float)(sx/BIAS_SAMPLES);
  biasGY = (float)(sy/BIAS_SAMPLES);
  biasGZ = (float)(sz/BIAS_SAMPLES);
  calTempC = readIMUTemp();
  angleX = angleY = accumX = accumY = 0.0f;

  #ifdef FEATURE_TELEMETRY
    statRecalCount++;
    float bxr = biasGX*(PI/180.f), byr = biasGY*(PI/180.f), bzr = biasGZ*(PI/180.f);
    statBiasRms = sqrtf((bxr*bxr + byr*byr + bzr*bzr) / 3.0f);
  #endif

  digitalWrite(LED_BLUE, HIGH);
  Serial.print("[CAL] T="); Serial.print(calTempC,1);
  Serial.print("C  bias="); Serial.print(biasGX,4);
  Serial.print(","); Serial.print(biasGY,4);
  Serial.print(","); Serial.println(biasGZ,4);
}

// ─── Motion curve ─────────────────────────────────────────────────────────────
float applyCurve(float v) {
  switch (cfg.curve) {
    case CURVE_SQUARE: return (v >= 0 ? 1.f : -1.f) * v * v;
    case CURVE_SQRT:   return (v >= 0 ? 1.f : -1.f) * sqrtf(fabsf(v));
    default:           return v;
  }
}
float applyAcceleration(float d) { return d * (1.0f + fabsf(d) * cfg.accelStrength); }

// ─── Telemetry push ───────────────────────────────────────────────────────────
#ifdef FEATURE_TELEMETRY
void pushTelemetry(unsigned long now) {
  telem.uptimeSeconds = now / 1000;
  telem.tempCelsius   = cachedTempC;
  telem.biasRmsRadS   = statBiasRms;
  telem.recalCount    = statRecalCount;
  #ifdef FEATURE_TAP_DETECTION
    telem.leftClicks  = statLeftClicks;
    telem.rightClicks = statRightClicks;
  #endif
  cfgTelemetry.write ((uint8_t*)&telem, sizeof(telem));
  cfgTelemetry.notify((uint8_t*)&telem, sizeof(telem));
}
#endif

// ─── Advertising ─────────────────────────────────────────────────────────────
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_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); }
    }
  #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.begin(1, 0);
  Bluefruit.setTxPower(4);
  Bluefruit.setName(safeMode ? "IMU Mouse (safe)" : "IMU Mouse");
  Bluefruit.Periph.setConnInterval(12, 24);  // 15-30ms — less aggressive, prevents stream disconnect

  Wire1.begin();  // LSM6DS3 is on internal I2C bus (Wire1), must init before imu.begin()
  if (imu.begin() != 0) {
    Serial.println("[ERROR] IMU init failed");
    while(1) { digitalWrite(LED_RED, !digitalRead(LED_RED)); delay(100); }
  }
  Serial.println("[OK] IMU ready");

  #ifdef FEATURE_TAP_DETECTION
    setupTapDetection();
  #endif

  cachedTempC = readIMUTemp();

  #ifdef FEATURE_BATTERY_MONITOR
    updateBattery();
  #endif

  calibrateGyroBias();

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

  blehid.begin();

  #ifdef FEATURE_BATTERY_MONITOR
    blebas.begin(); blebas.write(100);
  #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
  Serial.println();

  bootStartMs = millis();
  lastTime = lastBattTime = lastHeartbeat = lastTelemetry = millis();
}

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

  if (pendingCal)   { pendingCal   = false; calibrateGyroBias(); }
  if (pendingReset) { pendingReset = false; factoryReset(); }

  // Heartbeat LED
  if (now - lastHeartbeat >= HEARTBEAT_MS) {
    lastHeartbeat = now;
    int led = Bluefruit.connected() ? LED_BLUE : LED_RED;
    digitalWrite(led, LOW); delay(HEARTBEAT_DUR); digitalWrite(led, HIGH);
  }

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

  #ifdef FEATURE_TAP_DETECTION
    processTaps(now);
  #endif

  if (now - lastTime < (unsigned long)LOOP_RATE_MS) return;
  float dt = (now - lastTime) / 1000.0f;
  lastTime = now;
  if (dt <= 0.0f || dt > 0.5f) return;

  cachedTempC = readIMUTemp();

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

  // Gyro reads with optional temperature compensation
  float gx, gy, gz;
  #ifdef FEATURE_TEMP_COMPENSATION
    float correction = TEMP_COMP_COEFF_DPS_C * (cachedTempC - calTempC);
    gx = (imu.readFloatGyroX() - biasGX - correction) * (PI/180.0f);
    gy = (imu.readFloatGyroY() - biasGY - correction) * (PI/180.0f);
    gz = (imu.readFloatGyroZ() - biasGZ - correction) * (PI/180.0f);
  #else
    gx = (imu.readFloatGyroX() - biasGX) * (PI/180.0f);
    gy = (imu.readFloatGyroY() - biasGY) * (PI/180.0f);
    gz = (imu.readFloatGyroZ() - biasGZ) * (PI/180.0f);
  #endif

  float ax = imu.readFloatAccelX();
  float ay = imu.readFloatAccelY();
  float az = imu.readFloatAccelZ();

  // Complementary filter
  angleX = ALPHA*(angleX + gx*dt) + (1.0f - ALPHA)*atan2f(ax, sqrtf(ay*ay + az*az));
  angleY = ALPHA*(angleY + gy*dt) + (1.0f - ALPHA)*atan2f(ay, sqrtf(ax*ax + az*az));

  float fGy = (fabsf(gy) > cfg.deadZone) ? gy : 0.0f;
  float fGz = (fabsf(gz) > cfg.deadZone) ? gz : 0.0f;

  bool moving = (fGy != 0.0f || fGz != 0.0f);
  if  (moving) { idleFrames = 0; idleStartMs = 0; }
  else         { idleFrames++; if (idleStartMs == 0) idleStartMs = now; }
  bool idle = (idleFrames >= IDLE_FRAMES);

  #ifdef FEATURE_AUTO_RECAL
    if (idle && idleStartMs != 0 && (now - idleStartMs >= AUTO_RECAL_MS)) {
      Serial.println("[AUTO-CAL] Long idle — recalibrating...");
      idleStartMs = 0; calibrateGyroBias(); return;
    }
  #endif

  int8_t  moveX = 0, moveY = 0;
  uint8_t flags = 0;

  if (idle) {
    accumX = accumY = 0.0f;
    flags |= 0x01;
  } else {
    float rawX = applyAcceleration(applyCurve(-fGz * cfg.sensitivity * dt));
    float rawY = applyAcceleration(applyCurve(-fGy * cfg.sensitivity * dt));
    if (cfg.axisFlip & 0x01) rawX = -rawX;
    if (cfg.axisFlip & 0x02) rawY = -rawY;
    accumX += rawX; accumY += rawY;
    moveX = (int8_t)constrain((int)accumX, -127, 127);
    moveY = (int8_t)constrain((int)accumY, -127, 127);
    accumX -= moveX; accumY -= moveY;
    if (Bluefruit.connected() && (moveX != 0 || moveY != 0)) blehid.mouseMove(moveX, moveY);
  }

  #ifdef FEATURE_IMU_STREAM
    if (!safeMode && imuStreamEnabled && Bluefruit.connected()
        && (now - lastImuStream >= IMU_STREAM_RATE_MS)) {
      lastImuStream = now;
      ImuPacket pkt;
      pkt.gyroY_mDPS = (int16_t)constrain(gy*(180.f/PI)*1000.f, -32000, 32000);
      pkt.gyroZ_mDPS = (int16_t)constrain(gz*(180.f/PI)*1000.f, -32000, 32000);
      pkt.accelX_mg  = (int16_t)constrain(ax*1000.f, -32000, 32000);
      pkt.accelY_mg  = (int16_t)constrain(ay*1000.f, -32000, 32000);
      pkt.accelZ_mg  = (int16_t)constrain(az*1000.f, -32000, 32000);
      pkt.moveX      = moveX;
      pkt.moveY      = moveY;
      pkt.flags      = flags;
      pkt._pad       = 0;
      cfgImuStream.notify((uint8_t*)&pkt, sizeof(pkt));
    }
  #endif

  #ifdef DEBUG
    Serial.print("T="); Serial.print(cachedTempC,1);
    Serial.print(" gy="); Serial.print(gy,3);
    Serial.print(" gz="); Serial.print(gz,3);
    Serial.print(" mx="); Serial.print(moveX);
    Serial.print(" my="); Serial.println(moveY);
  #endif
}