#pragma once

#include <Orange/Math/Matrix.h>
#include <Orange/Math/Angle.h>
#include <Orange/Math/Basic.h>

namespace orange
{
    template <typename T>
    struct Quaternion : public Vec<T, 4> 
    {
        constexpr Quaternion()
            : Vec<T, 4>{ T{ 0 }, T{ 0 }, T{ 0 }, T{ 1 } } { }

        constexpr Quaternion(T x, T y, T z, T w)
            : Vec<T, 4>{ x, y, z, w } { }

        constexpr Quaternion(const Vec<T, 3> v, T s)
            : Vec<T, 4>{ v[0], v[1], v[2], s } { }

        constexpr Quaternion(const Quaternion& other) = default;


        constexpr Quaternion<T> operator*(const Quaternion<T>& b) const
        {
            const Quaternion<T>& a = *this;
            return Quaternion<T>
            {
	            a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
	            a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
	            a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
	            a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z
            };
        }

        constexpr Quaternion<T> operator*(float s) const
        {
            Quaternion<T> c = *this;

            c.x *= s;
            c.y *= s;
            c.z *= s;
            c.w *= s;

            return c;
        }

        constexpr Quaternion<T> operator/(const Quaternion<T>& other) = delete;

        constexpr Quaternion<T>& operator*=(const Quaternion<T>& other)
        {
            *this = *this * other;
            return *this;
        }

        constexpr Quaternion<T>& operator*=(float s)
        {
            Quaternion<T>& c = *this;

            c.x *= s;
            c.y *= s;
            c.z *= s;
            c.w *= s;

            return c;
        }

        constexpr Quaternion<T>& operator/=(const Quaternion<T>& other) = delete;

        // TODO: This sucks! Make this cleaner
        // We should get a proper subclass thing
        constexpr const Vec<T, 3>& vector() const
        { 
            return *reinterpret_cast<const Vec<T, 3>*>(this);
        }

        constexpr Vec<T, 3>& vector()
        { 
            return *reinterpret_cast<Vec<T, 3>*>(this);
        }

        constexpr const T& scalar() const
        {
            return (*this)[3];
        }

        constexpr T& scalar()
        {
            return (*this)[3];
        }
    };

    template <typename T>
    constexpr Quaternion<T> operator*(float s, const Quaternion<T>& q)
    {
        return q * s;
    }

    template <typename T>
    constexpr Vec<T, 3> vector(const Quaternion<T>& q)
    { 
        return q.vector();
    }

    template <typename T>
    constexpr T scalar(const Quaternion<T>& q)
    {
        return q.scalar();
    }

    template <typename T>
    float lengthSqr(const Quaternion<T>& q) { return dot(q, q); }

    template <typename T>
    float length(const Quaternion<T>& q) { return sqrtf(lengthSqr(q)); }

    template <typename T>
    float dot(const Quaternion<T>& a, const Quaternion<T>& b)
    {
        return dot(a.vector(), b.vector()) + a.w * b.w;
    }

