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

#pragma once

#include "glmCoreDefinitions.h"
#include "glmMathBase.h"
#include "glmAssert.h"
#include "glmAngle.h"
#include "glmVector2.h"
#include "glmMemory.h"
#include <xmmintrin.h>
#include <ostream>
#ifdef GLM_IS_COMPILER_GCC
#include <cstring>
#endif

#if GLM_IS_COMPILER_MSVC

#ifdef min
// save min definition and remove it
#define minWasDefined
#pragma push_macro("min")
#undef min
#endif

#ifdef max
// save max definition and remove it
#define maxWasDefined
#pragma push_macro("max")
#undef max
#endif

#endif

namespace glm
{
    class Quaternion;
    class Vector4;

    static const unsigned int _vector3ValuesSize = sizeof(float) * 3;

    class GLM_CORE_API GLM_ALIGN_16 Vector3
    {
    public:
        GLM_CLASS_STAT_ID( glm::MemoryStatIds::GolaemShared);

        float x;
        float y;
        float z;
        float _alignmentValue;

        static const Vector3& getNullVector();
        static const Vector3& getZUpVector();

        Vector3();
        explicit Vector3(float value);
        Vector3(float inX, float inY, float inZ);
        explicit Vector3(const float* inFloatArray);
        Vector3(const Vector3& vector);
        Vector3& operator=(const Vector3& vector);

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

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

        float norm2() const; // computes and returns the squared norm (length) of the vector.
        float norm() const;  // computes and returns the norm (length) of the vector.

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

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

        inline void minSelf(const Vector3& other);
        inline void maxSelf(const Vector3& other);
        inline void clampSelf(const Vector3& min, const Vector3& max);

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

        inline Vector2 project(Axis::Value axis) const;
        inline void project(Axis::Value axis, Vector2& result) const;
        inline Vector2 projectZ() const;
        inline void projectZ(Vector2& result) const;

        inline Vector3 projectAlong(const Vector3& uPVector) const;

        inline Vector3 computeRotateZ(Angle angle) const;
        inline void rotateZSelf(Angle angle);

        inline Vector3 cross(const Vector3& other) const;
        inline float dot(const Vector3& other) const;

        Vector3 computePerpendicular() const;

        inline void mulSelf(const Vector3& vector);
        inline void divSelf(const Vector3& vector);

        inline bool isFinite() const;
    };

    //-------------------------------------------------------------------------
    GLM_CORE_API std::istream& operator>>(std::istream& in, Vector3& vector);
    GLM_CORE_API void rotateVecByQuat(Vector3& result, const Quaternion& rotationQuat, const Vector3& original);
    GLM_CORE_API std::istream& operator>>(std::istream& in, Vector3& vector);
    GLM_CORE_API const char* toString(const Vector3& vector, char* buffer, size_t bufferSize);

    GLM_CORE_API float scalarTripleProduct(const Vector3& v1, const Vector3& v2, const Vector3& v3);

    GLM_CORE_API void computeTetrahedronBarcentricCoords(
        Vector4& result,
        const Vector3& a,
        const Vector3& b,
        const Vector3& c,
        const Vector3& d,
        const Vector3& p);

    //-------------------------------------------------------------------------
    inline Vector3::Vector3()
        :
#ifndef NDEBUG
        x(FLT_MAX)
        , y(FLT_MAX)
        , z(FLT_MAX)
        ,
#endif
        _alignmentValue(0.f) // can bug SSE computations otherwise
    {
    }

    //-------------------------------------------------------------------------
    inline Vector3::Vector3(float value)
        : x(value)
        , y(value)
        , z(value)
        , _alignmentValue(0.f) // can bug SSE computations otherwise
    {
    }

    //-------------------------------------------------------------------------
    inline Vector3::Vector3(float inX, float inY, float inZ)
        : x(inX)
        , y(inY)
        , z(inZ)
        , _alignmentValue(0.f) // can bug SSE computations otherwise
    {
    }

    //-------------------------------------------------------------------------
    inline Vector3::Vector3(const float* inFloatArray)
        : _alignmentValue(0.f)
    {
        memcpy(&x, inFloatArray, _vector3ValuesSize);
    }

    //-------------------------------------------------------------------------
    inline Vector3::Vector3(const Vector3& vector)
        : _alignmentValue(0.f)
    {
        memcpy(&x, &vector.x, _vector3ValuesSize);
    }

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

