#ifndef Foundation_Math_Quat_INCLUDED
#define Foundation_Math_Quat_INCLUDED

#include <math.h>
#include <assert.h>
#include <limits>

#include <foundation/Math/misc.hpp>
#include <foundation/Math/Vec.hpp>

/* Quaternion */

namespace Tn {

namespace math {

template <typename T>
struct Quat;

template <typename T>
inline Quat<T> conjugate(const Quat<T>&);

template <typename T>
inline Quat<T> operator*(const Quat<T>&, const Quat<T>&);

template <typename T>
struct Quat {

    typedef T component_type;

    T r, i, j, k;

    Quat() 
        : r(T(1)), 
          i(T(0)), 
          j(T(0)), 
          k(T(0)) {}
    Quat(T nr, T ni, T nj, T nk) : r(nr), i(ni), j(nj), k(nk) {}
    Quat(const Quat<T>& that) : r(that.r), i(that.i), j(that.j), k(that.k) {}
    Quat(T radians, const Vec<3,T>& axis) {
        T dst_l = sin(radians/T(2));		// should be
        T src_l = magnitude(axis);			// actually is
        if (src_l <= std::numeric_limits<T>::epsilon()) {
            i = T(0);
            j = T(0);
            k = T(0);
            r = T(1);
        } else {
            T s = dst_l / src_l;
            i = axis[0] * s;
            j = axis[1] * s;
            k = axis[2] * s;
            r = cos(radians/T(2));
        }
    }

    Quat(const Vec<3,T>& a, const Vec<3,T>& b) {

        /*
          This is undefined when a and b are parallel. Should check for
          degeneracy before normalizing at least in the case of a and b
          pointing in the same direction. (This routine actually has
          behaved very well in practice.)
        */

        Vec<3,T> x = normalize(a);
        Vec<3,T> z = normalize(cross(a,b));
        Vec<3,T> y = normalize(cross(z,x));

        Vec<3,T> v = normalize(b);

        T x_c = dot(v,x);
        T y_c = dot(v,y);

        T theta = atan2(y_c,x_c);

        T l = sin(theta/T(2));
        i = z[0] * l;
        j = z[1] * l;
        k = z[2] * l;
        r = cos(theta/T(2));
    }
};



template <class T>
inline Quat<T> operator*(const Quat<T>& a, const Quat<T>& b) {

    // both of these seem to work (or at least they seem to produce the
    // same result)

#if 0

    T s1(a.r);
    T s2(b.r);

    Vec<3,T> v1(a.i, a.j, a.k);
    Vec<3,T> v2(b.i, b.j, b.k);

    T s = s1*s2-dot(v1,v2);
    Vec<3,T> v(s1*v2+s2*v1+cross(v1,v2));

    return Quat<T>(s,v[0],v[1],v[2]);

#else
    return Quat<T>(a.r*b.r - a.i*b.i - a.j*b.j - a.k*b.k,
                   a.j*b.k - b.j*a.k + a.r*b.i + b.r*a.i,
                   a.k*b.i - b.k*a.i + a.r*b.j + b.r*a.j,
                   a.i*b.j - b.i*a.j + a.r*b.k + b.r*a.k);
#endif
}


template <typename T>
inline Quat<T> operator/(const Quat<T>& a, const Quat<T>& b) {
    return a*invert(b);
}


template <typename T>
inline Quat<T> conjugate(const Quat<T>& q) {
    return Quat<T>(q.r, -q.i, -q.j, -q.k);
}


template <typename T>
inline Quat<T> normalize(const Quat<T>& q) {

    T lsq = q.r*q.r + q.i*q.i + q.j*q.j + q.k*q.k;
    if (lsq<=std::numeric_limits<T>::epsilon()) {
        return Quat<T>(T(1), T(0), T(0), T(0));
    } else {
        T l = sqrt(lsq);
        return Quat<T>(q.r/l, q.i/l, q.j/l, q.k/l);
    }
}


template <class T>
inline Quat<T> slerp(const Quat<T>& a, const Quat<T>& b, T u) {

    const double very_small = std::numeric_limits<T>::epsilon();

    T costheta 
        = a.r*b.r 
        + a.i*b.i 
        + a.j*b.j 
        + a.k*b.k;

    T theta = acos(costheta);
    T sintheta = sin(theta);

    T s0, s1;
    if (sintheta>very_small) {
        s0 = sinf((1-u)*theta) / sintheta;
        s1 = sinf(u * theta) / sintheta;
    } else {
        s0 = (1-u);
        s1 = u;
    }

    return Quat<T>(s0*a.r + s1*b.r,
                   s0*a.i + s1*b.i,
                   s0*a.j + s1*b.j,
                   s0*a.k + s1*b.k);

}

typedef Quat<float> Quatf;
typedef Quat<double> Quatd;

} // end namespace math

} // end namespace Tn


template <typename T>  
inline Tn::math::Quat<T> invert(const Tn::math::Quat<T>& q) {

    T norm = sqrt(q.r*q.r + q.i*q.i + q.j*q.j + q.k*q.k);

    return Tn::math::Quat<T>(q.r/norm, -q.i/norm, -q.j/norm, -q.k/norm);
}


template <typename T>
inline bool isIdentity(Tn::math::Quat<T> const & q) {

    return
        (q.r==T(1)) &&
        (q.i==T(0)) &&
        (q.j==T(0)) &&
        (q.k==T(0));
}


#endif
