/**
 * Jingga
 *
 * @copyright Jingga
 * @license   OMS License 2.0
 * @version   1.0.0
 * @link      https://jingga.app
 * @link      https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html
 */
#ifndef TOS_MATH_MATRIX_QUATERNION_FLOAT32_H
#define TOS_MATH_MATRIX_QUATERNION_FLOAT32_H

#include "../../stdlib/Intrinsics.h"
#include "../../stdlib/Mathtypes.h"
#include "../../utils/MathUtils.h"
#include "../../utils/TestUtils.h"

inline
void quaternion_unit(v4_f32* quat)
{
    f32 length = sqrtf(quat->w * quat->w + quat->x * quat->x + quat->y * quat->y + quat->z * quat->z);

    quat->w /= length;
    quat->x /= length;
    quat->y /= length;
    quat->z /= length;
}

inline
void quaternion_from_euler(v4_f32* quat, float pitch, float yaw, float roll)
{
    float y = OMS_RAD2DEG(yaw * 0.5f);
    float cy = cosf(y);
    float sy = sinf(y);

    float p = OMS_RAD2DEG(pitch * 0.5f);
    float cp = cosf(p);
    float sp = sinf(p);

    float r = OMS_RAD2DEG(roll * 0.5f);
    float cr = cosf(r);
    float sr = sinf(r);

    quat->w = cr * cp * cy + sr * sp * sy;
    quat->x = sr * cp * cy - cr * sp * sy;
    quat->y = cr * sp * cy + sr * cp * sy;
    quat->z = cr * cp * sy - sr * sp * cy;

    quaternion_unit(quat);
}

inline
void quaternion_from_euler(v4_f32* quat, const v3_f32* v)
{
    float y = OMS_RAD2DEG(v->v * 0.5f);
    float cy = cosf(y);
    float sy = sinf(y);

    float p = OMS_RAD2DEG(v->u * 0.5f);
    float cp = cosf(p);
    float sp = sinf(p);

    float r = OMS_RAD2DEG(v->w * 0.5f);
    float cr = cosf(r);
    float sr = sinf(r);

    quat->w = cr * cp * cy + sr * sp * sy;
    quat->x = sr * cp * cy - cr * sp * sy;
    quat->y = cr * sp * cy + sr * cp * sy;
    quat->z = cr * cp * sy - sr * sp * cy;
}

void quaternion_to_euler(const v4_f32* quat, v3_f32* v) {
    // Pitch
    float sinp = 2.0f * (quat->w * quat->x + quat->y * quat->z);
    float cosp = 1.0f - 2.0f * (quat->x * quat->x + quat->y * quat->y);
    v->pitch = atan2f(sinp, cosp);

    // Check for gimbal lock
    float sinp_check = 2.0f * (quat->w * quat->x + quat->y * quat->z);
    if (OMS_ABS(sinp_check) >= 0.9999f) {
        v->yaw = atan2f(quat->x * quat->z - quat->w * quat->y, quat->w * quat->x + quat->y * quat->z);
        v->roll = 0.0f;
    } else {
        // Yaw
        float siny = 2.0f * (quat->w * quat->y - quat->z * quat->x);
        v->yaw = asinf(siny);

        // Roll
        float sinr = 2.0f * (quat->w * quat->z + quat->x * quat->y);
        float cosr = 1.0f - 2.0f * (quat->y * quat->y + quat->z * quat->z);
        v->roll = atan2f(sinr, cosr);
    }
}

void quaternion_multiply(v4_f32* quat, const v4_f32* quat1, const v4_f32* quat2)
{
    quat->w = quat1->w * quat2->w - quat1->x * quat2->x - quat1->y * quat2->y - quat1->z * quat2->z;
    quat->x = quat1->w * quat2->x + quat1->x * quat2->w + quat1->y * quat2->z - quat1->z * quat2->y;
    quat->y = quat1->w * quat2->y - quat1->x * quat2->z + quat1->y * quat2->w + quat1->z * quat2->x;
    quat->z = quat1->w * quat2->z + quat1->x * quat2->y - quat1->y * quat2->x + quat1->z * quat2->w;
}

void quaternion_inverse(v4_f32* quat, const v4_f32* quat_origin) {
    float norm = quat_origin->w * quat_origin->w
        + quat_origin->x * quat_origin->x
        + quat_origin->y * quat_origin->y
        + quat_origin->z * quat_origin->z;

    quat->w = quat_origin->w / norm;
    quat->x = -quat_origin->x / norm;
    quat->y = -quat_origin->y / norm;
    quat->z = -quat_origin->z / norm;
}

