#ifndef BNO055_H
#define BNO055_H

#include <stdint.h>
#include <stdbool.h>
#include "mpu6000.h"   /* IMUData */

/*
 * BNO055 NDOF IMU driver over I2C1 (shared bus — PB8=SCL, PB9=SDA).
 *
 * Issue #135: auto-detected alongside MPU6000. Acts as:
 *   PRIMARY  — when MPU6000 init fails (seamless fallback)
 *   AUGMENT  — when both present; BNO055 provides better NDOF-fused yaw
 *
 * I2C addresses probed: 0x28 (ADR=0, default) then 0x29 (ADR=1).
 * Chip-ID register 0x00 must read 0xA0.
 *
 * Operating mode: NDOF (0x0C) — 9DOF fusion with magnetometer.
 * Falls back to IMUPLUS (0x08, no mag) if mag calibration stalls.
 *
 * Calibration offsets are saved to/restored from STM32 RTC backup
 * registers (BKP0R–BKP6R = 28 bytes), identified by a magic word.
 * If valid offsets are present, bno055_is_ready() returns true
 * immediately after init. Otherwise, waits for gyro+accel cal ≥ 2.
 *
 * Temperature compensation is handled internally by the BNO055 silicon
 * (it compensates all three sensors continuously). bno055_temperature()
 * exposes the onboard thermometer reading for telemetry.
 *
 * Loop-rate note: BNO055 reads over I2C1 @100kHz take ~3ms, so the
 * main balance loop drops from ~1kHz (MPU6000/SPI) to ~250Hz when
 * BNO055 is active. 250Hz is sufficient for stable self-balancing.
 * PID gain tuning may be required when switching IMU sources.
 */

/* ---- Calibration status nibble masks (CALIB_STAT reg 0x35) ---- */
#define BNO055_CAL_SYS_MASK   0xC0u   /* bits [7:6] — overall system */
#define BNO055_CAL_GYR_MASK   0x30u   /* bits [5:4] — gyroscope */
#define BNO055_CAL_ACC_MASK   0x0Cu   /* bits [3:2] — accelerometer */
#define BNO055_CAL_MAG_MASK   0x03u   /* bits [1:0] — magnetometer */
/* Each field: 0=uncalibrated, 3=fully calibrated */

/*
 * bno055_init() — probe I2C1 for BNO055, reset, enter NDOF mode,
 * restore saved calibration offsets if present.
 * Requires i2c1_init() already called.
 * Returns 0 on success, -1 if not found.
 * Blocks ~750ms (POR + mode-switch settle).
 * Call BEFORE safety_init() (IWDG not yet running).
 */
int bno055_init(void);

/*
 * bno055_read(data) — fill IMUData from BNO055 NDOF fusion output.
 * Uses Euler angles for pitch/roll/yaw and gyro registers for pitch_rate.
 * Triggers one I2C burst read (~3ms at 100kHz).
 * Call from main loop balance gate (not every loop iteration).
 */
void bno055_read(IMUData *data);

/*
 * bno055_is_ready() — true when BNO055 is suitable for balance arming.
 * True immediately if offsets were restored from backup RAM.
 * Otherwise true once gyro calibration ≥ 2 and accel ≥ 2.
 */
bool bno055_is_ready(void);

/*
 * bno055_calib_status() — raw CALIB_STAT byte.
 * Use BNO055_CAL_*_MASK to extract individual sensor calibration levels.
 * Returned value is updated lazily on each bno055_read() call.
 */
uint8_t bno055_calib_status(void);

/*
 * bno055_temperature() — onboard temperature in °C (gyro source).
 * Updated once per second (every ~250 calls to bno055_read()).
 * Range: -40..+85°C. Use for telemetry reporting only.
 */
int8_t bno055_temperature(void);

/*
 * bno055_save_offsets() — write current calibration offsets to
 * STM32 RTC backup registers BKP0R–BKP6R (22 bytes + magic).
 * Call once after sys+acc+gyr calibration all reach level 3.
 * Returns true if successful, false if BNO055 not present.
 * Temporarily switches to CONFIGMODE — do NOT call while armed.
 */
bool bno055_save_offsets(void);

/*
 * bno055_restore_offsets() — read offsets from RTC backup registers
 * and write them to BNO055 hardware (in CONFIGMODE).
 * Called automatically by bno055_init().
 * Returns true if valid offsets found and applied.
 */
bool bno055_restore_offsets(void);

#endif /* BNO055_H */