#pragma once
#include "config.h"
#include "imu.h"

// Tracking pipeline, run once per IMU sample:
//   bias removal -> gravity estimate (gyro-propagated, accel-corrected)
//   -> roll compensation (cursor follows world yaw/pitch however the remote is twisted)
//   -> speed-adaptive low-pass (heavy at rest, none when moving fast)
//   -> radial soft dead zone -> curve/acceleration on speed (direction preserved)
//   -> sub-count accumulator drained by the HID report scheduler.

struct MotionDebug {
  float yaw, pitch;      // roll-compensated rates, rad/s (unfiltered)
  float speed;           // smoothed speed after dead zone, rad/s
  float up[3];           // gravity-up unit vector, device frame
  float rollDeg;         // twist around the pointing axis
  float bias[3];         // rad/s
  bool  biasValid;
  bool  still;           // last stillness window passed
  uint32_t biasUpdates;
  uint32_t samples;
};

void motionReset();
bool motionApplyCalibration(const float bias[3], const float accel[3], bool still);
void motionProcess(const ImuRaw& s, unsigned long nowMs);
bool motionTakeCounts(int8_t& dx, int8_t& dy);
void motionReturnCounts(int8_t dx, int8_t dy);
bool motionConsumeMoved();      // true if any sample since the last call was above the idle rate
void motionStartTest();         // synthetic right/left/down/up sweep for end-to-end testing
bool motionTestActive();
const MotionDebug& motionDebug();