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

extern LSM6DS3 imu;

// Sensor configuration used by the tracking pipeline.
// Gyro ±1000 dps (35 mdps/LSB): fine enough that quantisation sits below the
// sensor noise floor, while hand flicks never saturate. Accel ±4 g.
// Every sample is read from the hardware FIFO, so integration uses the sensor's
// own sample clock instead of loop timing, and nothing is lost when the loop stalls.
constexpr float IMU_ODR_HZ        = 208.0f;
constexpr float IMU_DT            = 1.0f / IMU_ODR_HZ;
constexpr float GYRO_DPS_PER_LSB  = 0.035f;
constexpr float GYRO_RAD_PER_LSB  = GYRO_DPS_PER_LSB * (PI / 180.0f);
constexpr float ACCEL_G_PER_LSB   = 0.000122f;

struct ImuRaw { int16_t gx, gy, gz, ax, ay, az; };

extern uint32_t imuFifoOverruns;
extern uint32_t imuI2cErrors;

void imuWriteReg(uint8_t reg, uint8_t val);
uint8_t imuReadReg(uint8_t reg);
void imuConfigure();
void imuFifoReset();
int  imuReadFifo(ImuRaw* out, int maxSamples);
float readIMUTemp();
void calibrateGyroBias();
float applyCurve(float v);
float applyAcceleration(float d);