    template <typename T>
    constexpr Quaternion<T> cross(const Quaternion<T>& a, const Quaternion<T>& b)
    {
        return Quaternion<T>
        {
            a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1],
            a[3] * b[1] + a[1] * b[3] + a[2] * b[0] - a[0] * b[2],
            a[3] * b[2] + a[2] * b[3] + a[0] * b[1] - a[1] * b[0],
            a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2]
        };
    }

    template <typename T>
    constexpr Quaternion<T> normalize(const Quaternion<T>& q) { return q * (1.0f / length(q)); }

    template <typename T>
    constexpr Quaternion<T> conjugate(const Quaternion<T>& q)
    {
        return Quaternion<T> { -vector(q), scalar(q) };
    }

    template <typename T>
    constexpr Quaternion<T> inverse(const Quaternion<T>& q)
    {
        return Quaternion<T> { conjugate(q) / length(q) };
    }


    template <typename T>
    constexpr Vec<T, 3> operator*(const Quaternion<T>& q, const Vec<T, 3> v)
    {
        const Vec<T, 3> t = 2.0f * cross(vector(q), v);
        return Vec<T, 3>{ v + scalar(q) * t + cross(vector(q), t) };
    }

    using quat = Quaternion<float>;

    inline Radian angle(const quat& q) { return 2.0f * Math::acos(q.w); }

    inline vec3 axis(const quat& q)
    {
        // 1 - cos(theta/2)*cos(theta/2) = sin(theta/2)*sin(theta/2)
        float s2 = 1.0f - q.w * q.w;

        if (s2 <= 0.0f)
            return vec3(0.0f, 0.0f, 1.0f);

        float invs2 = 1.0f / sqrtf(s2);

        return q.vector() * invs2;
    }

    inline quat angleAxis(const Radian& angle, const vec3& axis)
    {
        quat q;

        const vec3 a = normalize(axis);

        const float s = Math::sin(0.5f * angle);

        q.vector() = a * s;
        q.w = Math::cos(0.5f * angle);

        return q;
    }

    inline mat4 quaternionToMatrix4(const quat& q)
    {
        mat4 mat;
        quat a = normalize(q);

        const float xx = a.x * a.x;
        const float yy = a.y * a.y;
        const float zz = a.z * a.z;
        const float xy = a.x * a.y;
        const float xz = a.x * a.z;
        const float yz = a.y * a.z;
        const float wx = a.w * a.x;
        const float wy = a.w * a.y;
        const float wz = a.w * a.z;

        mat[0][0] = 1.0f - 2.0f * (yy + zz);
        mat[0][1] = 2.0f * (xy + wz);
        mat[0][2] = 2.0f * (xz - wy);

        mat[1][0] = 2.0f * (xy - wz);
        mat[1][1] = 1.0f - 2.0f * (xx + zz);
        mat[1][2] = 2.0f * (yz + wx);

        mat[2][0] = 2.0f * (xz + wy);
        mat[2][1] = 2.0f * (yz - wx);
        mat[2][2] = 1.0f - 2.0f * (xx + yy);

        return mat;
    }

    inline quat matrix4ToQuaternion(const mat4& m)
    {
        float fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2];
        float fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2];
        float fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1];
        float fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2];

        int biggestIndex = 0;
        float fourBiggestSquaredMinus1 = fourWSquaredMinus1;
        if (fourXSquaredMinus1 > fourBiggestSquaredMinus1)
        {
            fourBiggestSquaredMinus1 = fourXSquaredMinus1;
            biggestIndex = 1;
        }
        if (fourYSquaredMinus1 > fourBiggestSquaredMinus1)
        {
            fourBiggestSquaredMinus1 = fourYSquaredMinus1;
            biggestIndex = 2;
        }
        if (fourZSquaredMinus1 > fourBiggestSquaredMinus1)
        {
            fourBiggestSquaredMinus1 = fourZSquaredMinus1;
            biggestIndex = 3;
        }

        float biggestVal = sqrtf(fourBiggestSquaredMinus1 + 1.0f) * 0.5f;
        float mult = 0.25f / biggestVal;

        quat q;

        switch (biggestIndex)
        {
            case 0:
            {
                q.w = biggestVal;
                q.x = (m[1][2] - m[2][1]) * mult;
                q.y = (m[2][0] - m[0][2]) * mult;
                q.z = (m[0][1] - m[1][0]) * mult;
            }
            break;
            case 1:
            {
                q.w = (m[1][2] - m[2][1]) * mult;
                q.x = biggestVal;
                q.y = (m[0][1] + m[1][0]) * mult;
                q.z = (m[2][0] + m[0][2]) * mult;
            }
            break;
            case 2:
            {
                q.w = (m[2][0] - m[0][2]) * mult;
                q.x = (m[0][1] + m[1][0]) * mult;
                q.y = biggestVal;
                q.z = (m[1][2] + m[2][1]) * mult;
            }
            break;
            case 3:
            {
                q.w = (m[0][1] - m[1][0]) * mult;
                q.x = (m[2][0] + m[0][2]) * mult;
                q.y = (m[1][2] + m[2][1]) * mult;
                q.z = biggestVal;
            }
            break;
        }

        return q;
    }

    inline Radian roll(const quat& q)
    {
        return Math::atan2(2.0f * q[0] * q[1] + q[2] * q[3],
                           q[0] * q[0] + q[3] * q[3] - q[1] * q[1] - q[2] * q[2]);
    }

    inline Radian pitch(const quat& q)
    {
        return Math::atan2(2.0f * q[1] * q[2] + q[3] * q[0],
                           q[3] * q[3] - q[0] * q[0] - q[1] * q[1] + q[2] * q[2]);
    }

    inline Radian yaw(const quat& q)
    {
        return Math::asin(-2.0f * (q[0] * q[2] - q[3] * q[1]));
    }

    inline EulerAngles quaternionToEulerAngles(const quat& q)
    {
        return {pitch(q), yaw(q), roll(q)};
    }

    inline quat eulerAnglesToQuaternion(
        const EulerAngles& e,
        const vec3& xAxis = {1.0f, 0.0f, 0.0f},
        const vec3& yAxis = {0.0f, 1.0f, 0.0f},
        const vec3& zAxis = {0.0f, 0.0f, 1.0f})
    {
        quat p = angleAxis(e.pitch, xAxis);
        quat y = angleAxis(e.yaw, yAxis);
        quat r = angleAxis(e.roll, zAxis);

        return y * p * r;
    }
}