#ifndef __MPU6050_H
#define __MPU6050_H

#include "stm32f1xx_hal.h"

#define MPU6050_ADDR            0x68
#define MPU6050_ADDR_WRITE      (MPU6050_ADDR << 1)
#define MPU6050_ADDR_READ       ((MPU6050_ADDR << 1) | 0x01)

/* Register addresses */
#define MPU6050_REG_SMPLRT_DIV  0x19
#define MPU6050_REG_CONFIG      0x1A
#define MPU6050_REG_GYRO_CONFIG 0x1B
#define MPU6050_REG_ACCEL_CONFIG 0x1C
#define MPU6050_REG_ACCEL_XOUT_H 0x3B
#define MPU6050_REG_GYRO_XOUT_H 0x43
#define MPU6050_REG_PWR_MGMT_1  0x6B
#define MPU6050_REG_WHO_AM_I    0x75

/* Scale factors */
#define ACCEL_SCALE_2G      16384.0f
#define GYRO_SCALE_250DPS   131.0f

typedef struct {
    int16_t ax_raw, ay_raw, az_raw;
    int16_t gx_raw, gy_raw, gz_raw;
    float ax, ay, az;           /* g */
    float gx, gy, gz;           /* deg/s */
} MPU6050_Data;

typedef struct {
    float pitch;                /* deg, forward/backward tilt */
    float roll;                 /* deg, left/right tilt */
} Attitude;

/* Initialize MPU6050 (I2C) */
uint8_t MPU6050_Init(I2C_HandleTypeDef *hi2c);

/* Read raw sensor data */
uint8_t MPU6050_ReadRaw(I2C_HandleTypeDef *hi2c, MPU6050_Data *data);

/* Read and convert to engineering units */
uint8_t MPU6050_ReadScaled(I2C_HandleTypeDef *hi2c, MPU6050_Data *data);

/* Complementary filter: fuse gyro + accel into attitude */
void Attitude_Complementary(Attitude *att, const MPU6050_Data *sensor, float dt, float alpha);

/* Mahony AHRS update (quaternion-based) */
void Attitude_MahonyUpdate(Attitude *att, const MPU6050_Data *sensor, float dt);

/* Calibrate gyro bias (call when car is stationary) */
void MPU6050_CalibrateGyro(I2C_HandleTypeDef *hi2c, float *offset_x, float *offset_y, float *offset_z, uint16_t samples);

#endif /* __MPU6050_H */