/***************************************************************************
*                                                                          *
*  Copyright (C) Golaem S.A.  All Rights Reserved.                         *
*                                                                          *
****************************************************************************/

#pragma once

#include "glmCoreDefinitions.h"
#include "glmVector3.h"
#include "glmAngle.h"
#include "glmMathBase.h"
#include "glmAssert.h"
#include "glmMemory.h"
#include <xmmintrin.h>

namespace glm
{
    static const unsigned int _quaternionValuesSize = sizeof(float) * 4;

    // In this representation the quaternion is a 4 dimension vector.
    //	- the value at index 0 is X ;
    //	- the value at index 1 is Y ;
    //	- the value at index 2 is Z ;
    //	- the value at index 3 is W ;
    class GLM_CORE_API GLM_ALIGN_16 Quaternion
    {
    public:
        GLM_CLASS_STAT_ID( glm::MemoryStatIds::GolaemShared);

        float x;
        float y;
        float z;
        float w;

        static const Quaternion& getNullVector();
        static const Quaternion& getIdentity();
        static Angle angularDistance(const Quaternion& quaternion1, const Quaternion& quaternion2);
        static float dotProduct(const Quaternion& quaternion1, const Quaternion& quaternion2);
        static Quaternion getRotationBetweenUnitVectors(const Vector3& from, const Vector3& to);
        static void quaternionTranslationToDualQuaternion(const Quaternion& rotation, const Vector3& translation, Quaternion* dq);

        static void mul(Vector3& result, const Quaternion& mulR, const Vector3& mulT);                                  //rotate: result = mulR*mulT
        static void mulAdd(Vector3& result, const Vector3& addT, const Quaternion& mulR, const Vector3& mulT);          //rotate and translate: result = addT + mulR*mulT
        static void mulSub(Vector3& result, const Vector3& addT, const Quaternion& mulR, const Vector3& mulT);          //rotate and translate: result = addT - mulR*mulT
        static void mulmul(Quaternion& result, const Quaternion& mul1, const Quaternion& mul2, const Quaternion& mul3); //result = mul1*mul2*mul3
        static void mul_4(Quaternion result[4], const Quaternion mul1[4], const Quaternion mul2[4]);                    //for i in [0:3]: result[i] = mul1[i]*mul2[i]

        Quaternion();
        explicit Quaternion(float value);
        Quaternion(float inX, float inY, float inZ, float inW);
        explicit Quaternion(float* inFloatArray);
        Quaternion(Angle angle, const Vector3& axis);
        Quaternion(const Quaternion& vector);
        Quaternion& operator=(const Quaternion& vector);

        const float& operator[](unsigned int index) const;
        float& operator[](unsigned int index);

        bool isNull() const;
        void fill(float value);
        const float* getFloatValues() const;
        void setValues(float inX, float inY, float inZ, float inW);
        void setValues(const float* inValues);
        void setToIdentity();

        float norm2() const;
        float norm() const;

        bool isNormalized(float precision = GLM_NORMALIZED_PRECISION_SQR) const;
        void normalizeSelf();
        Quaternion computeNormalize() const;

        Quaternion operator-() const;
        Quaternion& operator+=(const Quaternion& vector);
        Quaternion& operator-=(const Quaternion& vector);
        Quaternion& operator*=(float scalar);
        Quaternion& operator/=(float scalar);

        bool operator<(const Quaternion& vector) const;
        bool operator==(const Quaternion& vector) const;
        bool operator!=(const Quaternion& vector) const;

        void setPivotX(Angle angle);
        void setPivotY(Angle angle);
        void setPivotZ(Angle angle);

        Quaternion operator*(const float& scalar) const;
        Vector3 operator*(const Vector3& vector) const;
        Quaternion operator*(const Quaternion& quaternion) const;
        void multiplySelfNoReturn(const Quaternion& quaternion);
        Quaternion& operator*=(const Quaternion& quaternion);
        Quaternion operator/(const float& scalar) const;
        Quaternion operator/(const Quaternion& quaternion) const;
        Quaternion& operator/=(const Quaternion& quaternion);

        Vector3 mulByUnitVectorX() const; // result = this * Vector3(1, 0, 0)
        Vector3 mulByUnitVectorY() const; // result = this * Vector3(0, 1, 0)
        Vector3 mulByUnitVectorZ() const; // result = this * Vector3(0, 0, 1)

