/*
 * IMU BLE Mouse — Seeed XIAO nRF52840 Sense  (v3.3)
 * ================================================================
 * Changes vs v3.2:
 *   - 10 characteristics → 4 (fixes SoftDevice RAM overflow)
 *   - configAttrTableSize reduced to 1024
 *   - All config params merged into one 16-byte ConfigBlob (0x1235)
 *   - chargeStatus merged into TelemetryPacket._pad (0x1237)
 *   - ImuStream (0x1238) and Command (0x1236) unchanged
 *   - Boot-loop detection retained
 *
 * ── BLE Config Service (UUID 0x1234) ───────────────────────────────
 *   UUID   | Len | R/W/N | Description
 *   ───────|─────|───────|──────────────────────────────────────────
 *   0x1235 | 16  |  R/W  | ConfigBlob — all settings in one write
 *   0x1236 |  1  |    W  | Command: 0x01=Cal  0xFF=FactoryReset
 *   0x1237 | 24  |  R/N  | TelemetryPacket, notified 1 Hz
 *   0x1238 | 14  |    N  | ImuPacket, notified ~100 Hz
 *
 * ── ConfigBlob (16 bytes, little-endian) ────────────────────────────
 *   float   sensitivity    [0]
 *   float   deadZone       [4]
 *   float   accelStrength  [8]
 *   uint8_t curve          [12]   0=LINEAR 1=SQUARE 2=SQRT
 *   uint8_t axisFlip       [13]   bit0=flipX bit1=flipY
 *   uint8_t chargeMode     [14]   0=OFF 1=50mA 2=100mA
 *   uint8_t _pad           [15]
 *
 * ── TelemetryPacket (24 bytes, little-endian) ────────────────────────
 *   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]   0=discharging 1=charging 2=full
 *   uint8_t  _pad          [23]
 *
 * ── ImuPacket (14 bytes, little-endian) ─────────────────────────────
 *   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]
 */

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

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

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

// ─── BLE Standard Services ────────────────────────────────────────────────────
BLEDis         bledis;
BLEHidAdafruit blehid;
BLEBas         blebas;

// ─── BLE Config Service — 4 characteristics only ─────────────────────────────
BLEService        cfgService   (0x1234);
BLECharacteristic cfgBlob      (0x1235);  // ConfigBlob  R/W  16 bytes
BLECharacteristic cfgCommand   (0x1236);  // Command       W   1 byte
BLECharacteristic cfgTelemetry (0x1237);  // Telemetry   R/N  24 bytes  1 Hz
BLECharacteristic cfgImuStream (0x1238);  // ImuStream     N  14 bytes ~100 Hz

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

#define LSM6DS3_CTRL1_XL    0x10
#define LSM6DS3_TAP_CFG     0x58
#define LSM6DS3_TAP_THS_6D  0x59
#define LSM6DS3_INT_DUR2    0x5A
#define LSM6DS3_WAKE_UP_THS 0x5B
#define LSM6DS3_MD1_CFG     0x5E
#define LSM6DS3_TAP_SRC     0x1C
#define LSM6DS3_OUT_TEMP_L  0x20
#define LSM6DS3_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     0xDEAD1238UL   // bumped — struct layout unchanged but version tag updated

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 ───────────────────────────────────────────────────────────────────
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 (what goes over BLE — no magic field) ─────────────────────────
struct __attribute__((packed)) ConfigBlob {
  float   sensitivity;
  float   deadZone;
  float   accelStrength;
  uint8_t curve;
  uint8_t axisFlip;
  uint8_t chargeMode;
  uint8_t _pad;
};
static_assert(sizeof(ConfigBlob) == 16, "ConfigBlob must be 16 bytes");

// ─── TelemetryPacket ──────────────────────────────────────────────────────────
struct __attribute__((packed)) TelemetryPacket {
  uint32_t uptimeSeconds;
  uint32_t leftClicks;
  uint32_t rightClicks;
  float    tempCelsius;
  float    biasRmsRadS;
  uint16_t recalCount;
  uint8_t  chargeStatus;   // replaces old _pad — no extra characteristic needed
  uint8_t  _pad;
};
static_assert(sizeof(TelemetryPacket) == 24, "TelemetryPacket must be 24 bytes");

// ─── ImuPacket ────────────────────────────────────────────────────────────────
struct __attribute__((packed)) ImuPacket {
  int16_t  gyroY_mDPS;
  int16_t  gyroZ_mDPS;
  int16_t  accelX_mg;
  int16_t  accelY_mg;
  int16_t  accelZ_mg;
  int8_t   moveX;
  int8_t   moveY;
  uint8_t  flags;
  uint8_t  _pad;
};
static_assert(sizeof(ImuPacket) == 14, "ImuPacket must be 14 bytes");

