/*
 * 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, ~10 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
 * ================================================================
 */

#include "config.h"
#include "imu.h"
#include "ble_config.h"
#include "battery.h"
#include "tap.h"
#include <bluefruit.h>
#include <Adafruit_LittleFS.h>
#include <InternalFileSystem.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

// ─── 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.060f, 0.08f, CURVE_LINEAR, 0x00, CHARGE_SLOW,
  /*tapThreshold=*/12, /*tapAction=*/TAP_ACTION_LEFT, /*tapKey=*/0, /*tapMod=*/0,
  /*jerkThreshold=*/2000.0f
};

// ─── Telemetry definition ─────────────────────────────────────────────────────
#ifdef FEATURE_TELEMETRY
TelemetryPacket telem = {};
#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       = 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 = 100;
#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

// ─── Global state definitions ─────────────────────────────────────────────────
float angleX = 0, angleY = 0;
float accumX = 0, accumY = 0;
// Low-pass filtered gravity estimate in device frame (for roll-independent axis projection)
float gravX = 0, gravY = 0, gravZ = 1.0f;
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;
  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

uint32_t loopStalls = 0;          // loop iterations where dt > 20ms (behind schedule)
bool pendingCal   = false;
bool pendingReset = false;

// ── Jerk-based shock detection — freeze cursor during tap impacts ────────────
// Jerk = da/dt (rate of change of acceleration).  Normal mouse rotation produces
// smooth accel changes (low jerk); a tap is a sharp impulse (very high jerk).
// This cleanly separates taps from any intentional motion regardless of speed.
unsigned long shockFreezeUntil = 0;
float prevAx = 0, prevAy = 0, prevAz = 0;     // previous frame's accel for Δa
const unsigned long SHOCK_FREEZE_MS = 80;       // hold freeze after last spike

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

bool safeMode         = false;
bool bootCountCleared = false;

