#include "quaternion.h"
#include "matrix.h"
#include "float.h"
#include "math.h"
#include "helpler_funtions.h"
///\ @brief 四元数的简单构造
void Quaternion(float q[4])
{
    q[0] = 1.0f;
    for (int i = 1; i < 4; ++i)
    {
        q[i] = 0.0f;
    }
}

///\ @brief 四元数归一化为单位四元数
void Quaternion_Normalize(float Q[4])
{
    float q_norm = Vector_GetNorm(Q, 4);
    Vector_DotProductRealNum(Q, 1.0F / q_norm, 4);
}

/**
 * @brief 四元数规范化 保证实部不小于0
 *
 */
void Quaternion_Canonicalize(float Q[4])
{
    for (int i = 0; i < 4; ++i)
    {
        if (fabsf(Q[i]) > FLT_EPSILON)
        {
            Vector_DotProductRealNum(Q, sign(Q[i]), 4);
            return;
        }
    }
}

/**
 * @brief 使用旋转矩阵构造四元数
 *
 * @param Q 四元数
 * @param R 旋转矩阵
 */
void Quaternion_ByDcm(const float R[3][3], float Q[4])
{
    float t = Matrix_GetTrace(&R[0][0], 3);

    if (t > 0.0f)
    {
        t = sqrtf(1.0f + t);
        Q[0] = 0.5f * t;
        t = 0.5f / t;
        Q[1] = (R[2][1] - R[1][2]) * t;
        Q[2] = (R[0][2] - R[2][0]) * t;
        Q[3] = (R[1][0] - R[0][1]) * t;
    }
    else if (R[0][0] > R[0][1] && R[0][0] > R[2][2])
    {
        t = sqrtf(1.0f + R[0][0] - R[1][1] - R[2][2]);
        Q[1] = 0.5F * t;
        t = 0.5f / t;
        Q[0] = (R[2][1] - R[1][2]) * t;
        Q[2] = (R[1][0] + R[0][1]) * t;
        Q[3] = (R[0][2] + R[2][0]) * t;
    }
    else if (R[1][1] > R[2][2])
    {
        t = sqrtf(1.0f - R[0][0] + R[1][1] - R[2][2]);
        Q[2] = 0.5F * t;
        t = 0.5f / t;
        Q[0] = (R[0][2] - R[2][0]) * t;
        Q[1] = (R[1][0] + R[0][1]) * t;
        Q[3] = (R[2][1] + R[1][2]) * t;
    }
    else
    {
        t = sqrtf(1.0f - R[0][0] - R[1][1] + R[2][2]);
        Q[3] = 0.5F * t;
        t = 0.5f / t;
        Q[0] = (R[1][0] - R[0][1]) * t;
        Q[1] = (R[0][2] + R[2][0]) * t;
        Q[2] = (R[2][1] + R[1][2]) * t;
    }
}

/**
 * @brief 使用欧拉角构造四元数
 *
 * @param euler 欧拉角, 顺序必须为 航向—>俯仰->横滚, 单位 rad
 * @param Q 四元数
 */
void Quaternion_ByEuler(float yaw, float pitch, float roll, float Q[4])
{
    Q[0] = (cosf(yaw / 2) * cosf(pitch / 2) * cosf(roll / 2) -
        sinf(yaw / 2) * sinf(pitch / 2) * sinf(roll / 2));
    Q[1] = (cosf(yaw / 2) * sinf(pitch / 2) * cosf(roll / 2) -
        sinf(yaw / 2) * cosf(pitch / 2) * sinf(roll / 2));
    Q[2] = (cosf(yaw / 2) * cosf(pitch / 2) * sinf(roll / 2) +
        sinf(yaw / 2) * sinf(pitch / 2) * cosf(roll / 2));
    Q[3] = (cosf(yaw / 2) * sinf(pitch / 2) * sinf(roll / 2) +
        sinf(yaw / 2) * cosf(pitch / 2) * cosf(roll / 2));
}

/**
 * @brief 使用轴角构造四元数
 *
 * @param anxisAngle 旋转轴角向量, 模为转角 rad, 方向为转轴
 * @param Q 四元数
 */
