#include "attitude.h"
#include "mpu6050.h"
#include "variables.h"
#include <math.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#define RAD2DEG            (57.2957795f)
#define ACC_LSB_PER_G      (16384.0f)
#define GYRO_LSB_PER_DPS   (131.0f)
#define ALPHA              (0.98f)

static float s_pitch_rad = 0.0f;

void Attitude_SetZero(void)
{
    pitch_offset = pitch_deg;
}

void Attitude_Init(void)
{
    s_pitch_rad = 0.0f;
    pitch_deg = 0.0f;
    pitch_offset = 0.0f;
    mpu_online = (MPU6050_Init() == 0) ? 1U : 0U;
}

void Attitude_Update(float dt)
{
    MPU6050_Raw_t r;

    if (mpu_online == 0U)
    {
        return;
    }

    if (MPU6050_ReadRaw(&r) != 0)
    {
        mpu_online = 0U;
        return;
    }

    const float ax = (float)r.ax / ACC_LSB_PER_G;
    const float ay = (float)r.ay / ACC_LSB_PER_G;
    const float az = (float)r.az / ACC_LSB_PER_G;
    const float gyro_y_dps = (float)r.gy / GYRO_LSB_PER_DPS;

    const float pitch_acc = atan2f(-ax, sqrtf((ay * ay) + (az * az)));
    const float pitch_gyro = s_pitch_rad + ((gyro_y_dps * (float)M_PI) / 180.0f) * dt;

    s_pitch_rad = (ALPHA * pitch_gyro) + ((1.0f - ALPHA) * pitch_acc);
    pitch_deg = (s_pitch_rad * RAD2DEG) - pitch_offset;
}
