///////////////////////////////////////////////////////////////////////////////////
/// OpenGL Mathematics (glm.g-truc.net)
///
/// Copyright (c) 2005 - 2015 G-Truc Creation (www.g-truc.net)
/// Permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to deal
/// in the Software without restriction, including without limitation the rights
/// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
/// copies of the Software, and to permit persons to whom the Software is
/// furnished to do so, subject to the following conditions:
/// 
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions of the Software.
/// 
/// Restrictions:
///             By making use of the Software for military purposes, you choose to make
///             a Bunny unhappy.
/// 
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
/// THE SOFTWARE.
///
/// @ref gtx_quaternion
/// @file glm/gtx/quaternion.inl
/// @date 2005-12-21 / 2011-06-07
/// @author Christophe Riccio
///////////////////////////////////////////////////////////////////////////////////

#include <limits>
#include "../gtc/constants.hpp"

namespace glm
{
        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tvec3<T, P> cross
        (
                tvec3<T, P> const & v,
                tquat<T, P> const & q
        )
        {
                return inverse(q) * v;
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tvec3<T, P> cross
        (
                tquat<T, P> const & q,
                tvec3<T, P> const & v
        )
        {
                return q * v;
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tquat<T, P> squad
        (
                tquat<T, P> const & q1,
                tquat<T, P> const & q2,
                tquat<T, P> const & s1,
                tquat<T, P> const & s2,
                T const & h)
        {
                return mix(mix(q1, q2, h), mix(s1, s2, h), static_cast<T>(2) * (static_cast<T>(1) - h) * h);
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tquat<T, P> intermediate
        (
                tquat<T, P> const & prev,
                tquat<T, P> const & curr,
                tquat<T, P> const & next
        )
        {
                tquat<T, P> invQuat = inverse(curr);
                return exp((log(next + invQuat) + log(prev + invQuat)) / static_cast<T>(-4)) * curr;
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tquat<T, P> exp
        (
                tquat<T, P> const & q
        )
        {
                tvec3<T, P> u(q.x, q.y, q.z);
                T Angle = glm::length(u);
                if (Angle < epsilon<T>())
                        return tquat<T, P>();

                tvec3<T, P> v(u / Angle);
                return tquat<T, P>(cos(Angle), sin(Angle) * v);
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tquat<T, P> log
        (
                tquat<T, P> const & q
        )
        {
                tvec3<T, P> u(q.x, q.y, q.z);
                T Vec3Len = length(u);

                if (Vec3Len < epsilon<T>())
                {
                        if(q.w > static_cast<T>(0))
                                return tquat<T, P>(log(q.w), static_cast<T>(0), static_cast<T>(0), static_cast<T>(0));
                        else if(q.w < static_cast<T>(0))
                                return tquat<T, P>(log(-q.w), pi<T>(), static_cast<T>(0), static_cast<T>(0));
                        else
                                return tquat<T, P>(std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity());
                }
                else
                {
                        T QuatLen = sqrt(Vec3Len * Vec3Len + q.w * q.w);
                        T t = atan(Vec3Len, T(q.w)) / Vec3Len;
                        return tquat<T, P>(log(QuatLen), t * q.x, t * q.y, t * q.z);
                }
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tquat<T, P> pow
        (
                tquat<T, P> const & x,
                T const & y
        )
        {
                if(abs(x.w) > (static_cast<T>(1) - epsilon<T>()))
                        return x;
                T Angle = acos(y);
                T NewAngle = Angle * y;
                T Div = sin(NewAngle) / sin(Angle);
                return tquat<T, P>(
                        cos(NewAngle),
                        x.x * Div,
                        x.y * Div,
                        x.z * Div);
        }

        //template <typename T, precision P>
        //GLM_FUNC_QUALIFIER tquat<T, P> sqrt
        //(
        //      tquat<T, P> const & q
        //)
        //{
        //      T q0 = static_cast<T>(1) - dot(q, q);
        //      return T(2) * (T(1) + q0) * q;
        //}

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tvec3<T, P> rotate
        (
                tquat<T, P> const & q,
                tvec3<T, P> const & v
        )
        {
                return q * v;
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tvec4<T, P> rotate
        (
                tquat<T, P> const & q,
                tvec4<T, P> const & v
        )
        {
                return q * v;
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER T extractRealComponent
        (
                tquat<T, P> const & q
        )
        {
                T w = static_cast<T>(1) - q.x * q.x - q.y * q.y - q.z * q.z;
                if(w < T(0))
                        return T(0);
                else
                        return -sqrt(w);
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER T length2
        (
                tquat<T, P> const & q
        )
        {
                return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tquat<T, P> shortMix
        (
                tquat<T, P> const & x,
                tquat<T, P> const & y,
                T const & a
        )
        {
                if(a <= static_cast<T>(0)) return x;
                if(a >= static_cast<T>(1)) return y;

                T fCos = dot(x, y);
                tquat<T, P> y2(y); //BUG!!! tquat<T> y2;
                if(fCos < static_cast<T>(0))
                {
                        y2 = -y;
                        fCos = -fCos;
                }

                //if(fCos > 1.0f) // problem
                T k0, k1;
                if(fCos > (static_cast<T>(1) - epsilon<T>()))
                {
                        k0 = static_cast<T>(1) - a;
                        k1 = static_cast<T>(0) + a; //BUG!!! 1.0f + a;
                }
                else
                {
                        T fSin = sqrt(T(1) - fCos * fCos);
                        T fAngle = atan(fSin, fCos);
                        T fOneOverSin = static_cast<T>(1) / fSin;
                        k0 = sin((static_cast<T>(1) - a) * fAngle) * fOneOverSin;
                        k1 = sin((static_cast<T>(0) + a) * fAngle) * fOneOverSin;
                }

                return tquat<T, P>(
                        k0 * x.w + k1 * y2.w,
                        k0 * x.x + k1 * y2.x,
                        k0 * x.y + k1 * y2.y,
                        k0 * x.z + k1 * y2.z);
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tquat<T, P> fastMix
        (
                tquat<T, P> const & x,
                tquat<T, P> const & y,
                T const & a
        )
        {
                return glm::normalize(x * (static_cast<T>(1) - a) + (y * a));
        }

        template <typename T, precision P>
        GLM_FUNC_QUALIFIER tquat<T, P> rotation
        (
                tvec3<T, P> const & orig,
                tvec3<T, P> const & dest
        )
        {
                T cosTheta = dot(orig, dest);
                tvec3<T, P> rotationAxis;

                if(cosTheta < static_cast<T>(-1) + epsilon<T>())
                {
                        // special case when vectors in opposite directions :
                        // there is no "ideal" rotation axis
                        // So guess one; any will do as long as it's perpendicular to start
                        // This implementation favors a rotation around the Up axis (Y),
                        // since it's often what you want to do.
                        rotationAxis = cross(tvec3<T, P>(0, 0, 1), orig);
                        if(length2(rotationAxis) < epsilon<T>()) // bad luck, they were parallel, try again!
                                rotationAxis = cross(tvec3<T, P>(1, 0, 0), orig);

                        rotationAxis = normalize(rotationAxis);
                        return angleAxis(pi<T>(), rotationAxis);
                }

                // Implementation from Stan Melax's Game Programming Gems 1 article
                rotationAxis = cross(orig, dest);

                T s = sqrt((T(1) + cosTheta) * static_cast<T>(2));
                T invs = static_cast<T>(1) / s;

                return tquat<T, P>(
                        s * static_cast<T>(0.5f), 
                        rotationAxis.x * invs,
                        rotationAxis.y * invs,
                        rotationAxis.z * invs);
        }

}//namespace glm