// ─── Tuning constants ─────────────────────────────────────────────────────────
const float ALPHA                    = 0.96f;
const int   LOOP_RATE_MS             = 10;
const int   BIAS_SAMPLES             = 200;
const int   IDLE_FRAMES              = 150;
const float TEMP_COMP_COEFF_DPS_C    = 0.004f;
const unsigned long AUTO_RECAL_MS    = 5UL * 60UL * 1000UL;
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 CLICK_HOLD_MS    = 40;
const unsigned long DOUBLE_TAP_WINDOW_MS = 400;
const unsigned long BOOT_SAFE_MS     = 5000;
const float BATT_FULL                = 4.20f;
const float BATT_EMPTY               = 3.00f;
const float BATT_CRITICAL            = 3.10f;

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

TelemetryPacket telem = {};

bool    imuStreamEnabled = false;
bool    tapPending       = false;
bool    clickButtonDown  = false;
uint8_t clickButton      = 0;
unsigned long tapSeenMs   = 0;
unsigned long clickDownMs = 0;

bool pendingCal   = false;
bool pendingReset = false;

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;

// ─── I2C helpers ──────────────────────────────────────────────────────────────
void imuWriteReg(uint8_t reg, uint8_t val) {
  Wire.beginTransmission(0x6A); Wire.write(reg); Wire.write(val); Wire.endTransmission();
}
uint8_t imuReadReg(uint8_t reg) {
  Wire.beginTransmission(0x6A); Wire.write(reg); Wire.endTransmission(false);
  Wire.requestFrom((uint8_t)0x6A, (uint8_t)1);
  return Wire.available() ? Wire.read() : 0;
}

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

// ─── Tap detection ────────────────────────────────────────────────────────────
void setupTapDetection() {
  imuWriteReg(LSM6DS3_CTRL1_XL,    0x60);
  imuWriteReg(LSM6DS3_TAP_CFG,     0x8E);
  imuWriteReg(LSM6DS3_TAP_THS_6D,  0x08);
  imuWriteReg(LSM6DS3_INT_DUR2,    0x77);
  imuWriteReg(LSM6DS3_WAKE_UP_THS, 0x80);
  imuWriteReg(LSM6DS3_MD1_CFG,     0x48);
  Serial.println("[TAP] Engine configured — single=LEFT, double=RIGHT");
}

// ─── 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"); }
}

// Push current config as a ConfigBlob to the BLE characteristic
void pushConfigBlob() {
  ConfigBlob blob;
  blob.sensitivity   = cfg.sensitivity;
  blob.deadZone      = cfg.deadZone;
  blob.accelStrength = cfg.accelStrength;
  blob.curve         = (uint8_t)cfg.curve;
  blob.axisFlip      = cfg.axisFlip;
  blob.chargeMode    = (uint8_t)cfg.chargeMode;
  blob._pad          = 0;
  cfgBlob.write((uint8_t*)&blob, sizeof(blob));
}

void factoryReset() {
  cfg = CFG_DEFAULTS; saveConfig();
  applyChargeMode(cfg.chargeMode);
  if (!safeMode) pushConfigBlob();
  telem = {};
  Serial.println("[CFG] Factory reset complete");
}

// ─── BLE callbacks ────────────────────────────────────────────────────────────
// Single callback handles the whole config blob
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] Blob written — sens="); Serial.print(cfg.sensitivity);
  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;
}

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

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

  // ConfigBlob — R/W 16 bytes
  cfgBlob.setProperties(CHR_PROPS_READ | CHR_PROPS_WRITE);
  cfgBlob.setPermission(SECMODE_OPEN, SECMODE_OPEN);
  cfgBlob.setFixedLen(sizeof(ConfigBlob));
  cfgBlob.setWriteCallback(onConfigBlobWrite);
  cfgBlob.begin();
  pushConfigBlob();

  // Command — W 1 byte
  cfgCommand.setProperties(CHR_PROPS_WRITE);
  cfgCommand.setPermission(SECMODE_OPEN, SECMODE_OPEN);
  cfgCommand.setFixedLen(1);
  cfgCommand.setWriteCallback(onCommandWrite);
  cfgCommand.begin();

  // Telemetry — R/N 24 bytes
  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));

  // ImuStream — N 14 bytes
  cfgImuStream.setProperties(CHR_PROPS_NOTIFY);
  cfgImuStream.setPermission(SECMODE_OPEN, SECMODE_NO_ACCESS);
  cfgImuStream.setFixedLen(sizeof(ImuPacket));
  cfgImuStream.setCccdWriteCallback(onImuStreamCccd);
  cfgImuStream.begin();
}