void Quaternion_ByAxisAngle(const float anxisAngle[3], float Q[4])
{
    float angle = Vector_GetNorm(anxisAngle, 3);

    if (angle < 1e-10)
    {
        /* 如果 angle 太小, 相当于没转 */
        Quaternion(Q);
    }
    else
    {
        float axis[3] = { anxisAngle[0], anxisAngle[1], anxisAngle[2] };
        Vector_DotProductRealNum(axis, 1.0f / angle, 3);
        float magnitude = sinf(angle * 0.5f);
        Q[0] = cosf(angle * 0.5f);
        for (int i = 0; i < 3; i++)
        {
            Q[i + 1] = axis[i] * magnitude;
        }
    }
}

/**
 * @brief 使用向量, 构造由 V1->V2 的最短旋转四元数
 *
 * @param Vsrc 旋转前的向量
 * @param Vdst 旋转后的向量
 * @param Q V1->V2 的最速旋转四元数
 */
void Quaternion_ByTwoVectors(const float Vsrc[3], const float Vdst[3], float Q[4])
{
    float Vcr[3], dot;
    const float eps = 1e-5;
    Vector_CrossProduct_3D(Vsrc, Vdst, Vcr);
    dot = Vector_DotProduct(Vsrc, Vdst, 3);

    if (dot < 0.0f && Vector_GetNorm(Vcr, 3) < eps)
    {
        /* 判断 Vsrc Vdst 是接近 180 度 */
        float Vtmp[3] = { fabsf(Vcr[0]), fabsf(Vcr[1]), fabsf(Vcr[2]) };
        if (Vtmp[0] < Vtmp[1])
        {
            if (Vtmp[0] < Vtmp[2])
            {
                Vtmp[0] = 1;
                Vtmp[1] = 0;
                Vtmp[2] = 0;
            }
            else
            {
                Vtmp[0] = 0;
                Vtmp[1] = 0;
                Vtmp[2] = 1;
            }
        }
        else
        {
            if (Vtmp[1] < Vtmp[2])
            {
                Vtmp[0] = 0;
                Vtmp[1] = 1;
                Vtmp[2] = 0;
            }
            else
            {
                Vtmp[0] = 0;
                Vtmp[1] = 0;
                Vtmp[2] = 1;
            }
        }
        Q[0] = 0;
        Vector_CrossProduct_3D(Vsrc, Vtmp, Vcr);
    }
    else
    {
        Q[0] = dot +
            sqrt(Vector_DotProduct(Vsrc, Vsrc, 3) * Vector_DotProduct(Vdst, Vdst, 3));
    }

    Q[1] = Vcr[0];
    Q[2] = Vcr[1];
    Q[3] = Vcr[2];

    Quaternion_Normalize(Q);
}

/**
 * @brief 单位四元数乘法
 *
 * @param Q1
 * @param Q2
 * @param Q Q1 X Q2
 */
void Quaternion_Multiplication(const float Q1[4], const float Q2[4], float Q[4])
{
    Q[0] = Q1[0] * Q2[0] - Q1[1] * Q2[1] - Q1[2] * Q2[2] - Q1[3] * Q2[3];
    Q[1] = Q1[1] * Q2[0] + Q1[0] * Q2[1] - Q1[3] * Q2[2] + Q1[2] * Q2[3];
    Q[2] = Q1[2] * Q2[0] + Q1[3] * Q2[1] + Q1[0] * Q2[2] - Q1[1] * Q2[3];
    Q[3] = Q1[3] * Q2[0] - Q1[2] * Q2[1] + Q1[1] * Q2[2] + Q1[0] * Q2[3];
}

/**
 * @brief 四元数求逆
 *
 * @param Q
 * @param Q_inv
 */
void Quaternion_Inversed(const float Q[4], float Q_inv[4])
{
    float q_norm = Vector_GetNorm(Q, 4);
    Q_inv[0] = Q[0] / q_norm;
    Q_inv[1] = -Q[1] / q_norm;
    Q_inv[2] = -Q[2] / q_norm;
    Q_inv[3] = -Q[3] / q_norm;
}

/**
 * @brief 将四元数求逆
 *
 * @param Q
 */
void Quaternion_Invert(float Q[4])
{
    float Q_inv[4];
    Quaternion_Inversed(Q, Q_inv);

    for (int i = 0; i < 4; ++i)
    {
        Q[i] = Q_inv[i];
    }
}

