/**
 * @file
 * @brief Functions related to 3D quaternions and Euler angles.
 * @author Krishna Vedala
 */

#include <stdio.h>
#ifdef __arm__  // if compiling for ARM-Cortex processors
#define LIBQUAT_ARM
#include <arm_math.h>
#else
#include <math.h>
#endif
#include <assert.h>

#include "geometry_datatypes.h"

/**
 * @addtogroup quats 3D Quaternion operations
 * @{
 */

/**
 * Function to convert given Euler angles to a quaternion.
 * \f{eqnarray*}{
 * q_{0} & =
 * &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
 * +
 * \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
 * q_{1} & =
 * &\sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
 * -
 * \cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
 * q_{2} & =
 * &\cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)
 * +
 * \sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)\\
 * q_{3} & =
 * &\cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right)
 * -
 * \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right)\\
 * \f}
 *
 * @param [in] in_euler input Euler angles instance
 * @returns converted quaternion
 */
quaternion quat_from_euler(const euler *in_euler)
{
    quaternion out_quat;

    if (!in_euler)  // if null
    {
        fprintf(stderr, "%s: Invalid input.", __func__);
        return out_quat;
    }

    quaternion temp;

    float cy = cosf(in_euler->yaw * 0.5f);
    float sy = sinf(in_euler->yaw * 0.5f);
    float cp = cosf(in_euler->pitch * 0.5f);
    float sp = sinf(in_euler->pitch * 0.5f);
    float cr = cosf(in_euler->roll * 0.5f);
    float sr = sinf(in_euler->roll * 0.5f);

    temp.w = cr * cp * cy + sr * sp * sy;
    temp.q1 = sr * cp * cy - cr * sp * sy;
    temp.q2 = cr * sp * cy + sr * cp * sy;
    temp.q3 = cr * cp * sy - sr * sp * cy;

    return temp;
}

/**
 * Function to convert given quaternion to Euler angles.
 * \f{eqnarray*}{
 * \phi & = &
 * \tan^{-1}\left[\frac{2\left(q_0q_1+q_2q_3\right)}{1-2\left(q_1^2+q_2^2\right)}\right]\\
 * \theta & =
 * &-\sin^{-1}\left[2\left(q_0q_2-q_3q_1\right)\right]\\
 * \psi & = &
 * \tan^{-1}\left[\frac{2\left(q_0q_3+q_1q_2\right)}{1-2\left(q_2^2+q_3^2\right)}\right]\\
 * \f}
 *
 * @param [in] in_quat input quaternion instance
 * @returns converted euler angles
 */
euler euler_from_quat(const quaternion *in_quat)
{
    euler out_euler;
    if (!in_quat)  // if null
    {
        fprintf(stderr, "%s: Invalid input.", __func__);
        return out_euler;
    }

    out_euler.roll = atan2f(
        2.f * (in_quat->w * in_quat->q1 + in_quat->q2 * in_quat->q3),
        1.f - 2.f * (in_quat->q1 * in_quat->q1 + in_quat->q2 * in_quat->q2));
    out_euler.pitch =
        asinf(2.f * (in_quat->w * in_quat->q2 + in_quat->q1 * in_quat->q3));
    out_euler.yaw = atan2f(
        2.f * (in_quat->w * in_quat->q3 + in_quat->q1 * in_quat->q2),
        1.f - 2.f * (in_quat->q2 * in_quat->q2 + in_quat->q3 * in_quat->q3));

    return out_euler;
}

/**
 * Function to multiply two quaternions.
 * \f{eqnarray*}{
 * \mathbf{c} & = & \mathbf{a}\otimes\mathbf{b}\\
 * & = & \begin{bmatrix}a_{0} & a_{1} & a_{2} &
 *  a_{3}\end{bmatrix}\otimes\begin{bmatrix}b_{0} & b_{1} & b_{2} &
 *  b_{3}\end{bmatrix}\\
 * & = &
 * \begin{bmatrix}
 *  a_{0}b_{0}-a_{1}b_{1}-a_{2}b_{2}-a_{3}b_{3}\\
 *  a_{0}b_{1}+a_{1}b_{0}+a_{2}b_{3}-a_{3}b_{2}\\
 *  a_{0}b_{2}-a_{1}b_{3}+a_{2}b_{0}+a_{3}b_{1}\\
 *  a_{0}b_{3}+a_{1}b_{2}-a_{2}b_{1}+a_{3}b_{0}
 * \end{bmatrix}^{T}
 * \f}
 *
 * @param [in] in_quat1 first input quaternion instance
 * @param [in] in_quat2 second input quaternion instance
 * @returns resultant quaternion
 */
quaternion quaternion_multiply(const quaternion *in_quat1,
                               const quaternion *in_quat2)
{
    quaternion out_quat;
    if (!in_quat1 || !in_quat2)  // if null
    {
        fprintf(stderr, "%s: Invalid input.", __func__);
        return out_quat;
    }

    out_quat.w = in_quat1->w * in_quat2->w - in_quat1->q1 * in_quat2->q1 -
                 in_quat1->q2 * in_quat2->q2 - in_quat1->q3 * in_quat2->q3;
    out_quat.q1 = in_quat1->w * in_quat2->q1 + in_quat1->q1 * in_quat2->w +
                  in_quat1->q2 * in_quat2->q3 - in_quat1->q3 * in_quat2->q2;
    out_quat.q2 = in_quat1->w * in_quat2->q2 - in_quat1->q1 * in_quat2->q3 +
                  in_quat1->q2 * in_quat2->w + in_quat1->q3 * in_quat2->q1;
    out_quat.q3 = in_quat1->w * in_quat2->q3 + in_quat1->q1 * in_quat2->q2 -
                  in_quat1->q2 * in_quat2->q1 + in_quat1->q3 * in_quat2->w;

    return out_quat;
}

/** @} */

static void test()
{
    quaternion quat = {0.7071f, 0.7071f, 0.f, 0.f};
    euler eul = euler_from_quat(&quat);
    printf("Euler: %.4g, %.4g, %.4g\n", eul.pitch, eul.roll, eul.yaw);

    quaternion test_quat = quat_from_euler(&eul);
    printf("Quaternion: %.4g %+.4g %+.4g %+.4g\n", test_quat.w,
           test_quat.dual.x, test_quat.dual.y, test_quat.dual.z);

    assert(fabsf(test_quat.w - quat.w) < .01);
    assert(fabsf(test_quat.q1 - quat.q1) < .01);
    assert(fabsf(test_quat.q2 - quat.q2) < .01);
    assert(fabsf(test_quat.q3 - quat.q3) < .01);
}

int main()
{
    test();
    return 0;
}