inline
void quaternion_to_rotation(f32* matrix, const v4_f32* quat)
{
    matrix[0] = 1.0f - 2.0f * (quat->y * quat->y + quat->z * quat->z);
    matrix[1] = 2.0f * (quat->x * quat->y - quat->z * quat->w);
    matrix[2] = 2.0f * (quat->x * quat->z + quat->y * quat->w);
    matrix[3] = 0.0f;

    matrix[4] = 2.0f * (quat->x * quat->y + quat->z * quat->w);
    matrix[5] = 1.0f - 2.0f * (quat->x * quat->x + quat->z * quat->z);
    matrix[6] = 2.0f * (quat->y * quat->z - quat->x * quat->w);
    matrix[7] = 0.0f;

    matrix[8] = 2.0f * (quat->x * quat->z - quat->y * quat->w);
    matrix[9] = 2.0f * (quat->y * quat->z + quat->x * quat->w);
    matrix[10] = 1.0f - 2.0f * (quat->x * quat->x + quat->y * quat->y);
    matrix[11] = 0.0f;

    matrix[12] = 0.0f;
    matrix[13] = 0.0f;
    matrix[14] = 0.0f;
    matrix[15] = 1.0f;
}

inline
void quaternion_to_rotation(f32* matrix, const v4_f32* quat)
{
    matrix[0] = 1.0f - 2.0f * (quat->y * quat->y + quat->z * quat->z);
    matrix[1] = 2.0f * (quat->x * quat->y - quat->z * quat->w);
    matrix[2] = 2.0f * (quat->x * quat->z + quat->y * quat->w);

    matrix[3] = 2.0f * (quat->x * quat->y + quat->z * quat->w);
    matrix[4] = 1.0f - 2.0f * (quat->x * quat->x + quat->z * quat->z);
    matrix[5] = 2.0f * (quat->y * quat->z - quat->x * quat->w);

    matrix[6] = 2.0f * (quat->x * quat->z - quat->y * quat->w);
    matrix[7] = 2.0f * (quat->y * quat->z + quat->x * quat->w);
    matrix[8] = 1.0f - 2.0f * (quat->x * quat->x + quat->y * quat->y);
}

inline
void quaternion_from_vec(v4_f32* quat, const v4_f32* vec)
{
    quat->x = vec->x;
    quat->y = vec->y;
    quat->z = vec->z;
    quat->w = 0;
}

inline
void quaternion_from_vec(v4_f32* quat, const v3_f32* vec)
{
    quat->x = vec->x;
    quat->y = vec->y;
    quat->z = vec->z;
    quat->w = 0;
}

inline
void quaternion_to_vec(v4_f32* vec, const v4_f32* quat)
{
    vec->x = quat->x;
    vec->y = quat->y;
    vec->z = quat->z;
    vec->w = 0.0f;
}

inline
void quaternion_to_vec(v3_f32* vec, const v4_f32* quat)
{
    vec->x = quat->x;
    vec->y = quat->y;
    vec->z = quat->z;
}

// active = point rotated respective to coordinate system
inline
void quaternion_rotate_active(v4_f32* p, const v4_f32* quat, const v4_f32* quat_inv)
{
    ASSERT_SIMPLE(OMS_ABS(x * x + y * y + z * z + w * z - 1.0f) < 0.01);

    v4_f32 p_tmp;
    quaternion_multiply(&p_tmp, quat_inv, p);
    quaternion_multiply(p, &p_tmp, quat);
}

// passive = coordinate system is rotated
inline
void quaternion_rotate_passive(v4_f32* p, const v4_f32* quat, const v4_f32* quat_inv)
{
    ASSERT_SIMPLE(OMS_ABS(x * x + y * y + z * z + w * w - 1.0f) < 0.01);

    v4_f32 p_tmp;
    quaternion_multiply(&p_tmp, quat, p);
    quaternion_multiply(p, &p_tmp, quat_inv);
}

// Rotation algorithm
// 1. create quat from pitch/yaw/roll
// 2. convert the quat to unit length
// 3. create quat_inv
// 4. create quat from vec
// 5. call quat_rotate_*
// 6. convert quat to vec
// @todo Since this is usually done on multiple vecs, we should probably accept an array of vecs and then use simd
void quaternion_rotate_active(v4_f32* vec, float pitch, float yaw, float roll)
{
    v4_f32 q;
    quaternion_from_euler(&q, pitch, yaw, roll); // q is already in unit length

    v4_f32 q_inv;
    quaternion_inverse(&q_inv, &q);

    v4_f32 p;
    quaternion_from_vec(&p, vec);

    quaternion_rotate_active(&p, &q, &q_inv);

    quaternion_to_vec(vec, &p);
}

void quaternion_rotate_passive(v4_f32* vec, float pitch, float yaw, float roll)
{
    v4_f32 q;
    quaternion_from_euler(&q, pitch, yaw, roll); // q is already in unit length

    v4_f32 q_inv;
    quaternion_inverse(&q_inv, &q);

    v4_f32 p;
    quaternion_from_vec(&p, vec);

    quaternion_rotate_passive(&p, &q, &q_inv);

    quaternion_to_vec(vec, &p);
}

#endif