#pragma once

#include <math.h>

/**
 * @brief 通过四元数构造旋转矩阵
 * 
 * @param Q 
 * @param dcm 
 */
static inline void DCM_ByQuaternion(const float Q[4], float dcm[3][3])
{
    float Q0 = Q[0], Q1 = Q[1], Q2 = Q[2], Q3 = Q[3];
    float Q0s = Q[0] * Q[0];
    float Q1s = Q[1] * Q[1];
    float Q2s = Q[2] * Q[2];
    float Q3s = Q[3] * Q[3];
    dcm[0][0] = Q0s + Q1s - Q2s - Q3s;
    dcm[0][1] = 2.0f * (Q1 * Q2 - Q0 * Q3);
    dcm[0][2] = 2.0f * (Q0 * Q2 + Q1 * Q3);
    dcm[1][0] = 2.0f * (Q1 * Q2 + Q0 * Q3);
    dcm[1][1] = Q0s - Q1s + Q2s - Q3s;
    dcm[1][2] = 2.0f * (Q2 * Q3 - Q0 * Q1);
    dcm[2][0] = 2.0f * (Q1 * Q3 - Q0 * Q2);
    dcm[2][1] = 2.0f * (Q0 * Q1 + Q2 * Q3);
    dcm[2][2] = Q0s - Q1s - Q2s + Q3s;
}

/**
 * @brief 通过欧拉角构造旋转矩阵
 *  欧拉角定义顺序必须为 航向->俯仰->横滚 rad
 * @param yaw 航向角
 * @param pitch 俯仰角
 * @param roll 横滚角
 * @param dcm 旋转矩阵, TbN阵
 */
static inline void DCM_ByEuler(float yaw, float pitch, float roll, float dcm[3][3])
{
    dcm[0][0] = cosf(roll) * cosf(yaw) - sinf(pitch) * sinf(roll) * sinf(yaw);
    dcm[0][1] = -cosf(pitch) * sinf(yaw);
    dcm[0][2] = sinf(roll) * cosf(yaw) + cosf(roll) * sinf(pitch) * sinf(yaw);

    dcm[1][0] = cosf(roll) * sinf(yaw) + sinf(roll) * sinf(pitch) * cosf(yaw);
    dcm[1][1] = cosf(pitch) * cosf(yaw);
    dcm[1][2] = sinf(roll) * sinf(yaw) - cosf(roll) * sinf(pitch) * cosf(yaw);

    dcm[2][0] = -sinf(roll) * cosf(pitch);
    dcm[2][1] = sinf(pitch);
    dcm[2][2] = cosf(roll) * cosf(pitch);
}

/**
 * @brief 使用 frame1 xyz轴向量 在 frame2 下的坐标，构建旋转矩阵
 *  x, y, z 必须为互相正交的单位向量
 * @param x frame1 x 轴在 frame2 系下的单位坐标
 * @param y frame1 y 轴在 frame2 系下的单位坐标
 * @param z frame1 z 轴在 frame2 系下的单位坐标
 */
static inline void DCM_ByXYZAxis(const float X[3], const float Y[3], const float Z[3], float dcm[3][3])
{
    for (int i = 0; i < 3; ++i)
    {
        dcm[i][0] = X[i];
        dcm[i][1] = Y[i];
        dcm[i][2] = Z[i];
    }
}