        return *this;
    }

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

    //-------------------------------------------------------------------------
    inline void Vector3::setNull()
    {
        memset(this, 0, _vector3ValuesSize);
    }

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

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

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

    //-------------------------------------------------------------------------
    inline void Vector3::setValues(float inX, float inY, float inZ)
    {
        GLM_DEBUG_ASSERT_MSG(glmFinite(inX) && glmFinite(inY) && glmFinite(inZ), "Can't setup a vector using a non finite value");
        x = inX;
        y = inY;
        z = inZ;
    }

    //-------------------------------------------------------------------------
    inline void Vector3::setValues(const float* inValues)
    {
        GLM_DEBUG_ASSERT_MSG(glmFinite(inValues[0]) && glmFinite(inValues[1]) && glmFinite(inValues[2]), "Can't setup a vector using a non finite value");
        memcpy(&x, inValues, _vector3ValuesSize);
    }

    //-------------------------------------------------------------------------
    inline const float& Vector3::operator[](unsigned int index) const
    {
        GLM_DEBUG_ASSERT_MSG(index < 3, "This is a 3d vector, it doesn't have a #%lu index.", index);
        return *((&x) + index);
    }

    //-------------------------------------------------------------------------
    inline float& Vector3::operator[](unsigned int index)
    {
        GLM_DEBUG_ASSERT_MSG(index < 3, "This is a 3d vector, it doesn't have a #%lu index.", index);
        return *((&x) + index);
    }

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

    //-------------------------------------------------------------------------
    inline float Vector3::norm() const
    {
        return sqrtf(x * x + y * y + z * z);
    }

    //-------------------------------------------------------------------------
    inline __m128 SSERSqrtNR(const __m128 x)
    {
        __m128 recip = _mm_rsqrt_ss(x);           // "estimate" opcode
        const static __m128 three = {3, 3, 3, 3}; // aligned consts for fast load
        const static __m128 half = {0.5f, 0.5f, 0.5f, 0.5f};
        __m128 halfrecip = _mm_mul_ss(half, recip);
        __m128 threeminus_xrr = _mm_sub_ss(three, _mm_mul_ss(x, _mm_mul_ss(recip, recip)));
        return _mm_mul_ss(halfrecip, threeminus_xrr);
    }

    //-------------------------------------------------------------------------
    inline void Vector3::normalizeSelf()
    {
        // note : implementation from
        // http://assemblyrequired.crashworks.org/2009/10/20/square-roots-in-vivo-normalizing-vectors/

        // load as a SIMD vector
        const __m128 vec = _mm_load_ps(&x);
        // compute a dot product by computing the square, and
        // then rotating the vector and adding, so that the
        // dot ends up in the low term (used by the scalar ops)
        __m128 dot = _mm_mul_ps(vec, vec);
        // rotate x under y and add together
        __m128 rotated = _mm_shuffle_ps(dot, dot, _MM_SHUFFLE(0, 3, 2, 1));  // YZWX ( shuffle macro is high to low word )
        dot = _mm_add_ss(dot, rotated);                                      // x^2 + y^2 in the low word
        rotated = _mm_shuffle_ps(rotated, rotated, _MM_SHUFFLE(0, 3, 2, 1)); // ZWXY
        dot = _mm_add_ss(dot, rotated);                                      // x^2 + y^2 + z^2 in the low word

        // from http://altdevblogaday.com/2011/08/21/practical-flt-point-tricks/, adding a very small float avoid testing if == 0
        const static __m128 minimalFloatValueToAvoidTestingForNullity = {1.0e-037f, 0.f, 0.f, 0.f};
        dot = _mm_add_ss(dot, minimalFloatValueToAvoidTestingForNullity); // x^2 + y^2 + z^2 in the low word, not null

        __m128 recipsqrt = SSERSqrtNR(dot);                                        // contains reciprocal square root in low term
        recipsqrt = _mm_shuffle_ps(recipsqrt, recipsqrt, _MM_SHUFFLE(0, 0, 0, 0)); // broadcast low term to all words

        // multiply 1/sqrtf(dotproduct) against all vector components, and write back
        const __m128 normalized = _mm_mul_ps(vec, recipsqrt);
        _mm_store_ps(&x, normalized);
    }

    //void normalizeSelf(/*float desiredNorm = 1.f*/)
    //{
    //	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 thisNorm2 = norm2() + very_small_float;
    //	float factor(1.f/*desiredNorm*/ * (  1.0 / sqrtf( thisNorm2 ) ) );
    //	x *= factor;
    //	y *= factor;
    //	z *= factor;
    //}

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

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

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

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

    //-------------------------------------------------------------------------
    inline Vector3& Vector3::operator-=(const Vector3& vector)
    {
        //*(__m128*)&x = _mm_sub_ps( *(__m128*)&x, *(__m128*)&vector.x );

        x -= vector.x;
        y -= vector.y;
        z -= vector.z;
        return *this;
    }

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

    //-------------------------------------------------------------------------
    inline Vector3& Vector3::operator/=(float scalar)
    {
        GLM_DEBUG_ASSERT_MSG(glmFinite(scalar), "Undefined scalar");
        GLM_DEBUG_ASSERT_MSG(scalar != 0, "Division by 0!");
        x /= scalar;
        y /= scalar;
        z /= scalar;
        return *this;
    }

    //-------------------------------------------------------------------------
    inline bool Vector3::operator<(const Vector3& 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;

        return false;
    }

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

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

    //-------------------------------------------------------------------------
    inline Vector2 Vector3::project(Axis::Value axis) const
    {
        switch (axis)
        {
        case Axis::X:
            return Vector2(y, z);
        case Axis::Y:
            return Vector2(x, z);
        case Axis::Z:
            return Vector2(x, y);
        default:
            GLM_EXIT("Unknown axis given");
        }
    }

    //-------------------------------------------------------------------------
    inline void Vector3::project(Axis::Value axis, Vector2& result) const
    {
        switch (axis)
        {
        case Axis::X:
            result.setValues(y, z);
            return;
        case Axis::Y:
            result.setValues(x, z);
            return;
        case Axis::Z:
            result.setValues(x, y);
            return;
        default:
            GLM_EXIT("Unknown axis given");
        }
    }

    //-------------------------------------------------------------------------
    inline Vector2 Vector3::projectZ() const
    {
        return Vector2(x, y);
    }

    //-------------------------------------------------------------------------
    inline void Vector3::projectZ(Vector2& result) const
    {
        result.x = x;
        result.y = y;
    }

    //-------------------------------------------------------------------------
    inline Vector3 Vector3::computeRotateZ(Angle angle) const
    {
        Vector3 result;
        float cz(cosf(angle));
        float sz(sinf(angle));
        result.setValues(x * cz - y * sz, x * sz + y * cz, z);
        return result;
    }

    //-------------------------------------------------------------------------
    inline void Vector3::rotateZSelf(Angle angle)
    {
        float cz(cosf(angle));
        float sz(sinf(angle));
        setValues(x * cz - y * sz, x * sz + y * cz, z);
    }

    //-------------------------------------------------------------------------
    inline Vector3 Vector3::cross(const Vector3& other) const
    {
        return Vector3(
            y * other.z - z * other.y,
            z * other.x - x * other.z,
            x * other.y - y * other.x);
    }

    //-------------------------------------------------------------------------
    inline float Vector3::dot(const Vector3& other) const
    {
        return x * other.x + y * other.y + z * other.z;
    }

    //-------------------------------------------------------------------------
    inline void Vector3::mulSelf(const Vector3& vector)
    {
        x *= vector.x;
        y *= vector.y;
        z *= vector.z;
    }

    //-------------------------------------------------------------------------
    inline void Vector3::divSelf(const Vector3& vector)
    {
        x /= vector.x;
        y /= vector.y;
        z /= vector.z;
    }

    //-------------------------------------------------------------------------
    inline bool approxEqual(const Vector3& v1, const Vector3& v2, float precision)
    {
        return approxEqual(v1.x, v2.x, precision) && approxEqual(v1.y, v2.y, precision) && approxEqual(v1.z, v2.z, precision);
    }

    //-------------------------------------------------------------------------
    inline bool approxDiff(const Vector3& v1, const Vector3& v2, float precision)
    {
        return approxDiff(v1.x, v2.x, precision) || approxDiff(v1.y, v2.y, precision) || approxDiff(v1.z, v2.z, precision);
    }

    // Bound a value in an interval.
