#include "euler.h"
#include <math.h>
#include "helpler_funtions.h"
#include "quaternion.h"
#include "dcm.h"

inline void Euler_ByDcm(const float dcm[3][3], euler_angle_t *euler)
{
    const float eps = 1e-4f;
    /* 由于欧拉角描述姿态存在奇异点, 需要单独对奇异情况下人为定义角值 */

    /* 如果 y 轴非竖直朝上或下 */
    if (fabsf(dcm[2][1]) < 1.0f - eps)
    {
        euler->pitch = atanf(dcm[2][1] / sqrtf(1.0f - dcm[2][1] * dcm[2][1]));
        euler->yaw = atan2f(-dcm[0][1], dcm[1][1]);
        euler->roll = atan2f(-dcm[2][0], dcm[2][2]);
    }
    /* 如果 Y 轴竖直朝上或下 */
    else
    {
        euler->pitch = sign(dcm[2][1]) * 0.5f * M_PI;
        euler->roll = 0.0f;
        /* 此时人为定义 y 轴在水平面的投影反向为航向角  */
        euler->yaw = atan2f(dcm[0][2], -dcm[1][2]);
    }
}

 void Euler_ByQuaternion(const float Q[4], euler_angle_t *euler)
{
    float dcm[3][3];
    DCM_ByQuaternion(Q, dcm);
    Euler_ByDcm(dcm, euler);
}