#pragma once

#include <stdint.h>

typedef struct
{
    uint8_t frac_bits;
    int32_t kp;
    int32_t ki;
    int32_t integral_x;
    int32_t integral_y;
    int32_t integral_z;
    int32_t q0;
    int32_t q1;
    int32_t q2;
    int32_t q3;
} mahony_filter_t;

void mahony_filter_init(mahony_filter_t *filter, uint8_t frac_bits);
void mahony_filter_reset(mahony_filter_t *filter);
void mahony_filter_set_gains(mahony_filter_t *filter, int32_t kp, int32_t ki);
void mahony_filter_update_imu(
    mahony_filter_t *filter,
    int32_t gx,
    int32_t gy,
    int32_t gz,
    int32_t ax,
    int32_t ay,
    int32_t az,
    int32_t dt);
void mahony_filter_get_quaternion(
    const mahony_filter_t *filter,
    int32_t *q0,
    int32_t *q1,
    int32_t *q2,
    int32_t *q3);
void mahony_filter_get_yaw_pitch_roll(
    const mahony_filter_t *filter,
    int32_t *yaw,
    int32_t *pitch,
    int32_t *roll);