        Quaternion computeInverse() const; // Quaternion must be normalized as input.
        void inverseSelf();                // Quaternion must be normalized as input.
        Quaternion computeConjugate() const;
        void conjugateSelf();

        // computes and return the column of the associated transformation matrix at a given index.
        Vector3 getColumnX() const;
        Vector3 getColumnY() const;
        Vector3 getColumnZ() const;
        void getColumnZ(Vector3& returnValue) const;
        float extractOld2DOrientation() const;

        // computes and return the row of the associated transformation matrix at a given index.
        Vector3 getRow0() const;
        Vector3 getRow1() const;
        Vector3 getRow2() const;

        void multiplyAlphaQuat(const Quaternion& quat2, float alpha); // this is NOT a slerp !! Computes a multiply between current and another quat, and a scalar
        static Quaternion slerp(const Quaternion& quat1, const Quaternion& quat2, float t);

        Quaternion computeRotateZ(Angle az) const;
    };

    //-------------------------------------------------------------------------
    GLM_CORE_API Quaternion quaternionFromAngle(Angle angle); // get a Quaternion representing the old 2D orientation of (angle) around Z axis, X axis is 0
    GLM_CORE_API const char* toString(const Quaternion& quaternion, char* buffer, size_t bufferSize);

    //-------------------------------------------------------------------------
    inline std::ostream& operator<<(std::ostream& out, const Quaternion& quaternion)
    {
        out << "(" << quaternion.x << "," << quaternion.y << "," << quaternion.z << "," << quaternion.w << ")";
        return out;
    }

    //-------------------------------------------------------------------------
    inline Quaternion::Quaternion()
#ifndef NDEBUG
        : x(FLT_MAX)
        , y(FLT_MAX)
        , z(FLT_MAX)
        , w(FLT_MAX)
#endif
    {
    }

    //-------------------------------------------------------------------------
    inline Quaternion::Quaternion(float value)
        : x(value)
        , y(value)
        , z(value)
        , w(value)
    {
    }

    //-------------------------------------------------------------------------
    inline Quaternion::Quaternion(float inX, float inY, float inZ, float inW)
        : x(inX)
        , y(inY)
        , z(inZ)
        , w(inW)
    {
    }

    //-------------------------------------------------------------------------
    inline Quaternion::Quaternion(float* inFloatArray)
    {
        memcpy(&x, inFloatArray, _quaternionValuesSize);
    }

    //-------------------------------------------------------------------------
    inline Quaternion::Quaternion(const Quaternion& vector)
    {
        memcpy(&x, &vector.x, _quaternionValuesSize);
    }

    //-------------------------------------------------------------------------
    inline Quaternion::Quaternion(Angle angle, const Vector3& axis)
    {
        GLM_DEBUG_ASSERT(axis.isNormalized());
        float sinAngleDiv2 = sinf(angle * 0.5f);
        float cosAngleDiv2 = cosf(angle * 0.5f);
        x = sinAngleDiv2 * axis.x;
        y = sinAngleDiv2 * axis.y;
        z = sinAngleDiv2 * axis.z;
        w = cosAngleDiv2;
    }

    //-------------------------------------------------------------------------
    inline Quaternion& Quaternion::operator=(const Quaternion& vector)
    {
        GLM_DEBUG_ASSERT_MSG(this != &vector, "Quaternion assignment called with itself as value to copy");
        memcpy(&x, &vector.x, _quaternionValuesSize);

        return *this;
    }

    //-------------------------------------------------------------------------
    inline const float& Quaternion::operator[](unsigned int index) const
    {
        GLM_DEBUG_ASSERT(index < 4);
        return *((&x) + index);
    }

    //-------------------------------------------------------------------------
    inline float& Quaternion::operator[](unsigned int index)
    {
        GLM_DEBUG_ASSERT(index < 4);
        return *((&x) + index);
    }

    //-------------------------------------------------------------------------
    inline void Quaternion::fill(float value)
    {
        x = y = z = w = value;
    }

    //-------------------------------------------------------------------------
    inline const float* Quaternion::getFloatValues() const
    {
        return &x;
    }

    //-------------------------------------------------------------------------
    inline bool Quaternion::isNull() const
    {
        return (x == 0.f && y == 0.f && z == 0.f && w == 0.f);
    }

    //-------------------------------------------------------------------------
    inline float Quaternion::norm2() const
    {
        return (x * x + y * y + z * z + w * w);
    }