/**
 * @brief 根据 frame1 的角速率, 求 q21 的微分
 *  比如 frame1 为机体坐标系, frame2 为东北天坐标系
 *  V1 为向量在 frame1 的坐标, V2 为向量在 fram2 的坐标
 *  [0, V2] = q21 * [0, V1] * q21^-1
 *  q21_deriv = q21 * [0, 0.5 * angle_rate1]
 *
 * @param q21 当前四元数
 * @param angle_rate1 frame 1 下的三轴角速率
 * @param q21_deriv q21 的微分
 */
void Quaternion_Derivative1(const float q21[4], const float angle_rate1[3], float q21_deriv[4])
{
    float v[4] = { 0,
                  0.5f * angle_rate1[0],
                  0.5f * angle_rate1[1],
                  0.5f * angle_rate1[2] };

    Quaternion_Multiplication(q21, v, q21_deriv);
}

/**
 * @brief 根据 frame2 的角速率, 求 q21 的微分
 *  比如 frame1 为机体坐标系, frame2 为东北天坐标系
 *  V1 为向量在 frame1 的坐标, V2 为向量在 fram2 的坐标
 *  q21 为 [0,V2] = q21 * [0,V1] * q21^-1
 *  q21_deriv = [0, 0.5 * angle_rate1] * q21
 *
 * @param q21 当前四元数
 * @param angle_rate1 frame 1 下的三轴角速率
 * @param q21_deriv q21 的微分
 */
void Quaternion_Derivative2(const float q21[4], const float angle_rate2[3], float q21_deriv[4])
{
    float v[4] = { 0,
                  0.5f * angle_rate2[0],
                  0.5f * angle_rate2[1],
                  0.5f * angle_rate2[2] };

    Quaternion_Multiplication(v, q21, q21_deriv);
}

/**
 * @brief 将四元数按照轴角进行旋转
 *
 * @param axisAngle 轴角向量
 * @param Q 四元数
 */
void Quaternion_RotateByAxisAngle(const float axisAngle[3], float Q[4])
{
    /* 根据轴角向量计算旋转四元数 */
    float q_rot[4], q_temp[4];
    Quaternion_ByAxisAngle(axisAngle, q_rot);

    Quaternion_Multiplication(q_rot, Q, q_temp);

    for (int i = 0; i < 4; ++i)
    {
        Q[i] = q_temp[i];
    }
}

/**
 * @brief 通过四元数将 frame1 下的 V1 转到 frame2 下的 V2
 *
 * @param Q 四元数
 * @param V1
 * @param V2
 */
void Quaternion_Conj(const float Q[4], const float V1[3], float V2[3])
{
    float v_temp1[4] = { 0, V1[0], V1[1], V1[2] };
    float v_temp2[4], v_temp3[4];

    Quaternion_Multiplication(Q, v_temp1, v_temp2);
    Quaternion_Inversed(Q, v_temp1);
    Quaternion_Multiplication(v_temp2, v_temp1, v_temp3);

    V2[0] = v_temp3[1];
    V2[1] = v_temp3[2];
    V2[2] = v_temp3[3];
}

/**
 * @brief 通过四元数将 frame2 下的 V2 转到 frame1 下的 V1
 *
 * @param Q 四元数
 * @param V1
 * @param V2
 */
void Quaternion_ConjInv(const float Q[4], const float V2[3], float V1[3])
{
    float v_temp1[4] = { 0, V2[0], V2[1], V2[2] };
    float v_temp2[4], v_temp3[4];

    Quaternion_Multiplication(v_temp1, Q, v_temp2);
    Quaternion_Inversed(Q, v_temp1);
    Quaternion_Multiplication(v_temp1, v_temp2, v_temp3);

    V1[0] = v_temp3[1];
    V1[1] = v_temp3[2];
    V1[2] = v_temp3[3];
}

/**
 * @brief 通过四元数求其对应的旋转矩阵的 Z 轴
 * 可参考由四元数构造旋转矩阵
 *
 * @param Q 四元数
 * @param dcm_z 旋转矩阵 z 轴
 */
void Quaternion_GetDcmZ(const float Q[4], float dcm_z[3])
{
    float a = Q[0];
    float b = Q[1];
    float c = Q[2];
    float d = Q[3];

    dcm_z[0] = 2 * (a * c + b * d);
    dcm_z[1] = 2 * (c * d - a * b);
    dcm_z[2] = a * a - b * b - c * c + d * d;
}