// ─── 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(16, 32);  // 20-40ms — wider interval reduces SoftDevice TX stalls

  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); }  // fault: red rapid blink
  }
  Serial.println("[OK] IMU ready");

  #ifdef FEATURE_TAP_DETECTION
    setupTapDetection();
  #endif

  cachedTempC = readIMUTemp();

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

  calibrateGyroBias();
  // Seed previous-accel for jerk detection so first frame doesn't spike
  prevAx = imu.readFloatAccelX(); prevAy = imu.readFloatAccelY(); prevAz = imu.readFloatAccelZ();

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

  // Serial commands: 'c' = calibrate, 'r' = factory reset
  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 (pendingCal)   { pendingCal = false; calibrateGyroBias(); prevAx = imu.readFloatAccelX(); prevAy = imu.readFloatAccelY(); prevAz = imu.readFloatAccelZ(); }
  if (pendingReset) { pendingReset = false; factoryReset(); }

  // Heartbeat LED
  if (now - lastHeartbeat >= HEARTBEAT_MS) {
    lastHeartbeat = now;
    int led = Bluefruit.connected() ? LED_BLUE : LED_GREEN;  // blue=BT connected, green=advertising
    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;
  // Threshold 50ms: intentional heartbeat blink (30ms) won't false-trigger;
  // real SoftDevice stalls (100ms+) and unexpected delays still get flagged.
  if (dt > 0.050f) { loopStalls++; Serial.print("[STALL] dt="); Serial.print(dt*1000.f,1); Serial.print("ms stalls="); Serial.println(loopStalls); }

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

  // ── Jerk-based shock detection — freeze cursor during tap impacts ────────
  // Jerk = da/dt.  Normal rotation = smooth accel changes (low jerk);
  // a tap is a sharp impulse (very high jerk).
  float jx = (ax - prevAx) / dt, jy = (ay - prevAy) / dt, jz = (az - prevAz) / dt;
  float jerkSq = jx*jx + jy*jy + jz*jz;
  prevAx = ax; prevAy = ay; prevAz = az;
  bool shocked = (jerkSq > cfg.jerkThreshold) || (now < shockFreezeUntil);
  if (jerkSq > cfg.jerkThreshold) shockFreezeUntil = now + SHOCK_FREEZE_MS;

  // Complementary filter — gx=pitch axis, gz=yaw axis on this board layout
  // During shock: gyro-only integration to avoid accel spike corrupting angles
  if (shocked) {
    angleX += gx * dt;
    angleY += gz * dt;
  } else {
    angleX = ALPHA*(angleX + gx*dt) + (1.0f - ALPHA)*atan2f(ax, sqrtf(ay*ay + az*az));
    angleY = ALPHA*(angleY + gz*dt) + (1.0f - ALPHA)*atan2f(ay, sqrtf(ax*ax + az*az));
  }

  // ── Gravity-based axis decomposition ──────────────────────────────────────
  // Low-pass filter accel to get a stable gravity estimate in device frame.
  // This lets us project angular velocity onto world-aligned axes regardless
  // of how the device is rolled.  Device forward (pointing) axis = X.
  // Confirmed by diagnostics: GX=roll, GY=nod, GZ=pan in user's hold.
  // Skip update during shock to protect the gravity estimate from tap spikes.
  const float GRAV_LP = 0.05f;
  if (!shocked) {
    gravX += GRAV_LP * (ax - gravX);
    gravY += GRAV_LP * (ay - gravY);
    gravZ += GRAV_LP * (az - gravZ);
  }

  float gN = sqrtf(gravX*gravX + gravY*gravY + gravZ*gravZ);
  if (gN < 0.3f) gN = 1.0f;
  float gnx = gravX/gN, gny = gravY/gN, gnz = gravZ/gN;

  // Screen-right = cross(forward, up) = cross([1,0,0], [gnx,gny,gnz])
  //              = [0, -gnz, gny]
  float ry = -gnz, rz = gny;
  float rN = sqrtf(ry*ry + rz*rz);
  if (rN < 0.01f) { ry = -1.0f; rz = 0.0f; rN = 1.0f; }
  ry /= rN; rz /= rN;

  // Yaw  (cursor X) = angular velocity component around gravity (vertical)
  // Pitch (cursor Y) = angular velocity component around screen-right
  float yawRate   = gx*gnx + gy*gny + gz*gnz;
  float pitchRate = -(gy*ry + gz*rz);

  // Projected rates amplify residual gyro bias (especially GY drift on pitch axis).
  // Use a wider dead zone for pitch to prevent constant cursor drift at rest.
  float fYaw   = (fabsf(yawRate)   > cfg.deadZone)       ? yawRate   : 0.0f;
  float fPitch = (fabsf(pitchRate) > cfg.deadZone * 3.0f) ? pitchRate : 0.0f;

  // DIAG: print every 500ms to debug gravity projection — remove when confirmed
  #ifdef DEBUG
  { static unsigned long lastDiag = 0;
    if (now - lastDiag >= 500) { lastDiag = now;
      Serial.print("[PROJ] grav="); Serial.print(gnx,2); Serial.print(","); Serial.print(gny,2); Serial.print(","); Serial.print(gnz,2);
      Serial.print(" R="); Serial.print(ry,2); Serial.print(","); Serial.print(rz,2);
      Serial.print(" gyro="); Serial.print(gx,2); Serial.print(","); Serial.print(gy,2); Serial.print(","); Serial.print(gz,2);
      Serial.print(" yaw="); Serial.print(yawRate,3); Serial.print(" pitch="); Serial.println(pitchRate,3);
    }
  }
  #endif

  bool moving = (fPitch != 0.0f || fYaw != 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(); prevAx = imu.readFloatAccelX(); prevAy = imu.readFloatAccelY(); prevAz = imu.readFloatAccelZ(); return;
    }
  #endif

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

  if (shocked) {
    // Shock freeze — discard accumulated sub-pixel motion and suppress output
    accumX = accumY = 0.0f;
    flags |= 0x08;  // bit3 = shock freeze active
  } else if (idle) {
    accumX = accumY = 0.0f;
    flags |= 0x01;
  } else {
    float rawX = applyAcceleration(applyCurve(-fYaw   * cfg.sensitivity * dt));
    float rawY = applyAcceleration(applyCurve(-fPitch * 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;

      if (now < streamBackoffUntil) {
        // Backing off — host TX buffer congested, skip to avoid 100ms block
      } else {
        ImuPacket pkt;
        pkt.gyroX_mDPS = (int16_t)constrain(gx*(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;
        if (cfgImuStream.notify((uint8_t*)&pkt, sizeof(pkt))) {
          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;
      }
    }
  #endif
}