    //-------------------------------------------------------------------------
    inline float Quaternion::norm() const
    {
        return sqrtf(norm2());
    }

    //-------------------------------------------------------------------------
    inline Quaternion Quaternion::computeNormalize() const
    {
        Quaternion normalizedCopy(*this);
        normalizedCopy.normalizeSelf();
        return normalizedCopy;
    }

    //-------------------------------------------------------------------------
    inline bool Quaternion::isNormalized(float precision) const
    {
        return (isNull() || approxEqual(norm2(), 1.f, precision));
    }

    //-------------------------------------------------------------------------
    inline Quaternion& Quaternion::operator+=(const Quaternion& vector)
    {
        x += vector.x;
        y += vector.y;
        z += vector.z;
        w += vector.w;
        return *this;
    }

    //-------------------------------------------------------------------------
    inline Quaternion& Quaternion::operator-=(const Quaternion& vector)
    {
        x -= vector.x;
        y -= vector.y;
        z -= vector.z;
        w -= vector.w;
        return *this;
    }

    //-------------------------------------------------------------------------
    inline Quaternion Quaternion::operator-() const
    {
        Quaternion result;
        result.x = -x;
        result.y = -y;
        result.z = -z;
        result.w = -w;
        return result;
    }

    //-------------------------------------------------------------------------
    inline Quaternion& Quaternion::operator*=(float scalar)
    {
        GLM_DEBUG_ASSERT_MSG(glmFinite(scalar), "Undefined scalar");
        x *= scalar;
        y *= scalar;
        z *= scalar;
        w *= scalar;
        return *this;
    }

    //-------------------------------------------------------------------------
    inline Quaternion& Quaternion::operator/=(float scalar)
    {
        GLM_DEBUG_ASSERT_MSG(scalar != 0, "Quaternion::operator/ scalar is zero");
        GLM_DEBUG_ASSERT_MSG(glmFinite(scalar), "Undefined scalar");
        x /= scalar;
        y /= scalar;
        z /= scalar;
        w /= scalar;
        return *this;
    }

    //-------------------------------------------------------------------------
    inline bool Quaternion::operator<(const Quaternion& vector) const
    {
        if (x < vector.x)
            return true;
        if (x > vector.x)
            return false;
        if (y < vector.y)
            return true;
        if (y > vector.y)
            return false;
        if (z < vector.z)
            return true;
        if (z > vector.z)
            return false;
        if (w < vector.w)
            return true;
        if (w > vector.w)
            return false;

        return false;
    }

    //-------------------------------------------------------------------------
    inline bool Quaternion::operator==(const Quaternion& vector) const
    {
        // no memcmp, float +0.0 and -0.0 are encoded differently
        return x == vector.x && y == vector.y && z == vector.z && w == vector.w;
    }

    //-------------------------------------------------------------------------
    inline bool Quaternion::operator!=(const Quaternion& vector) const
    {
        return x != vector.x || y != vector.y || z != vector.z || w != vector.w;
    }

    //-------------------------------------------------------------------------
    inline void Quaternion::setValues(float inX, float inY, float inZ, float inW)
    {
        GLM_DEBUG_ASSERT_MSG(glmFinite(inX), "Quaternion::set called with not finite scalar (x = %f)", inX);
        GLM_DEBUG_ASSERT_MSG(glmFinite(inY), "Quaternion::set called with not finite scalar (y = %f)", inY);
        GLM_DEBUG_ASSERT_MSG(glmFinite(inZ), "Quaternion::set called with not finite scalar (z = %f)", inZ);
        GLM_DEBUG_ASSERT_MSG(glmFinite(inW), "Quaternion::set called with not finite scalar (w = %f)", inW);
        x = inX;
        y = inY;
        z = inZ;
        w = inW;
    }

    //-------------------------------------------------------------------------
    inline void Quaternion::setValues(const float* inValues)
    {
        GLM_DEBUG_ASSERT_MSG(glmFinite(inValues[0]), "Quaternion::set called with not finite scalar (x = %f)", inValues[0]);
        GLM_DEBUG_ASSERT_MSG(glmFinite(inValues[1]), "Quaternion::set called with not finite scalar (y = %f)", inValues[1]);
        GLM_DEBUG_ASSERT_MSG(glmFinite(inValues[2]), "Quaternion::set called with not finite scalar (z = %f)", inValues[2]);
        GLM_DEBUG_ASSERT_MSG(glmFinite(inValues[3]), "Quaternion::set called with not finite scalar (w = %f)", inValues[3]);

        memcpy(&x, inValues, _quaternionValuesSize);
    }