#if GLM_IS_COMPILER_MSVC && (defined(_M_X64) || defined(_M_IA64))
    inline void min(const Vector3& a, const Vector3& b, Vector3& result)
    {
        // Branchless SSE min.
        _mm_store_ps(&result.x, _mm_min_ps(_mm_load_ps(&a.x), _mm_load_ps(&b.x)));
    }

    inline void max(const Vector3& a, const Vector3& b, Vector3& result)
    {
        // Branchless SSE min.
        _mm_store_ps(&result.x, _mm_max_ps(_mm_load_ps(&a.x), _mm_load_ps(&b.x)));
    }

    inline void clamp(const Vector3& value, const Vector3& min, const Vector3& max, Vector3& result)
    {
        // Branchless SSE clamp.
        _mm_store_ps(&result.x, _mm_min_ps(_mm_max_ps(_mm_load_ps(&value.x), _mm_load_ps(&min.x)), _mm_load_ps(&max.x)));
    }
#else
    inline void min(const Vector3& a, const Vector3& b, Vector3& result)
    {
        result.x = min(a.x, b.x);
        result.y = min(a.y, b.y);
        result.z = min(a.z, b.z);
    }

    inline void max(const Vector3& a, const Vector3& b, Vector3& result)
    {
        result.x = max(a.x, b.x);
        result.y = max(a.y, b.y);
        result.z = max(a.z, b.z);
    }

    inline void clamp(const Vector3& value, const Vector3& a, const Vector3& b, Vector3& result)
    {
        result.x = clamp(value.x, a.x, b.x);
        result.y = clamp(value.y, a.y, b.y);
        result.z = clamp(value.z, a.z, b.z);
    }
