//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

//Distributed under the Boost Software License, Version 1.0. (See accompanying

//file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef UUID_E6519754D19211DFB8405F74DFD72085

#define UUID_E6519754D19211DFB8405F74DFD72085

#include <boost/qvm/inline.hpp>

#include <boost/qvm/enable_if.hpp>

#include <boost/qvm/deduce_quat.hpp>

#include <boost/qvm/mat_traits.hpp>

#include <boost/qvm/scalar_traits.hpp>

#include <boost/qvm/math.hpp>

#include <boost/qvm/assert.hpp>

#include <boost/qvm/error.hpp>

#include <boost/qvm/throw_exception.hpp>

#include <string>

namespace

boost

    {

    namespace

    qvm

        {

        namespace

        qvm_detail

            {

            BOOST_QVM_INLINE_CRITICAL

            void const *

            get_valid_ptr_quat_operations()

                {

                static int const obj=0;

                return &obj;

                }

            }

        ////////////////////////////////////////////////

        namespace

        msvc_parse_bug_workaround

            {

            template <class A,class B>

            struct

            quats

                {

                static bool const value=is_quat<A>::value && is_quat<B>::value;

                };

            }

        namespace

        qvm_to_string_detail

            {

            template <class T>

            std::string to_string( T const & x );

            }

        template <class A>

        inline

        typename boost::enable_if_c<

            is_quat<A>::value,

            std::string>::type

        to_string( A const & a )

            {

            using namespace qvm_to_string_detail;

            return '('+

                to_string(quat_traits<A>::template read_element<0>(a))+','+

                to_string(quat_traits<A>::template read_element<1>(a))+','+

                to_string(quat_traits<A>::template read_element<2>(a))+','+

                to_string(quat_traits<A>::template read_element<3>(a))+')';

            }

        ////////////////////////////////////////////////

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value && is_quat<B>::value,

            A &>::type

        assign( A & a, B const & b )

            {

            quat_traits<A>::template write_element<0>(a) = quat_traits<B>::template read_element<0>(b);

            quat_traits<A>::template write_element<1>(a) = quat_traits<B>::template read_element<1>(b);

            quat_traits<A>::template write_element<2>(a) = quat_traits<B>::template read_element<2>(b);

            quat_traits<A>::template write_element<3>(a) = quat_traits<B>::template read_element<3>(b);

            return a;

            }

        template <class A,class B,class Cmp>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value && is_quat<B>::value,

            bool>::type

        cmp( A const & a, B const & b, Cmp f )

            {

            typedef typename deduce_scalar<

                typename quat_traits<A>::scalar_type,

                typename quat_traits<B>::scalar_type>::type T;

            T q1[4] =

                {

                quat_traits<A>::template read_element<0>(a),

                quat_traits<A>::template read_element<1>(a),

                quat_traits<A>::template read_element<2>(a),

                quat_traits<A>::template read_element<3>(a)

                };

            T q2[4] =

                {

                quat_traits<B>::template read_element<0>(b),

                quat_traits<B>::template read_element<1>(b),

                quat_traits<B>::template read_element<2>(b),

                quat_traits<B>::template read_element<3>(b)

                };

            int i;

            for( i=0; i!=4; ++i )

                if( !f(q1[i],q2[i]) )

                    break;

            if( i==4 )

                return true;

            for( i=0; i!=4; ++i )

                if( !f(q1[i],-q2[i]) )

                    return false;

            return true;

            }

        ////////////////////////////////////////////////

        template <class R,class A>

        BOOST_QVM_INLINE_TRIVIAL

        typename enable_if_c<

            is_quat<R>::value && is_quat<A>::value,

            R>::type

        convert_to( A const & a )

            {

            R r;

            quat_traits<R>::template write_element<0>(r) = quat_traits<A>::template read_element<0>(a);

            quat_traits<R>::template write_element<1>(r) = quat_traits<A>::template read_element<1>(a);

            quat_traits<R>::template write_element<2>(r) = quat_traits<A>::template read_element<2>(a);

            quat_traits<R>::template write_element<3>(r) = quat_traits<A>::template read_element<3>(a);

            return r;

            }

        template <class R,class A>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<R>::value && is_mat<A>::value &&

            mat_traits<A>::rows==3 && mat_traits<A>::cols==3,

            R>::type

        convert_to( A const & a )

            {

            typedef typename mat_traits<A>::scalar_type T;

            T const mat[3][3] =

                {

                    { mat_traits<A>::template read_element<0,0>(a), mat_traits<A>::template read_element<0,1>(a), mat_traits<A>::template read_element<0,2>(a) },

                    { mat_traits<A>::template read_element<1,0>(a), mat_traits<A>::template read_element<1,1>(a), mat_traits<A>::template read_element<1,2>(a) },

                    { mat_traits<A>::template read_element<2,0>(a), mat_traits<A>::template read_element<2,1>(a), mat_traits<A>::template read_element<2,2>(a) }

                };

            R r;

            if( mat[0][0]+mat[1][1]+mat[2][2] > scalar_traits<T>::value(0) )

                {

                T t = mat[0][0] + mat[1][1] + mat[2][2] + scalar_traits<T>::value(1);

                T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;

                quat_traits<R>::template write_element<0>(r)=s*t;

                quat_traits<R>::template write_element<1>(r)=(mat[2][1]-mat[1][2])*s;

                quat_traits<R>::template write_element<2>(r)=(mat[0][2]-mat[2][0])*s;

                quat_traits<R>::template write_element<3>(r)=(mat[1][0]-mat[0][1])*s;

                }

            else if( mat[0][0]>mat[1][1] && mat[0][0]>mat[2][2] )

                {

                T t = mat[0][0] - mat[1][1] - mat[2][2] + scalar_traits<T>::value(1);

                T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;

                quat_traits<R>::template write_element<0>(r)=(mat[2][1]-mat[1][2])*s;

                quat_traits<R>::template write_element<1>(r)=s*t;

                quat_traits<R>::template write_element<2>(r)=(mat[1][0]+mat[0][1])*s;

                quat_traits<R>::template write_element<3>(r)=(mat[0][2]+mat[2][0])*s;

                }

            else if( mat[1][1]>mat[2][2] )

                {

                T t = - mat[0][0] + mat[1][1] - mat[2][2] + scalar_traits<T>::value(1);

                T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;

                quat_traits<R>::template write_element<0>(r)=(mat[0][2]-mat[2][0])*s;

                quat_traits<R>::template write_element<1>(r)=(mat[1][0]+mat[0][1])*s;

                quat_traits<R>::template write_element<2>(r)=s*t;

                quat_traits<R>::template write_element<3>(r)=(mat[2][1]+mat[1][2])*s;

                }

            else

                {

                T t = - mat[0][0] - mat[1][1] + mat[2][2] + scalar_traits<T>::value(1);

                T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;

                quat_traits<R>::template write_element<0>(r)=(mat[1][0]-mat[0][1])*s;

                quat_traits<R>::template write_element<1>(r)=(mat[0][2]+mat[2][0])*s;

                quat_traits<R>::template write_element<2>(r)=(mat[2][1]+mat[1][2])*s;

                quat_traits<R>::template write_element<3>(r)=s*t;

                }

            return r;

            }

        ////////////////////////////////////////////////

        template <class A>

        BOOST_QVM_INLINE_OPERATIONS

        typename lazy_enable_if_c<

            is_quat<A>::value,

            deduce_quat<A> >::type

        conjugate( A const & a )

            {

            typedef typename deduce_quat<A>::type R;

            R r;

            quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a);

            quat_traits<R>::template write_element<1>(r)=-quat_traits<A>::template read_element<1>(a);

            quat_traits<R>::template write_element<2>(r)=-quat_traits<A>::template read_element<2>(a);

            quat_traits<R>::template write_element<3>(r)=-quat_traits<A>::template read_element<3>(a);

            return r;

            }

        ////////////////////////////////////////////////

        namespace

        qvm_detail

            {

            template <class T>

            class

            identity_quat_

                {

                identity_quat_( identity_quat_ const & );

                identity_quat_ & operator=( identity_quat_ const & );

                ~identity_quat_();

                public:

                template <class R>

                BOOST_QVM_INLINE_TRIVIAL

                operator R() const

                    {

                    R r;

                    assign(r,*this);

                    return r;

                    }

                };

            }

        template <class T>

        struct

        quat_traits< qvm_detail::identity_quat_<T> >

            {

            typedef qvm_detail::identity_quat_<T> this_quaternion;

            typedef T scalar_type;

            template <int I>

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type

            read_element( this_quaternion const & x )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<4);

                return scalar_traits<T>::value(I==0);

                }

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type

            read_element_idx( int i, this_quaternion const & x )

                {

                BOOST_QVM_ASSERT(i>=0);

                BOOST_QVM_ASSERT(i<4);

                return scalar_traits<T>::value(i==0);

                }

            };

        template <class T>

        struct

        deduce_quat< qvm_detail::identity_quat_<T> >

            {

            typedef quat<T> type;

            };

        template <class T>

        struct

        deduce_quat2< qvm_detail::identity_quat_<T>, qvm_detail::identity_quat_<T> >

            {

            typedef quat<T> type;

            };

        template <class T>

        BOOST_QVM_INLINE_TRIVIAL

        qvm_detail::identity_quat_<T> const &

        identity_quat()

            {

            return *(qvm_detail::identity_quat_<T> const *)qvm_detail::get_valid_ptr_quat_operations();

            }

        template <class A>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value,

            void>::type

        set_identity( A & a )

            {

            typedef typename quat_traits<A>::scalar_type T;

            T const zero=scalar_traits<T>::value(0);

            T const one=scalar_traits<T>::value(1);

            quat_traits<A>::template write_element<0>(a) = one;

            quat_traits<A>::template write_element<1>(a) = zero;

            quat_traits<A>::template write_element<2>(a) = zero;

            quat_traits<A>::template write_element<3>(a) = zero;

            }

        ////////////////////////////////////////////////

        namespace

        qvm_detail

            {

            template <class OriginalType,class Scalar>

            class

            quaternion_scalar_cast_

                {

                quaternion_scalar_cast_( quaternion_scalar_cast_ const & );

                quaternion_scalar_cast_ & operator=( quaternion_scalar_cast_ const & );

                ~quaternion_scalar_cast_();

                public:

                template <class T>

                BOOST_QVM_INLINE_TRIVIAL

                quaternion_scalar_cast_ &

                operator=( T const & x )

                    {

                    assign(*this,x);

                    return *this;

                    }

                template <class R>

                BOOST_QVM_INLINE_TRIVIAL

                operator R() const

                    {

                    R r;

                    assign(r,*this);

                    return r;

                    }

                };

            template <bool> struct scalar_cast_quaternion_filter { };

            template <> struct scalar_cast_quaternion_filter<true> { typedef int type; };

            }

        template <class OriginalType,class Scalar>

        struct

        quat_traits< qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> >

            {

            typedef Scalar scalar_type;

            typedef qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> this_quaternion;

            template <int I>

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type

            read_element( this_quaternion const & x )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<4);

                return scalar_type(quat_traits<OriginalType>::template read_element<I>(reinterpret_cast<OriginalType const &>(x)));

                }

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type

            read_element_idx( int i, this_quaternion const & x )

                {

                BOOST_QVM_ASSERT(i>=0);

                BOOST_QVM_ASSERT(i<4);

                return scalar_type(quat_traits<OriginalType>::read_element_idx(i,reinterpret_cast<OriginalType const &>(x)));

                }

            };

        template <class Scalar,class T>

        BOOST_QVM_INLINE_TRIVIAL

        qvm_detail::quaternion_scalar_cast_<T,Scalar> const &

        scalar_cast( T const & x, typename qvm_detail::scalar_cast_quaternion_filter<is_quat<T>::value>::type=0 )

            {

            return reinterpret_cast<qvm_detail::quaternion_scalar_cast_<T,Scalar> const &>(x);

            }

        ////////////////////////////////////////////////

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value && is_scalar<B>::value,

            A &>::type

        operator/=( A & a, B b )

            {

            quat_traits<A>::template write_element<0>(a)/=b;

            quat_traits<A>::template write_element<1>(a)/=b;

            quat_traits<A>::template write_element<2>(a)/=b;

            quat_traits<A>::template write_element<3>(a)/=b;

            return a;

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename lazy_enable_if_c<

            is_quat<A>::value && is_scalar<B>::value,

            deduce_quat<A> >::type

        operator/( A const & a, B b )

            {

            typedef typename deduce_quat<A>::type R;

            R r;

            quat_traits<R>::template write_element<0>(r) = quat_traits<A>::template read_element<0>(a)/b;

            quat_traits<R>::template write_element<1>(r) = quat_traits<A>::template read_element<1>(a)/b;

            quat_traits<R>::template write_element<2>(r) = quat_traits<A>::template read_element<2>(a)/b;

            quat_traits<R>::template write_element<3>(r) = quat_traits<A>::template read_element<3>(a)/b;

            return r;

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename lazy_enable_if_c<

            is_quat<A>::value && is_quat<B>::value,

            deduce_scalar<typename quat_traits<A>::scalar_type,typename quat_traits<B>::scalar_type> >::type

        dot( A const & a, B const & b )

            {

            typedef typename quat_traits<A>::scalar_type Ta;

            typedef typename quat_traits<B>::scalar_type Tb;

            typedef typename deduce_scalar<Ta,Tb>::type Tr;

            Ta const a0=quat_traits<A>::template read_element<0>(a);

            Ta const a1=quat_traits<A>::template read_element<1>(a);

            Ta const a2=quat_traits<A>::template read_element<2>(a);

            Ta const a3=quat_traits<A>::template read_element<3>(a);

            Tb const b0=quat_traits<B>::template read_element<0>(b);

            Tb const b1=quat_traits<B>::template read_element<1>(b);

            Tb const b2=quat_traits<B>::template read_element<2>(b);

            Tb const b3=quat_traits<B>::template read_element<3>(b);

            Tr const dp=a0*b0+a1*b1+a2*b2+a3*b3;

            return dp;

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value && is_quat<B>::value,

            bool>::type

        operator==( A const & a, B const & b )

            {

            return

                quat_traits<A>::template read_element<0>(a)==quat_traits<B>::template read_element<0>(b) &&

                quat_traits<A>::template read_element<1>(a)==quat_traits<B>::template read_element<1>(b) &&

                quat_traits<A>::template read_element<2>(a)==quat_traits<B>::template read_element<2>(b) &&

                quat_traits<A>::template read_element<3>(a)==quat_traits<B>::template read_element<3>(b);

            }

        template <class A>

        BOOST_QVM_INLINE_OPERATIONS

        typename lazy_enable_if_c<

            is_quat<A>::value,

            deduce_quat<A> >::type

        inverse( A const & a )

            {

            typedef typename deduce_quat<A>::type R;

            typedef typename quat_traits<A>::scalar_type TA;

            TA aa = quat_traits<A>::template read_element<0>(a);

            TA ab = quat_traits<A>::template read_element<1>(a);

            TA ac = quat_traits<A>::template read_element<2>(a);

            TA ad = quat_traits<A>::template read_element<3>(a);

            TA m2 = ab*ab + ac*ac + ad*ad + aa*aa;

            if( m2==scalar_traits<TA>::value(0) )

                BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());

            TA rm=scalar_traits<TA>::value(1)/m2;

            R r;

            quat_traits<R>::template write_element<0>(r) = aa*rm;

            quat_traits<R>::template write_element<1>(r) = -ab*rm;

            quat_traits<R>::template write_element<2>(r) = -ac*rm;

            quat_traits<R>::template write_element<3>(r) = -ad*rm;

            return r;

            }

        template <class A>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value,

            typename quat_traits<A>::scalar_type>::type

        mag_sqr( A const & a )

            {

            typedef typename quat_traits<A>::scalar_type T;

            T x=quat_traits<A>::template read_element<0>(a);

            T y=quat_traits<A>::template read_element<1>(a);

            T z=quat_traits<A>::template read_element<2>(a);

            T w=quat_traits<A>::template read_element<3>(a);

            return x*x+y*y+z*z+w*w;

            }

        template <class A>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value,

            typename quat_traits<A>::scalar_type>::type

        mag( A const & a )

            {

            typedef typename quat_traits<A>::scalar_type T;

            T x=quat_traits<A>::template read_element<0>(a);

            T y=quat_traits<A>::template read_element<1>(a);

            T z=quat_traits<A>::template read_element<2>(a);

            T w=quat_traits<A>::template read_element<3>(a);

            return sqrt<T>(x*x+y*y+z*z+w*w);

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if<

            msvc_parse_bug_workaround::quats<A,B>,

            A &>::type

        operator-=( A & a, B const & b )

            {

            quat_traits<A>::template write_element<0>(a)-=quat_traits<B>::template read_element<0>(b);

            quat_traits<A>::template write_element<1>(a)-=quat_traits<B>::template read_element<1>(b);

            quat_traits<A>::template write_element<2>(a)-=quat_traits<B>::template read_element<2>(b);

            quat_traits<A>::template write_element<3>(a)-=quat_traits<B>::template read_element<3>(b);

            return a;

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename lazy_enable_if_c<

            is_quat<A>::value && is_quat<B>::value,

            deduce_quat2<A,B> >::type

        operator-( A const & a, B const & b )

            {

            typedef typename deduce_quat2<A,B>::type R;

            R r;

            quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)-quat_traits<B>::template read_element<0>(b);

            quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)-quat_traits<B>::template read_element<1>(b);

            quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)-quat_traits<B>::template read_element<2>(b);

            quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)-quat_traits<B>::template read_element<3>(b);

            return r;

            }

        template <class A>

        BOOST_QVM_INLINE_OPERATIONS

        typename lazy_enable_if_c<

            is_quat<A>::value,

            deduce_quat<A> >::type

        operator-( A const & a )

            {

            typedef typename deduce_quat<A>::type R;

            R r;

            quat_traits<R>::template write_element<0>(r)=-quat_traits<A>::template read_element<0>(a);

            quat_traits<R>::template write_element<1>(r)=-quat_traits<A>::template read_element<1>(a);

            quat_traits<R>::template write_element<2>(r)=-quat_traits<A>::template read_element<2>(a);

            quat_traits<R>::template write_element<3>(r)=-quat_traits<A>::template read_element<3>(a);

            return r;

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if<

            msvc_parse_bug_workaround::quats<A,B>,

            A &>::type

        operator*=( A & a, B const & b )

            {

            typedef typename quat_traits<A>::scalar_type TA;

            typedef typename quat_traits<B>::scalar_type TB;

            TA const aa=quat_traits<A>::template read_element<0>(a);

            TA const ab=quat_traits<A>::template read_element<1>(a);

            TA const ac=quat_traits<A>::template read_element<2>(a);

            TA const ad=quat_traits<A>::template read_element<3>(a);

            TB const ba=quat_traits<B>::template read_element<0>(b);

            TB const bb=quat_traits<B>::template read_element<1>(b);

            TB const bc=quat_traits<B>::template read_element<2>(b);

            TB const bd=quat_traits<B>::template read_element<3>(b);

            quat_traits<A>::template write_element<0>(a) = aa*ba - ab*bb - ac*bc - ad*bd;

            quat_traits<A>::template write_element<1>(a) = aa*bb + ab*ba + ac*bd - ad*bc;

            quat_traits<A>::template write_element<2>(a) = aa*bc + ac*ba + ad*bb - ab*bd;

            quat_traits<A>::template write_element<3>(a) = aa*bd + ad*ba + ab*bc - ac*bb;

            return a;

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value && is_scalar<B>::value,

            A &>::type

        operator*=( A & a, B b )

            {

            quat_traits<A>::template write_element<0>(a)*=b;

            quat_traits<A>::template write_element<1>(a)*=b;

            quat_traits<A>::template write_element<2>(a)*=b;

            quat_traits<A>::template write_element<3>(a)*=b;

            return a;

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename lazy_enable_if_c<

            is_quat<A>::value && is_quat<B>::value,

            deduce_quat2<A,B> >::type

        operator*( A const & a, B const & b )

            {

            typedef typename deduce_quat2<A,B>::type R;

            typedef typename quat_traits<A>::scalar_type TA;

            typedef typename quat_traits<B>::scalar_type TB;

            TA const aa=quat_traits<A>::template read_element<0>(a);

            TA const ab=quat_traits<A>::template read_element<1>(a);

            TA const ac=quat_traits<A>::template read_element<2>(a);

            TA const ad=quat_traits<A>::template read_element<3>(a);

            TB const ba=quat_traits<B>::template read_element<0>(b);

            TB const bb=quat_traits<B>::template read_element<1>(b);

            TB const bc=quat_traits<B>::template read_element<2>(b);

            TB const bd=quat_traits<B>::template read_element<3>(b);

            R r;

            quat_traits<R>::template write_element<0>(r) = aa*ba - ab*bb - ac*bc - ad*bd;

            quat_traits<R>::template write_element<1>(r) = aa*bb + ab*ba + ac*bd - ad*bc;

            quat_traits<R>::template write_element<2>(r) = aa*bc + ac*ba + ad*bb - ab*bd;

            quat_traits<R>::template write_element<3>(r) = aa*bd + ad*ba + ab*bc - ac*bb;

            return r;

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename lazy_enable_if_c<

            is_quat<A>::value && is_scalar<B>::value,

            deduce_quat<A> >::type

        operator*( A const & a, B b )

            {

            typedef typename deduce_quat<A>::type R;

            R r;

            quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)*b;

            quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)*b;

            quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)*b;

            quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)*b;

            return r;

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value && is_quat<B>::value,

            bool>::type

        operator!=( A const & a, B const & b )

            {

            return

                quat_traits<A>::template read_element<0>(a)!=quat_traits<B>::template read_element<0>(b) ||

                quat_traits<A>::template read_element<1>(a)!=quat_traits<B>::template read_element<1>(b) ||

                quat_traits<A>::template read_element<2>(a)!=quat_traits<B>::template read_element<2>(b) ||

                quat_traits<A>::template read_element<3>(a)!=quat_traits<B>::template read_element<3>(b);

            }

        template <class A>

        BOOST_QVM_INLINE_OPERATIONS

        typename lazy_enable_if_c<

            is_quat<A>::value,

            deduce_quat<A> >::type

        normalized( A const & a )

            {

            typedef typename quat_traits<A>::scalar_type T;

            T const a0=quat_traits<A>::template read_element<0>(a);

            T const a1=quat_traits<A>::template read_element<1>(a);

            T const a2=quat_traits<A>::template read_element<2>(a);

            T const a3=quat_traits<A>::template read_element<3>(a);

            T const m2=a0*a0+a1*a1+a2*a2+a3*a3;

            if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) )

                BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());

            T const rm=scalar_traits<T>::value(1)/sqrt<T>(m2);

            typedef typename deduce_quat<A>::type R;

            R r;

            quat_traits<R>::template write_element<0>(r)=a0*rm;

            quat_traits<R>::template write_element<1>(r)=a1*rm;

            quat_traits<R>::template write_element<2>(r)=a2*rm;

            quat_traits<R>::template write_element<3>(r)=a3*rm;

            return r;

            }

        template <class A>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value,

            void>::type

        normalize( A & a )

            {

            typedef typename quat_traits<A>::scalar_type T;

            T const a0=quat_traits<A>::template read_element<0>(a);

            T const a1=quat_traits<A>::template read_element<1>(a);

            T const a2=quat_traits<A>::template read_element<2>(a);

            T const a3=quat_traits<A>::template read_element<3>(a);

            T const m2=a0*a0+a1*a1+a2*a2+a3*a3;

            if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) )

                BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());

            T const rm=scalar_traits<T>::value(1)/sqrt<T>(m2);

            quat_traits<A>::template write_element<0>(a)*=rm;

            quat_traits<A>::template write_element<1>(a)*=rm;

            quat_traits<A>::template write_element<2>(a)*=rm;

            quat_traits<A>::template write_element<3>(a)*=rm;

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if<

            msvc_parse_bug_workaround::quats<A,B>,

            A &>::type

        operator+=( A & a, B const & b )

            {

            quat_traits<A>::template write_element<0>(a)+=quat_traits<B>::template read_element<0>(b);

            quat_traits<A>::template write_element<1>(a)+=quat_traits<B>::template read_element<1>(b);

            quat_traits<A>::template write_element<2>(a)+=quat_traits<B>::template read_element<2>(b);

            quat_traits<A>::template write_element<3>(a)+=quat_traits<B>::template read_element<3>(b);

            return a;

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename lazy_enable_if_c<

            is_quat<A>::value && is_quat<B>::value,

            deduce_quat2<A,B> >::type

        operator+( A const & a, B const & b )

            {

            typedef typename deduce_quat2<A,B>::type R;

            R r;

            quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)+quat_traits<B>::template read_element<0>(b);

            quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)+quat_traits<B>::template read_element<1>(b);

            quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)+quat_traits<B>::template read_element<2>(b);

            quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)+quat_traits<B>::template read_element<3>(b);

            return r;

            }

        template <class A,class B,class C>

        BOOST_QVM_INLINE_OPERATIONS

        typename lazy_enable_if_c<

            is_quat<A>::value && is_quat<B>::value && is_scalar<C>::value,

            deduce_quat2<A,B> >::type

        slerp( A const & a, B const & b, C t )

            {

            typedef typename deduce_quat2<A,B>::type R;

            typedef typename quat_traits<R>::scalar_type TR;

            TR const one = scalar_traits<TR>::value(1);

            TR dp = dot(a,b);

            TR sc=one;

            if( dp < one )

                {

                TR const theta = acosf(dp);

                TR const invsintheta = one/sin<TR>(theta);

                TR const scale = sin<TR>(theta*(one-t)) * invsintheta;

                TR const invscale = sin<TR>(theta*t) * invsintheta * sc;

                return a*scale + b*invscale;

                }

            else

                return normalized(a+(b-a)*t);

            }

        ////////////////////////////////////////////////

        namespace

        qvm_detail

            {

            template <class T>

            class

            qref_

                {

                qref_( qref_ const & );

                qref_ & operator=( qref_ const & );

                ~qref_();

                public:

                template <class R>

                BOOST_QVM_INLINE_TRIVIAL

                qref_ &

                operator=( R const & x )

                    {

                    assign(*this,x);

                    return *this;

                    }

                template <class R>

                BOOST_QVM_INLINE_TRIVIAL

                operator R() const

                    {

                    R r;

                    assign(r,*this);

                    return r;

                    }

                };

            }

        template <class Q>

        struct quat_traits;

        template <class Q>

        struct

        quat_traits< qvm_detail::qref_<Q> >

            {

            typedef typename quat_traits<Q>::scalar_type scalar_type;

            typedef qvm_detail::qref_<Q> this_quaternion;

            template <int I>

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type

            read_element( this_quaternion const & x )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<4);

                return quat_traits<Q>::template read_element<I>(reinterpret_cast<Q const &>(x));

                }

            template <int I>

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type &

            write_element( this_quaternion & x )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<4);

                return quat_traits<Q>::template write_element<I>(reinterpret_cast<Q &>(x));

                }

            };

        template <class Q>

        struct

        deduce_quat< qvm_detail::qref_<Q> >

            {

            typedef quat<typename quat_traits<Q>::scalar_type> type;

            };

        template <class Q>

        BOOST_QVM_INLINE_TRIVIAL

        typename enable_if_c<

            is_quat<Q>::value,

            qvm_detail::qref_<Q> const &>::type

        qref( Q const & a )

            {

            return reinterpret_cast<qvm_detail::qref_<Q> const &>(a);

            }

        template <class Q>

        BOOST_QVM_INLINE_TRIVIAL

        typename enable_if_c<

            is_quat<Q>::value,

            qvm_detail::qref_<Q> &>::type

        qref( Q & a )

            {

            return reinterpret_cast<qvm_detail::qref_<Q> &>(a);

            }

        ////////////////////////////////////////////////

        namespace

        qvm_detail

            {

            template <class T>

            class

            zero_q_

                {

                zero_q_( zero_q_ const & );

                zero_q_ & operator=( zero_q_ const & );

                ~zero_q_();

                public:

                template <class R>

                BOOST_QVM_INLINE_TRIVIAL

                operator R() const

                    {

                    R r;

                    assign(r,*this);

                    return r;

                    }

                };

            }

        template <class T>

        struct

        quat_traits< qvm_detail::zero_q_<T> >

            {

            typedef qvm_detail::zero_q_<T> this_quaternion;

            typedef T scalar_type;

            template <int I>

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type

            read_element( this_quaternion const & x )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<4);

                return scalar_traits<scalar_type>::value(0);

                }

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type

            read_element_idx( int i, this_quaternion const & x )

                {

                BOOST_QVM_ASSERT(i>=0);

                BOOST_QVM_ASSERT(i<4);

                return scalar_traits<scalar_type>::value(0);

                }

            };

        template <class T>

        BOOST_QVM_INLINE_TRIVIAL

        qvm_detail::zero_q_<T> const &

        zero_quat()

            {

            return *(qvm_detail::zero_q_<T> const *)qvm_detail::get_valid_ptr_quat_operations();

            }

        template <class A>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value,

            void>::type

        set_zero( A & a )

            {

            typedef typename quat_traits<A>::scalar_type T;

            T const zero=scalar_traits<T>::value(0);

            quat_traits<A>::template write_element<0>(a) = zero;

            quat_traits<A>::template write_element<1>(a) = zero;

            quat_traits<A>::template write_element<2>(a) = zero;

            quat_traits<A>::template write_element<3>(a) = zero;

            }

        ////////////////////////////////////////////////

        namespace

        qvm_detail

            {

            template <class V>

            struct

            rot_quat_

                {

                typedef typename vec_traits<V>::scalar_type scalar_type;

                scalar_type a[4];

                template <class Angle>

                BOOST_QVM_INLINE

                rot_quat_( V const & axis, Angle angle )

                    {

                    scalar_type const x=vec_traits<V>::template read_element<0>(axis);

                    scalar_type const y=vec_traits<V>::template read_element<1>(axis);

                    scalar_type const z=vec_traits<V>::template read_element<2>(axis);

                    scalar_type const m2=x*x+y*y+z*z;

                    if( m2==scalar_traits<scalar_type>::value(0) )

                        BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());

                    scalar_type const rm=scalar_traits<scalar_type>::value(1)/sqrt<scalar_type>(m2);

                    angle/=2;

                    scalar_type const s=sin<Angle>(angle);

                    a[0] = cos<Angle>(angle);

                    a[1] = rm*x*s;

                    a[2] = rm*y*s;

                    a[3] = rm*z*s;

                    }

                template <class R>

                BOOST_QVM_INLINE_TRIVIAL

                operator R() const

                    {

                    R r;

                    assign(r,*this);

                    return r;

                    }

                };

            }

        template <class V>

        struct

        quat_traits< qvm_detail::rot_quat_<V> >

            {

            typedef qvm_detail::rot_quat_<V> this_quaternion;

            typedef typename this_quaternion::scalar_type scalar_type;

            template <int I>

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type

            read_element( this_quaternion const & x )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<4);

                return x.a[I];

                }

            };

        template <class V>

        struct

        deduce_quat< qvm_detail::rot_quat_<V> >

            {

            typedef quat<typename vec_traits<V>::scalar_type> type;

            };

        template <class A,class Angle>

        BOOST_QVM_INLINE

        typename enable_if_c<

            is_vec<A>::value && vec_traits<A>::dim==3,

            qvm_detail::rot_quat_<A> >::type

        rot_quat( A const & axis, Angle angle )

            {

            return qvm_detail::rot_quat_<A>(axis,angle);

            }

        template <class A,class B,class Angle>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value &&

            is_vec<B>::value && vec_traits<B>::dim==3,

            void>::type

        set_rot( A & a, B const & axis, Angle angle )

            {

            assign(a,rot_quat(axis,angle));

            }

        template <class A,class B,class Angle>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value &&

            is_vec<B>::value && vec_traits<B>::dim==3,

            void>::type

        rotate( A & a, B const & axis, Angle angle )

            {

            a *= rot_quat(axis,angle);

            }

        ////////////////////////////////////////////////

        namespace

        qvm_detail

            {

            template <class T>

            struct

            rotx_quat_

                {

                BOOST_QVM_INLINE_TRIVIAL

                rotx_quat_()

                    {

                    }

                template <class R>

                BOOST_QVM_INLINE_TRIVIAL

                operator R() const

                    {

                    R r;

                    assign(r,*this);

                    return r;

                    }

                private:

                rotx_quat_( rotx_quat_ const & );

                rotx_quat_ & operator=( rotx_quat_ const & );

                ~rotx_quat_();

                };

            template <int I>

            struct

            rotx_q_get

                {

                template <class T>

                static

                BOOST_QVM_INLINE_CRITICAL

                T

                get( T const & )

                    {

                    return scalar_traits<T>::value(0);

                    }

                };

            template <>

            struct

            rotx_q_get<1>

                {

                template <class T>

                static

                BOOST_QVM_INLINE_CRITICAL

                T

                get( T const & angle )

                    {

                    return sin<T>(angle/2);

                    }

                };

            template <>

            struct

            rotx_q_get<0>

                {

                template <class T>

                static

                BOOST_QVM_INLINE_CRITICAL

                T

                get( T const & angle )

                    {

                    return cos<T>(angle/2);

                    }

                };

            }

        template <class Angle>

        struct

        quat_traits< qvm_detail::rotx_quat_<Angle> >

            {

            typedef qvm_detail::rotx_quat_<Angle> this_quaternion;

            typedef Angle scalar_type;

            template <int I>

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type

            read_element( this_quaternion const & x )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<4);

                return qvm_detail::rotx_q_get<I>::get(reinterpret_cast<Angle const &>(x));

                }

            };

        template <class Angle>

        struct

        deduce_quat< qvm_detail::rotx_quat_<Angle> >

            {

            typedef quat<Angle> type;

            };

        template <class Angle>

        struct

        deduce_quat2< qvm_detail::rotx_quat_<Angle>, qvm_detail::rotx_quat_<Angle> >

            {

            typedef quat<Angle> type;

            };

        template <class Angle>

        BOOST_QVM_INLINE_TRIVIAL

        qvm_detail::rotx_quat_<Angle> const &

        rotx_quat( Angle const & angle )

            {

            return reinterpret_cast<qvm_detail::rotx_quat_<Angle> const &>(angle);

            }

        template <class A,class Angle>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value,

            void>::type

        set_rotx( A & a, Angle angle )

            {

            assign(a,rotx_quat(angle));

            }

        template <class A,class Angle>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value,

            void>::type

        rotate_x( A & a, Angle angle )

            {

            a *= rotx_quat(angle);

            }

        ////////////////////////////////////////////////

        namespace

        qvm_detail

            {

            template <class T>

            struct

            roty_quat_

                {

                BOOST_QVM_INLINE_TRIVIAL

                roty_quat_()

                    {

                    }

                template <class R>

                BOOST_QVM_INLINE_TRIVIAL

                operator R() const

                    {

                    R r;

                    assign(r,*this);

                    return r;

                    }

                private:

                roty_quat_( roty_quat_ const & );

                roty_quat_ & operator=( roty_quat_ const & );

                ~roty_quat_();

                };

            template <int I>

            struct

            roty_q_get

                {

                template <class T>

                static

                BOOST_QVM_INLINE_CRITICAL

                T

                get( T const & )

                    {

                    return scalar_traits<T>::value(0);

                    }

                };

            template <>

            struct

            roty_q_get<2>

                {

                template <class T>

                static

                BOOST_QVM_INLINE_CRITICAL

                T

                get( T const & angle )

                    {

                    return sin<T>(angle/2);

                    }

                };

            template <>

            struct

            roty_q_get<0>

                {

                template <class T>

                static

                BOOST_QVM_INLINE_CRITICAL

                T

                get( T const & angle )

                    {

                    return cos<T>(angle/2);

                    }

                };

            }

        template <class Angle>

        struct

        quat_traits< qvm_detail::roty_quat_<Angle> >

            {

            typedef qvm_detail::roty_quat_<Angle> this_quaternion;

            typedef Angle scalar_type;

            template <int I>

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type

            read_element( this_quaternion const & x )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<4);

                return qvm_detail::roty_q_get<I>::get(reinterpret_cast<Angle const &>(x));

                }

            };

        template <class Angle>

        struct

        deduce_quat< qvm_detail::roty_quat_<Angle> >

            {

            typedef quat<Angle> type;

            };

        template <class Angle>

        struct

        deduce_quat2< qvm_detail::roty_quat_<Angle>, qvm_detail::roty_quat_<Angle> >

            {

            typedef quat<Angle> type;

            };

        template <class Angle>

        BOOST_QVM_INLINE_TRIVIAL

        qvm_detail::roty_quat_<Angle> const &

        roty_quat( Angle const & angle )

            {

            return reinterpret_cast<qvm_detail::roty_quat_<Angle> const &>(angle);

            }

        template <class A,class Angle>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value,

            void>::type

        set_roty( A & a, Angle angle )

            {

            assign(a,roty_quat(angle));

            }

        template <class A,class Angle>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value,

            void>::type

        rotate_y( A & a, Angle angle )

            {

            a *= roty_quat(angle);

            }

        ////////////////////////////////////////////////

        namespace

        qvm_detail

            {

            template <class T>

            struct

            rotz_quat_

                {

                BOOST_QVM_INLINE_TRIVIAL

                rotz_quat_()

                    {

                    }

                template <class R>

                BOOST_QVM_INLINE_TRIVIAL

                operator R() const

                    {

                    R r;

                    assign(r,*this);

                    return r;

                    }

                private:

                rotz_quat_( rotz_quat_ const & );

                rotz_quat_ & operator=( rotz_quat_ const & );

                ~rotz_quat_();

                };

            template <int I>

            struct

            rotz_q_get

                {

                template <class T>

                static

                BOOST_QVM_INLINE_CRITICAL

                T

                get( T const & )

                    {

                    return scalar_traits<T>::value(0);

                    }

                };

            template <>

            struct

            rotz_q_get<3>

                {

                template <class T>

                static

                BOOST_QVM_INLINE_CRITICAL

                T

                get( T const & angle )

                    {

                    return sin<T>(angle/2);

                    }

                };

            template <>

            struct

            rotz_q_get<0>

                {

                template <class T>

                static

                BOOST_QVM_INLINE_CRITICAL

                T

                get( T const & angle )

                    {

                    return cos<T>(angle/2);

                    }

                };

            }

        template <class Angle>

        struct

        quat_traits< qvm_detail::rotz_quat_<Angle> >

            {

            typedef qvm_detail::rotz_quat_<Angle> this_quaternion;

            typedef Angle scalar_type;

            template <int I>

            static

            BOOST_QVM_INLINE_CRITICAL

            scalar_type

            read_element( this_quaternion const & x )

                {

                BOOST_QVM_STATIC_ASSERT(I>=0);

                BOOST_QVM_STATIC_ASSERT(I<4);

                return qvm_detail::rotz_q_get<I>::get(reinterpret_cast<Angle const &>(x));

                }

            };

        template <class Angle>

        struct

        deduce_quat< qvm_detail::rotz_quat_<Angle> >

            {

            typedef quat<Angle> type;

            };

        template <class Angle>

        struct

        deduce_quat2< qvm_detail::rotz_quat_<Angle>, qvm_detail::rotz_quat_<Angle> >

            {

            typedef quat<Angle> type;

            };

        template <class Angle>

        BOOST_QVM_INLINE_TRIVIAL

        qvm_detail::rotz_quat_<Angle> const &

        rotz_quat( Angle const & angle )

            {

            return reinterpret_cast<qvm_detail::rotz_quat_<Angle> const &>(angle);

            }

        template <class A,class Angle>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value,

            void>::type

        set_rotz( A & a, Angle angle )

            {

            assign(a,rotz_quat(angle));

            }

        template <class A,class Angle>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value,

            void>::type

        rotate_z( A & a, Angle angle )

            {

            a *= rotz_quat(angle);

            }

        template <class A,class B>

        BOOST_QVM_INLINE_OPERATIONS

        typename enable_if_c<

            is_quat<A>::value && is_vec<B>::value && vec_traits<B>::dim==3,

            typename quat_traits<A>::scalar_type>::type

        axis_angle( A const & a, B & b )

            {

            typedef typename quat_traits<A>::scalar_type T;

            T a0=quat_traits<A>::template read_element<0>(a);

            T a1=quat_traits<A>::template read_element<1>(a);

            T a2=quat_traits<A>::template read_element<2>(a);

            T a3=quat_traits<A>::template read_element<3>(a);

            if( a0>1 )

                {

                T const m2=a0*a0+a1*a1+a2*a2+a3*a3;

                if( m2==scalar_traits<T>::value(0) )

                    BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());

                T const s=sqrt<T>(m2);

                a0/=s;

                a1/=s;

                a2/=s;

                a3/=s;

                }

            if( T s=sqrt<T>(1-a0*a0) )

                {

                vec_traits<B>::template write_element<0>(b) = a1/s;

                vec_traits<B>::template write_element<1>(b) = a2/s;

                vec_traits<B>::template write_element<2>(b) = a3/s;

                }

            else

                {

                typedef typename vec_traits<B>::scalar_type T;

                vec_traits<B>::template write_element<0>(b) = scalar_traits<T>::value(1);

                vec_traits<B>::template write_element<1>(b) = vec_traits<B>::template write_element<2>(b) = scalar_traits<T>::value(0);

                }

            return scalar_traits<T>::value(2) * qvm::acos(a0);

            }

        ////////////////////////////////////////////////

        namespace

        sfinae

            {

            using ::boost::qvm::assign;

            using ::boost::qvm::cmp;

            using ::boost::qvm::convert_to;

            using ::boost::qvm::conjugate;

            using ::boost::qvm::set_identity;

            using ::boost::qvm::set_zero;

            using ::boost::qvm::scalar_cast;

            using ::boost::qvm::operator/=;

            using ::boost::qvm::operator/;

            using ::boost::qvm::dot;

            using ::boost::qvm::operator==;

            using ::boost::qvm::inverse;

            using ::boost::qvm::mag_sqr;

            using ::boost::qvm::mag;

            using ::boost::qvm::slerp;

            using ::boost::qvm::operator-=;

            using ::boost::qvm::operator-;

            using ::boost::qvm::operator*=;

            using ::boost::qvm::operator*;

            using ::boost::qvm::operator!=;

            using ::boost::qvm::normalized;

            using ::boost::qvm::normalize;

            using ::boost::qvm::operator+=;

            using ::boost::qvm::operator+;

            using ::boost::qvm::qref;

            using ::boost::qvm::rot_quat;

            using ::boost::qvm::set_rot;

            using ::boost::qvm::rotate;

            using ::boost::qvm::rotx_quat;

            using ::boost::qvm::set_rotx;

            using ::boost::qvm::rotate_x;

            using ::boost::qvm::roty_quat;

            using ::boost::qvm::set_roty;

            using ::boost::qvm::rotate_y;

            using ::boost::qvm::rotz_quat;

            using ::boost::qvm::set_rotz;

            using ::boost::qvm::rotate_z;

            }

        ////////////////////////////////////////////////

        }

    }