    //-------------------------------------------------------------------------
    inline void Quaternion::setToIdentity()
    {
        x = y = z = 0.f;
        w = 1.f;
    }

    //-------------------------------------------------------------------------
    inline void Quaternion::setPivotX(Angle angle)
    {
        GLM_DEBUG_ASSERT(glmFinite(angle)); //test valeur infinie ou indefinie
        x = sinf(angle * 0.5f);
        y = z = 0.f;
        w = cosf(angle * 0.5f);
    }

    //-------------------------------------------------------------------------
    inline void Quaternion::setPivotY(Angle angle)
    {
        GLM_DEBUG_ASSERT(glmFinite(angle)); //test valeur infinie ou indefinie
        y = sinf(angle * 0.5f);
        x = z = 0.f;
        w = cosf(angle * 0.5f);
    }

    //-------------------------------------------------------------------------
    inline void Quaternion::setPivotZ(Angle angle)
    {
        GLM_DEBUG_ASSERT(glmFinite(angle)); //test valeur infinie ou indefinie
        z = sinf(angle * 0.5f);
        x = y = 0.f;
        w = cosf(angle * 0.5f);
    }

    //-------------------------------------------------------------------------
    inline Quaternion Quaternion::operator*(const float& scalar) const
    {
        GLM_DEBUG_ASSERT_MSG(glmFinite(scalar), "Quaternion::operator* scalar is not finite");
        return Quaternion(x * scalar, y * scalar, z * scalar, w * scalar);
    }

    //-------------------------------------------------------------------------
    inline Quaternion Quaternion::operator/(const float& scalar) const
    {
        GLM_DEBUG_ASSERT_MSG(scalar != 0, "Quaternion::operator/ scalar is zero");
        GLM_DEBUG_ASSERT_MSG(glmFinite(scalar), "Quaternion::operator/ scalar is not finite");
        return Quaternion(x / scalar, y / scalar, z / scalar, w / scalar);
    }

    //-------------------------------------------------------------------------
    inline Quaternion Quaternion::computeInverse() const // Quaternion must be normalized as input.
    {
        return Quaternion(x, y, z, -w);
    }

    //-------------------------------------------------------------------------
    inline void Quaternion::inverseSelf() // Quaternion must be normalized as input.
    {
        w = -w;
    }

    //-------------------------------------------------------------------------
    inline Quaternion Quaternion::computeConjugate() const
    {
        return Quaternion(x, y, z, -w);
    }

    //-------------------------------------------------------------------------
    inline void Quaternion::conjugateSelf()
    {
        w = -w;
    }

    //-------------------------------------------------------------------------
    inline Vector3 Quaternion::getColumnX() const
    {
        float y2(y + y);
        float z2(z + z);
        return Vector3(1 - ((y * y2) + (z * z2)), (x * y2) + (w * z2), (x * z2) - (w * y2));
    }

    //-------------------------------------------------------------------------
    inline Vector3 Quaternion::getColumnY() const
    {
        float x2(x + x);
        float y2(y + y);
        float z2(z + z);
        return Vector3((x * y2) - (w * z2), 1 - ((x * x2) + (z * z2)), (y * z2) + (w * x2));
    }

    //-------------------------------------------------------------------------
    inline Vector3 Quaternion::getColumnZ() const
    {
        float x2(x + x);
        float y2(y + y);
        float z2(z + z);
        return Vector3((x * z2) + (w * y2), (y * z2) - (w * x2), 1 - ((x * x2) + (y * y2)));
    }

    //-------------------------------------------------------------------------
    inline void Quaternion::getColumnZ(Vector3& returnValue) const
    {
        float x2(x + x);
        float y2(y + y);
        float z2(z + z);
        returnValue.x = (x * z2) + (w * y2);
        returnValue.y = (y * z2) - (w * x2);
        returnValue.z = 1 - ((x * x2) + (y * y2));
    }

    //-------------------------------------------------------------------------
    inline float Quaternion::extractOld2DOrientation() const
    {
        float y2(y + y);
        float z2(z + z);
        return atan2f((x * y2) + (w * z2), 1 - ((y * y2) + (z * z2)));
    }