// ─── Battery ──────────────────────────────────────────────────────────────────
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);
  // chargeStatus is now pushed via telemetry packet — no separate characteristic
  telem.chargeStatus = (uint8_t)status;
  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); }
}

// ─── 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;
  telem.recalCount++;
  float bxr = biasGX*(PI/180.f), byr = biasGY*(PI/180.f), bzr = biasGZ*(PI/180.f);
  telem.biasRmsRadS = sqrtf((bxr*bxr + byr*byr + bzr*bzr) / 3.0f);
  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); }

// ─── Tap state machine ────────────────────────────────────────────────────────
void processTaps(unsigned long now) {
  if (clickButtonDown && (now - clickDownMs >= CLICK_HOLD_MS)) {
    blehid.mouseButtonPress(clickButton, false);
    clickButtonDown = false; clickButton = 0;
  }
  uint8_t tapSrc = imuReadReg(LSM6DS3_TAP_SRC);
  bool singleTap = (tapSrc & 0x20) != 0;
  bool doubleTap = (tapSrc & 0x10) != 0;
  bool tapEvent  = (tapSrc & 0x40) != 0;
  if (!tapEvent) {
    if (tapPending && (now - tapSeenMs >= DOUBLE_TAP_WINDOW_MS)) {
      tapPending = false;
      if (!clickButtonDown) {
        Serial.println("[TAP] Single → LEFT");
        blehid.mouseButtonPress(MOUSE_BUTTON_LEFT, true);
        clickButton = MOUSE_BUTTON_LEFT; clickButtonDown = true; clickDownMs = now;
        telem.leftClicks++;
      }
    }
    return;
  }
  if (doubleTap && !clickButtonDown) {
    tapPending = false;
    Serial.println("[TAP] Double → RIGHT");
    blehid.mouseButtonPress(MOUSE_BUTTON_RIGHT, true);
    clickButton = MOUSE_BUTTON_RIGHT; clickButtonDown = true; clickDownMs = now;
    telem.rightClicks++;
    return;
  }
  if (singleTap && !tapPending && !clickButtonDown) { tapPending = true; tapSeenMs = now; }
}

// ─── Telemetry ────────────────────────────────────────────────────────────────
void pushTelemetry(unsigned long now) {
  telem.uptimeSeconds = now / 1000;
  telem.tempCelsius   = cachedTempC;
  // telem.chargeStatus is updated in updateBattery()
  cfgTelemetry.write ((uint8_t*)&telem, sizeof(telem));
  cfgTelemetry.notify((uint8_t*)&telem, sizeof(telem));
}

// ─── 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);
  Bluefruit.Advertising.addService(blebas);
  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 ───────────────────────────────────────────────────
  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 detected — safe mode");
    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); }
  }

  loadConfig();
  applyChargeMode(cfg.chargeMode);

  // 1024 is sufficient for 4 characteristics (was 3072/2048 — both overflowed)
  Bluefruit.configAttrTableSize(1024);

  Bluefruit.begin(1, 0);
  Bluefruit.setTxPower(4);
  Bluefruit.setName(safeMode ? "IMU Mouse (safe)" : "IMU Mouse");
  Bluefruit.Periph.setConnInterval(6, 12);

  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");

  setupTapDetection();
  cachedTempC = readIMUTemp();
  updateBattery();
  calibrateGyroBias();

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

  blehid.begin();
  blebas.begin(); blebas.write(100);

  if (!safeMode) {
    setupConfigService();
    Serial.println("[OK] Config service started (4 characteristics)");
  } else {
    Serial.println("[SAFE] Config service skipped — basic mouse only");
  }

  startAdvertising();
  Serial.print("[OK] Advertising as '");
  Serial.print(safeMode ? "IMU Mouse (safe)" : "IMU Mouse");
  Serial.println("'");

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

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

  if (!bootCountCleared && (now - bootStartMs >= BOOT_SAFE_MS)) {
    bootCount = 0; bootCountCleared = true;
    Serial.println("[BOOT] Stable — boot counter cleared");
  }

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

  if (now - lastBattTime >= BATT_REPORT_MS) { lastBattTime = now; updateBattery(); }

  processTaps(now);

  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();

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

  float tempDelta  = cachedTempC - calTempC;
  float correction = TEMP_COMP_COEFF_DPS_C * tempDelta;

  float gx = (imu.readFloatGyroX() - biasGX - correction) * (PI/180.0f);
  float gy = (imu.readFloatGyroY() - biasGY - correction) * (PI/180.0f);
  float gz = (imu.readFloatGyroZ() - biasGZ - correction) * (PI/180.0f);

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

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

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

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

  if (!safeMode && imuStreamEnabled && Bluefruit.connected()) {
    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));
  }

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