#endif


//=============================================================================================================
/**
* @file     filterdata.cpp
* @author   Florian Schlembach <florian.schlembach@tu-ilmenau.de>;
*           Christoph Dinh <chdinh@nmr.mgh.harvard.edu>;
*           Matti Hamalainen <msh@nmr.mgh.harvard.edu>;
*           Jens Haueisen <jens.haueisen@tu-ilmenau.de>
* @version  1.0
* @date     February, 2014
*
* @section  LICENSE
*
* Copyright (C) 2014, Florian Schlembach, Christoph Dinh and Matti Hamalainen. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification, are permitted provided that
* the following conditions are met:
*     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
*       following disclaimer.
*     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
*       the following disclaimer in the documentation and/or other materials provided with the distribution.
*     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
*       to endorse or promote products derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
* PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*
* @brief    Contains all FilterData.
*
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "filterdata.h"

#include "../mnemath.h"

#include "parksmcclellan.h"
#include "cosinefilter.h"

//*************************************************************************************************************
//=============================================================================================================
// STL INCLUDES
//=============================================================================================================

#include <iostream>

//*************************************************************************************************************
//=============================================================================================================
// Qt INCLUDES
//=============================================================================================================

#include <QDebug>

//*************************************************************************************************************
//=============================================================================================================
// Eigen INCLUDES
//=============================================================================================================

#include <Eigen/SparseCore>
//#ifndef EIGEN_FFTW_DEFAULT
//#define EIGEN_FFTW_DEFAULT
//#endif
#include <unsupported/Eigen/FFT>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace UTILSLIB;

//*************************************************************************************************************

FilterData::FilterData()
: m_Type(UNKNOWN)
, m_iFilterOrder(80)
, m_iFFTlength(512)
, m_sName("Unknown")
, m_dParksWidth(0.1)
, m_designMethod(External)
, m_dCenterFreq(0.5)
, m_dBandwidth(0.1)
, m_sFreq(1000)
, m_dLowpassFreq(4)
, m_dHighpassFreq(40)
{

}

//*************************************************************************************************************

FilterData::FilterData(QString unique_name, FilterType type, int order, double centerfreq, double bandwidth, double parkswidth, double sFreq, qint32 fftlength, DesignMethod designMethod)
: m_Type(type)
, m_iFilterOrder(order)
, m_iFFTlength(fftlength)
, m_sName(unique_name)
, m_dParksWidth(parkswidth)
, m_designMethod(designMethod)
, m_dCenterFreq(centerfreq)
, m_dBandwidth(bandwidth)
, m_sFreq(sFreq)
{
    designFilter();
}

//*************************************************************************************************************

void FilterData::designFilter()
{
    switch(m_designMethod) {
        case Tschebyscheff: {
            ParksMcClellan filter(m_iFilterOrder, m_dCenterFreq, m_dBandwidth, m_dParksWidth, (ParksMcClellan::TPassType)m_Type);
            m_dCoeffA = filter.FirCoeff;

            //fft-transform m_dCoeffA in order to be able to perform frequency-domain filtering
            fftTransformCoeffs();

            break;
        }

        case Cosine: {
            CosineFilter filtercos;

            switch(m_Type) {
                case LPF:
                    filtercos = CosineFilter (m_iFFTlength,
                                            (m_dCenterFreq)*(m_sFreq/2),
                                            m_dParksWidth*(m_sFreq/2),
                                            (m_dCenterFreq)*(m_sFreq/2),
                                            m_dParksWidth*(m_sFreq/2),
                                            m_sFreq,
                                            (CosineFilter::TPassType)m_Type);

                    break;

                case HPF:
                    filtercos = CosineFilter (m_iFFTlength,
                                            (m_dCenterFreq)*(m_sFreq/2),
                                            m_dParksWidth*(m_sFreq/2),
                                            (m_dCenterFreq)*(m_sFreq/2),
                                            m_dParksWidth*(m_sFreq/2),
                                            m_sFreq,
                                            (CosineFilter::TPassType)m_Type);

                    break;

                case BPF:
                    filtercos = CosineFilter (m_iFFTlength,
                                            (m_dCenterFreq + m_dBandwidth/2)*(m_sFreq/2),
                                            m_dParksWidth*(m_sFreq/2),
                                            (m_dCenterFreq - m_dBandwidth/2)*(m_sFreq/2),
                                            m_dParksWidth*(m_sFreq/2),
                                            m_sFreq,
                                            (CosineFilter::TPassType)m_Type);

                    break;
            }

            //This filter is designed in the frequency domain, hence the time domain impulse response need to be shortend by the users dependent number of taps
            m_dCoeffA.resize(m_iFilterOrder);
            m_dCoeffA.head(m_iFilterOrder/2) = filtercos.m_dCoeffA.tail(m_iFilterOrder/2);
            m_dCoeffA.tail(m_iFilterOrder/2) = filtercos.m_dCoeffA.head(m_iFilterOrder/2);

            //Now generate the fft version of the shortened impulse response
            fftTransformCoeffs();

            break;
        }
    }

    switch(m_Type) {
        case LPF:
            m_dLowpassFreq = 0;
            m_dHighpassFreq = m_dCenterFreq*(m_sFreq/2);
        break;

        case HPF:
            m_dLowpassFreq = m_dCenterFreq*(m_sFreq/2);
            m_dHighpassFreq = 0;
        break;

        case BPF:
            m_dLowpassFreq = (m_dCenterFreq + m_dBandwidth/2)*(m_sFreq/2);
            m_dHighpassFreq = (m_dCenterFreq - m_dBandwidth/2)*(m_sFreq/2);
        break;
    }

}

//*************************************************************************************************************

void FilterData::fftTransformCoeffs()
{
    //zero-pad m_dCoeffA to m_iFFTlength
    RowVectorXd t_coeffAzeroPad = RowVectorXd::Zero(m_iFFTlength);
    t_coeffAzeroPad.head(m_dCoeffA.cols()) = m_dCoeffA;

    //generate fft object
    Eigen::FFT<double> fft;
    fft.SetFlag(fft.HalfSpectrum);

    //fft-transform filter coeffs
    m_dFFTCoeffA = RowVectorXcd::Zero(m_iFFTlength);
    fft.fwd(m_dFFTCoeffA,t_coeffAzeroPad);
}

//*************************************************************************************************************

RowVectorXd FilterData::applyConvFilter(const RowVectorXd& data, bool keepOverhead, CompensateEdgeEffects compensateEdgeEffects) const
{
    if(data.cols()<m_dCoeffA.cols() && compensateEdgeEffects==MirrorData){
        qDebug()<<QString("Error in FilterData: Number of filter taps(%1) bigger then data size(%2). Not enough data to perform mirroring!").arg(m_dCoeffA.cols()).arg(data.cols());
        return data;
    }

    //Do zero padding or mirroring depending on user input
    RowVectorXd t_dataZeroPad = RowVectorXd::Zero(2*m_dCoeffA.cols() + data.cols());
    RowVectorXd t_filteredTime = RowVectorXd::Zero(2*m_dCoeffA.cols() + data.cols());

    switch(compensateEdgeEffects) {
        case MirrorData:
            t_dataZeroPad.head(m_dCoeffA.cols()) = data.head(m_dCoeffA.cols()).reverse();   //front
            t_dataZeroPad.segment(m_dCoeffA.cols(), data.cols()) = data;                    //middle
            t_dataZeroPad.tail(m_dCoeffA.cols()) = data.tail(m_dCoeffA.cols()).reverse();   //back
            break;

        case ZeroPad:
            t_dataZeroPad.segment(m_dCoeffA.cols(), data.cols()) = data;
            break;

        default:
            t_dataZeroPad.segment(m_dCoeffA.cols(), data.cols()) = data;
            break;
    }

    //Do the convolution
    for(int i=m_dCoeffA.cols(); i<t_filteredTime.cols(); i++)
        t_filteredTime(i-m_dCoeffA.cols()) = t_dataZeroPad.segment(i-m_dCoeffA.cols(),m_dCoeffA.cols()) * m_dCoeffA.transpose();

    //Return filtered data
    if(!keepOverhead)
        return t_filteredTime.segment(m_dCoeffA.cols()/2, data.cols());

    return t_filteredTime.head(data.cols()+m_dCoeffA.cols());
}

//*************************************************************************************************************

RowVectorXd FilterData::applyFFTFilter(const RowVectorXd& data, bool keepOverhead, CompensateEdgeEffects compensateEdgeEffects) const
{
    if(data.cols()<m_dCoeffA.cols() && compensateEdgeEffects==MirrorData) {
        qDebug()<<QString("Error in FilterData: Number of filter taps(%1) bigger then data size(%2). Not enough data to perform mirroring!").arg(m_dCoeffA.cols()).arg(data.cols());
        return data;
    }

//    std::cout<<"m_iFFTlength: "<<m_iFFTlength<<std::endl;
//    std::cout<<"2*m_dCoeffA.cols() + data.cols(): "<<2*m_dCoeffA.cols() + data.cols()<<std::endl;

    if(2*m_dCoeffA.cols() + data.cols()>m_iFFTlength) {
        qDebug()<<"Error in FilterData: Number of mirroring/zeropadding size plus data size is bigger then fft length!";
        return data;
    }

    //Do zero padding or mirroring depending on user input
    RowVectorXd t_dataZeroPad = RowVectorXd::Zero(m_iFFTlength);

    switch(compensateEdgeEffects) {
        case MirrorData:
            t_dataZeroPad.head(m_dCoeffA.cols()) = data.head(m_dCoeffA.cols()).reverse();   //front
            t_dataZeroPad.segment(m_dCoeffA.cols(), data.cols()) = data;                    //middle
            t_dataZeroPad.tail(m_dCoeffA.cols()) = data.tail(m_dCoeffA.cols()).reverse();   //back
            break;

        case ZeroPad:
            t_dataZeroPad.head(data.cols()) = data;
            break;

        default:
            t_dataZeroPad.head(data.cols()) = data;
            break;
    }

    //generate fft object
    Eigen::FFT<double> fft;
    fft.SetFlag(fft.HalfSpectrum);

    //fft-transform data sequence
    RowVectorXcd t_freqData;
    fft.fwd(t_freqData,t_dataZeroPad);

    //perform frequency-domain filtering
    RowVectorXcd t_filteredFreq = m_dFFTCoeffA.array()*t_freqData.array();

    //inverse-FFT
    RowVectorXd t_filteredTime;
    fft.inv(t_filteredTime,t_filteredFreq);

    //Return filtered data
    if(!keepOverhead)
        return t_filteredTime.segment(m_dCoeffA.cols()/2, data.cols());

    return t_filteredTime.head(data.cols()+m_dCoeffA.cols());
}

//*************************************************************************************************************

QString FilterData::getStringForDesignMethod(const FilterData::DesignMethod &designMethod)
{
    QString designMethodString = "External";

    if(designMethod == FilterData::External)
        designMethodString = "External";

    if(designMethod == FilterData::Cosine)
        designMethodString = "Cosine";

    if(designMethod == FilterData::Tschebyscheff)
        designMethodString = "Tschebyscheff";

    return designMethodString;
}

//*************************************************************************************************************

QString FilterData::getStringForFilterType(const FilterData::FilterType &filterType)
{
    QString filterTypeString = "LPF";

    if(filterType == FilterData::LPF)
        filterTypeString = "LPF";

    if(filterType == FilterData::HPF)
        filterTypeString = "HPF";

    if(filterType == FilterData::BPF)
        filterTypeString = "BPF";

    if(filterType == FilterData::NOTCH)
        filterTypeString = "NOTCH";

    return filterTypeString;
}

//*************************************************************************************************************

FilterData::DesignMethod FilterData::getDesignMethodForString(const QString &designMethodString)
{
    FilterData::DesignMethod designMethod = FilterData::External;

    if(designMethodString == "External")
        designMethod = FilterData::External;

    if(designMethodString == "Tschebyscheff")
        designMethod = FilterData::Tschebyscheff;

    if(designMethodString == "Cosine")
        designMethod = FilterData::Cosine;

    return designMethod;
}

//*************************************************************************************************************

FilterData::FilterType FilterData::getFilterTypeForString(const QString &filterTypeString)
{
    FilterData::FilterType filterType = FilterData::UNKNOWN;

    if(filterTypeString == "LPF")
        filterType = FilterData::LPF;

    if(filterTypeString == "HPF")
        filterType = FilterData::HPF;

    if(filterTypeString == "BPF")
        filterType = FilterData::BPF;

    if(filterTypeString == "NOTCH")
        filterType = FilterData::NOTCH;

    return filterType;
}