#ifndef osml_quaternion_h
#define osml_quaternion_h

#include <osml_vector.h>

OSML_INLINE quaternion_t quaternion_identity(void)
{
    quaternion_t result;
    result.w = 1.0f;
    result.x = 0.0f;
    result.y = 0.0f;
    result.z = 0.0f;
    return result;
}

OSML_INLINE quaternion_t quaternion(float w, float x, float y, float z)
{
    quaternion_t result;
    result.w = w;
    result.x = x;
    result.y = y;
    result.z = z;
    return result;
}

OSML_INLINE quaternion_t quaternion_from_angle_around_axis(float angle, vector_t axis)
{
    float sin_half_angle = osml_sinf(0.5f * angle);
    float cos_half_angle = osml_cosf(0.5f * angle);
    
    vector_t normalized_axis = vector_normalized(axis);
    
    return quaternion(cos_half_angle, sin_half_angle * normalized_axis.x,
                                      sin_half_angle * normalized_axis.y,
                                      sin_half_angle * normalized_axis.z);
}

OSML_INLINE quaternion_t quaternion_add(quaternion_t lhs, quaternion_t rhs)
{
    return quaternion(lhs.w + rhs.w, lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z);
}

OSML_INLINE quaternion_t quaternion_subtract(quaternion_t lhs, quaternion_t rhs)
{
    return quaternion(lhs.w - rhs.w, lhs.x - rhs.x, lhs.y - rhs.y, lhs.z - rhs.z);
}

OSML_INLINE quaternion_t quaternion_scale(quaternion_t lhs, float rhs)
{
    return quaternion(lhs.w * rhs, lhs.x * rhs, lhs.y * rhs, lhs.z * rhs);
}

OSML_INLINE float quaternion_dot_product(quaternion_t lhs, quaternion_t rhs)
{
    return (lhs.w * rhs.w) + (lhs.x * rhs.x) + (lhs.y * rhs.y) + (lhs.z * rhs.z);
}

OSML_INLINE float quaternion_norm(quaternion_t q)
{
    return quaternion_dot_product(q, q);
}

OSML_INLINE quaternion_t quaternion_normalized(quaternion_t q)
{
    float reciprocal_length = osml_reciprocal_sqrtf(quaternion_norm(q));
    return quaternion(q.w * reciprocal_length,
                      q.x * reciprocal_length,
                      q.y * reciprocal_length,
                      q.z * reciprocal_length);
}

OSML_INLINE quaternion_t quaternion_conjugate(quaternion_t q)
{
    return quaternion(q.w, -q.x, -q.y, -q.z);
}

OSML_INLINE quaternion_t quaternion_inverse(quaternion_t q)
{
    float reciprocal_norm = 1.0f / quaternion_norm(q);
    return quaternion(q.w * reciprocal_norm,
                     -q.x * reciprocal_norm,
                     -q.y * reciprocal_norm,
                     -q.z * reciprocal_norm);
}

OSML_INLINE quaternion_t quaternion_product(quaternion_t lhs, quaternion_t rhs)
{
    vector_t v_left = vector3(lhs.x, lhs.y, lhs.z);
    vector_t v_right = vector3(rhs.x, rhs.y, rhs.z);
    
    float angle = (lhs.w * rhs.w) - (vector_dot_product(v_left, v_right));
    
    vector_t new_v_left = vector_scale(v_left, rhs.w);
    vector_t new_v_right = vector_scale(v_right, lhs.w);
    vector_t perpendicular = vector_cross_product(v_left, v_right);
    
    vector_t new_v = vector_add(new_v_left, vector_add(new_v_right, perpendicular));
    
    return quaternion(angle, new_v.x, new_v.y, new_v.z);
}

OSML_INLINE vector_t quaternion_rotate_vector(quaternion_t q, vector_t v)
{
    quaternion_t q_inverse = quaternion_inverse(q);
    quaternion_t v_q = quaternion(0.0f, v.x, v.y, v.z);
    
    quaternion_t product =
        quaternion_product(q, quaternion_product(v_q, q_inverse));
    return vector3(product.x, product.y, product.z);
}

#endif