#endif

    // Bound a value in an interval.
#if GLM_IS_COMPILER_MSVC && (defined(_M_X64) || defined(_M_IA64))
    inline void Vector3::minSelf(const Vector3& other)
    {
        // Branchless SSE min.
        _mm_store_ps(&x, _mm_min_ps(_mm_load_ps(&x), _mm_load_ps(&other.x)));
    }

    inline void Vector3::maxSelf(const Vector3& other)
    {
        // Branchless SSE min.
        _mm_store_ps(&x, _mm_max_ps(_mm_load_ps(&x), _mm_load_ps(&other.x)));
    }

    inline void Vector3::clampSelf(const Vector3& min, const Vector3& max)
    {
        // Branchless SSE clamp.
        _mm_store_ps(&x, _mm_min_ps(_mm_max_ps(_mm_load_ps(&x), _mm_load_ps(&min.x)), _mm_load_ps(&max.x)));
    }
#else
    inline void Vector3::minSelf(const Vector3& other)
    {
        x = min(x, other.x);
        y = min(y, other.y);
        z = min(z, other.z);
    }

    inline void Vector3::maxSelf(const Vector3& other)
    {
        x = max(x, other.x);
        y = max(y, other.y);
        z = max(z, other.z);
    }

    inline void Vector3::clampSelf(const Vector3& min, const Vector3& max)
    {
        x = clamp(x, min.x, max.x);
        y = clamp(y, min.y, max.y);
        z = clamp(z, min.z, max.z);
    }
#endif

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

    //-----------------------------------------------------------------------------
    inline Vector3 operator^(const Vector3& vector1, const Vector3& vector2)
    {
        return Vector3(
            vector1.y * vector2.z - vector1.z * vector2.y,
            vector1.z * vector2.x - vector1.x * vector2.z,
            vector1.x * vector2.y - vector1.y * vector2.x);
    }

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

    //-----------------------------------------------------------------------------
    inline void memberToMemberProduct(const Vector3& v1, const Vector3& v2, Vector3& result)
    {
        result.x = v1.x * v2.x;
        result.y = v1.y * v2.y;
        result.z = v1.z * v2.z;
    }

    //-----------------------------------------------------------------------------
    inline Vector3 memberToMemberProduct(const Vector3& v1, const Vector3& v2)
    {
        return Vector3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z);
    }

    //-----------------------------------------------------------------------------
    inline float max(const Vector3& vector)
    {
        return max(vector.z, max(vector.x, vector.y));
    }

    //-------------------------------------------------------------------------
    inline Vector3 Vector3::projectAlong(const Vector3& upVector) const
    {
        GLM_DEBUG_ASSERT(upVector.isNormalized());
        return (*this - dot(upVector) * upVector);
    }

    //-----------------------------------------------------------------------------
    inline bool vector2AtRightOfVector1(const Vector3& vector1, const Vector3& vector2, const Vector3 upvector)
    {
        // we get the right vector (right handed referential in golaem, up is "z", v1 is "x")

        // if v2 has a positive dot with side, it is on the right, else on the left
        return vector1 == vector2 || vector1.cross(upvector).dot(vector2) >= -GLM_EPSILON; // same tolerance as in signedArea
    }

    //-----------------------------------------------------------------------------
    inline bool Vector3::isFinite() const
    {
        return glmFinite(x) && glmFinite(y) && glmFinite(z);
    }
} // namespace glm

#if GLM_IS_COMPILER_MSVC

#ifdef minWasDefined
// restore min definition
#pragma pop_macro("min")
#endif

#ifdef maxWasDefined
// restore max definition
#pragma pop_macro("max")
#endif

#endif
