#include "madgwick.h"
#include "stdint.h"
#include "stdio.h"

#define MADGWICK_BETA           0.1f
#define MADGWICK_SAMPLE_RATE    100.0f
#define DEG2RAD                 3.14f/180.0f

madgwick_handle_t madgwick_handle;
madgwick_quat_data_t quat_data;

int main(){
    madgwick_cfg_t madgwick_cfg;
    madgwick_cfg.beta = MADGWICK_BETA;
    madgwick_cfg.sample_freq = MADGWICK_SAMPLE_RATE;
    madgwick_handle = madgwick_init(&madgwick_cfg);

    madgwick_update_6dof(madgwick_handle,
                             0.6 * DEG2RAD,
                             1 * DEG2RAD,
                             3 * DEG2RAD,
                             0.43,
                             0.56,
                             1.3);

    madgwick_get_quaternion(madgwick_handle, &quat_data);
    float roll = 180.0 / 3.14 * atan2(2 * (quat_data.q0 * quat_data.q1 + quat_data.q2 * quat_data.q3), 1 - 2 * (quat_data.q1 * quat_data.q1 + quat_data.q2 * quat_data.q2));
    float pitch = 180.0 / 3.14 * asin(2 * (quat_data.q0 * quat_data.q2 - quat_data.q3 * quat_data.q1));
    float yaw = 180.0 / 3.14 * atan2f(quat_data.q0 * quat_data.q3 + quat_data.q1 * quat_data.q2, 0.5f - quat_data.q2 * quat_data.q2 - quat_data.q3 * quat_data.q3);

    printf("Roll: %f\nPitch: %f\nYaw: %f\n", roll, pitch, yaw);

    return 0;
}