    //-------------------------------------------------------------------------
    inline Vector3 Quaternion::getRow0() const
    {
        float y2(y + y);
        float z2(z + z);
        return Vector3(1 - ((y * y2) + (z * z2)), (x * y2) - (w * z2), (x * z2) + (w * y2));
    }

    //-------------------------------------------------------------------------
    inline Vector3 Quaternion::getRow1() const
    {
        float x2(x + x);
        float y2(y + y);
        float z2(z + z);
        return Vector3((x * y2) + (w * z2), 1 - ((x * x2) + (z * z2)), (y * z2) - (w * x2));
    }

    //-------------------------------------------------------------------------
    inline Vector3 Quaternion::getRow2() const
    {
        float x2(x + x);
        float y2(y + y);
        float z2(z + z);
        return Vector3((x * z2) - (w * y2), (y * z2) + (w * x2), 1 - ((x * x2) + (y * y2)));
    }

    //-------------------------------------------------------------------------
    inline Quaternion Quaternion::computeRotateZ(Angle az) const
    {
        GLM_DEBUG_ASSERT_MSG(glmFinite(az), "Quaternion::rotateZ angle is not finite");

        Angle az2mi(az * 0.5f);
        float qz2(sinf(az2mi));
        float qz3(cosf(az2mi));

        return Quaternion(qz3 * x - qz2 * y,
                          qz3 * y + qz2 * x,
                          qz3 * z + qz2 * w,
                          qz3 * w - qz2 * z);
    }

    //-------------------------------------------------------------------------
#if GLM_IS_COMPILER_MSVC && (defined(_M_X64) || defined(_M_IA64))
    inline void Quaternion::normalizeSelf()
    {
        __m128 a = _mm_load_ps(&x);
        __m128 dot = glm::vMathVfDot4(a, a);
        a = _mm_mul_ps(a, glm::newtonRapsonRsqrt4(dot));
        _mm_store_ps(&x, a);
    }
#else
    inline void Quaternion::normalizeSelf()
    {
        // note : if mostly used a float specialization based on
        // http://assemblyrequired.crashworks.org/2009/10/20/square-roots-in-vivo-normalizing-vectors/
        // adapted for 4D vectors could be much more efficient
        // currently, we mainly use doubles, so this is not useful

        static const float very_small_float = 1.0e-037f; // from http://altdevblogaday.com/2011/08/21/practical-flt-point-tricks/, adding a very small float avoid testing if == 0
        float factor = (1.f / sqrtf(x * x + y * y + z * z + w * w + very_small_float));
        x *= factor;
        y *= factor;
        z *= factor;
        w *= factor;
    }
#endif

    //-------------------------------------------------------------------------
    inline bool approxEqual(const Quaternion& q1, const Quaternion& q2, float precision)
    {
        return (approxEqual(q1.x, q2.x, precision) && approxEqual(q1.y, q2.y, precision) && approxEqual(q1.z, q2.z, precision) && approxEqual(q1.w, q2.w, precision)) || (approxEqual(q1.x, -q2.x, precision) && approxEqual(q1.y, -q2.y, precision) && approxEqual(q1.z, -q2.z, precision) && approxEqual(q1.w, -q2.w, precision));
    }

    //-----------------------------------------------------------------------------
    inline bool approxDiff(const Quaternion& q1, const Quaternion& q2, float precision)
    {
        return !approxEqual(q1, q2, precision);
    }

    //-------------------------------------------------------------------------
    inline Quaternion operator*(const float& scalar, const Quaternion& vector)
    {
        return Quaternion(vector.x * scalar, vector.y * scalar, vector.z * scalar, vector.w * scalar);
    }
    inline float dot(const Quaternion& q0, const Quaternion& q1)
    {
        return q0.x * q1.x + q0.y * q1.y + q0.z * q1.z + q0.w * q1.w;
    }
    inline Quaternion operator+(const Quaternion& vector1, const Quaternion& vector2)
    {
        return Quaternion(vector1.x + vector2.x, vector1.y + vector2.y, vector1.z + vector2.z, vector1.w + vector2.w);
    }
    inline Quaternion operator-(const Quaternion& vector1, const Quaternion& vector2)
    {
        return Quaternion(vector1.x - vector2.x, vector1.y - vector2.y, vector1.z - vector2.z, vector1.w - vector2.w);
    }

} // namespace glm
