/* -*- C++ -*- ------------------------------------------------------------
 
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/

The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).

 *-----------------------------------------------------------------------*/
/** @file
 *  @brief
 */

#ifndef interpolation_h
#define interpolation_h

#include <cml/mathlib/matrix_rotation.h>

/* Interpolation functions.
 *
 * @todo: This code works, but it needs a lot of cleanup.
 */

namespace cml {

struct function_expects_args_of_same_type_error;

namespace detail {

//////////////////////////////////////////////////////////////////////////////
// Helper struct to promote vectors, quaternions, and matrices
//////////////////////////////////////////////////////////////////////////////

template< class T1, class T2, class ResultT > struct TypePromote;

template< class T >
struct TypePromote< T,T,et::scalar_result_tag > {
    typedef T temporary_type;
};

template< class T1, class T2 >
struct TypePromote< T1,T2,et::scalar_result_tag > {
    typedef et::ExprTraits<T1> traits_1;
    typedef et::ExprTraits<T2> traits_2;
    typedef typename traits_1::result_tag result_type_1;
    typedef typename traits_2::result_tag result_type_2;
    
    /* Check that results are of the same type */
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_2>::is_true),
        function_expects_args_of_same_type_error);

    typedef typename et::ScalarPromote<T1,T2>::type temporary_type;
};

template< class T1, class T2 >
struct TypePromote< T1,T2,et::vector_result_tag > {
    typedef et::ExprTraits<T1> traits_1;
    typedef et::ExprTraits<T2> traits_2;
    typedef typename traits_1::result_tag result_type_1;
    typedef typename traits_2::result_tag result_type_2;
    
    /* Check that results are of the same type */
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_2>::is_true),
        function_expects_args_of_same_type_error);

    /* @todo: This should be VectorPromote<> for symmetry with the other
     * type promotions.
     */
    typedef typename CrossPromote<T1,T2>::promoted_vector temporary_type;
};

template< class T1, class T2 >
struct TypePromote< T1,T2,et::matrix_result_tag > {
    typedef et::ExprTraits<T1> traits_1;
    typedef et::ExprTraits<T2> traits_2;
    typedef typename traits_1::result_tag result_type_1;
    typedef typename traits_2::result_tag result_type_2;
    
    /* Check that results are of the same type */
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_2>::is_true),
        function_expects_args_of_same_type_error);

    typedef typename et::MatrixPromote2<T1,T2>::temporary_type temporary_type;
};

template< class T1, class T2 >
struct TypePromote< T1,T2,et::quaternion_result_tag > {
    typedef et::ExprTraits<T1> traits_1;
    typedef et::ExprTraits<T2> traits_2;
    typedef typename traits_1::result_tag result_type_1;
    typedef typename traits_2::result_tag result_type_2;
    
    /* Check that results are of the same type */
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_2>::is_true),
        function_expects_args_of_same_type_error);

    typedef typename et::QuaternionPromote2<T1,T2>::temporary_type
        temporary_type;
};

template< class T1, class T2, class T3, class ResultT > struct TypePromote3;

template< class T1, class T2, class T3 >
struct TypePromote3< T1,T2,T3,et::matrix_result_tag > {
    typedef et::ExprTraits<T1> traits_1;
    typedef et::ExprTraits<T2> traits_2;
    typedef et::ExprTraits<T3> traits_3;
    typedef typename traits_1::result_tag result_type_1;
    typedef typename traits_2::result_tag result_type_2;
    typedef typename traits_3::result_tag result_type_3;
    
    /* Check that results are of the same type */
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_2>::is_true),
        function_expects_args_of_same_type_error);
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_3>::is_true),
        function_expects_args_of_same_type_error);

    typedef typename et::MatrixPromote3<T1,T2,T3>::temporary_type
        temporary_type;
    typedef typename temporary_type::value_type value_type;
};

template< class T1, class T2, class T3 >
struct TypePromote3< T1,T2,T3,et::quaternion_result_tag > {
    typedef et::ExprTraits<T1> traits_1;
    typedef et::ExprTraits<T2> traits_2;
    typedef et::ExprTraits<T3> traits_3;
    typedef typename traits_1::result_tag result_type_1;
    typedef typename traits_2::result_tag result_type_2;
    typedef typename traits_3::result_tag result_type_3;
    
    /* Check that results are of the same type */
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_2>::is_true),
        function_expects_args_of_same_type_error);
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_3>::is_true),
        function_expects_args_of_same_type_error);

    typedef typename et::QuaternionPromote3<T1,T2,T3>::temporary_type
        temporary_type;
    typedef typename temporary_type::value_type value_type;
};

template <
    class T1, class T2, class T3, class T4, class ResultT
> struct TypePromote4;

template< class T1, class T2, class T3, class T4 >
struct TypePromote4< T1,T2,T3,T4,et::matrix_result_tag > {
    typedef et::ExprTraits<T1> traits_1;
    typedef et::ExprTraits<T2> traits_2;
    typedef et::ExprTraits<T3> traits_3;
    typedef et::ExprTraits<T4> traits_4;
    typedef typename traits_1::result_tag result_type_1;
    typedef typename traits_2::result_tag result_type_2;
    typedef typename traits_3::result_tag result_type_3;
    typedef typename traits_4::result_tag result_type_4;
    
    /* Check that results are of the same type */
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_2>::is_true),
        function_expects_args_of_same_type_error);
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_3>::is_true),
        function_expects_args_of_same_type_error);
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_4>::is_true),
        function_expects_args_of_same_type_error);

    typedef typename et::MatrixPromote4<T1,T2,T3,T4>::temporary_type
        temporary_type;
    typedef typename temporary_type::value_type value_type;
};

template< class T1, class T2, class T3, class T4 >
struct TypePromote4< T1,T2,T3,T4,et::quaternion_result_tag > {
    typedef et::ExprTraits<T1> traits_1;
    typedef et::ExprTraits<T2> traits_2;
    typedef et::ExprTraits<T3> traits_3;
    typedef et::ExprTraits<T4> traits_4;
    typedef typename traits_1::result_tag result_type_1;
    typedef typename traits_2::result_tag result_type_2;
    typedef typename traits_3::result_tag result_type_3;
    typedef typename traits_4::result_tag result_type_4;
    
    /* Check that results are of the same type */
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_2>::is_true),
        function_expects_args_of_same_type_error);
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_3>::is_true),
        function_expects_args_of_same_type_error);
    CML_STATIC_REQUIRE_M(
        (same_type<result_type_1, result_type_4>::is_true),
        function_expects_args_of_same_type_error);

    typedef typename et::QuaternionPromote4<T1,T2,T3,T4>::temporary_type
        temporary_type;
    typedef typename temporary_type::value_type value_type;
};

//////////////////////////////////////////////////////////////////////////////
// Helper functions to resize a vector, quaternion or matrix
//////////////////////////////////////////////////////////////////////////////

// Should be able to catch all no-ops with a generic function template...

template < class T1, class T2, class SizeTag > void
InterpResize(T1& t1, const T2& t2, SizeTag) {}

// Catch vector and matrix resizes...

template< typename E, class A, class VecT > void
InterpResize(vector<E,A>& v, const VecT& target, dynamic_size_tag) {
    v.resize(target.size());
}

template< typename E, class A, class B, class L, class MatT > void
InterpResize(matrix<E,A,B,L>& m, const MatT& target, dynamic_size_tag) {
    m.resize(target.rows(),target.cols());
}

//////////////////////////////////////////////////////////////////////////////
// Construction of 'intermediate' quaternions and matrices for use with squad
//////////////////////////////////////////////////////////////////////////////

#if 0
template < class QuatT_1, class QuatT_2 >
typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
concatenate_quaternions(
    const QuatT_1& q1,
    const QuatT_2& q2,
    positive_cross)
{
    return q2 * q1;
}

template < class QuatT_1, class QuatT_2 >
typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
concatenate_quaternions(
    const QuatT_1& q1,
    const QuatT_2& q2,
    negative_cross)
{
    return q1 * q2;
}

template< class T1, class T2, class T3, class SizeT >
typename detail::TypePromote3<
    T1,T2,T3,typename et::ExprTraits<T1>::result_tag
>::temporary_type
squad_intermediate(
    const T1& t1,
    const T2& t2,
    const T3& t3,
    typename detail::TypePromote3<
        T1, T2, T3, typename et::ExprTraits<T1>::result_tag
    >::value_type tolerance,
    et::quaternion_result_tag,
    SizeT)
{
    typedef et::ExprTraits<T1> traits_1;
    typedef typename traits_1::result_tag result_type_1;

    typedef typename detail::TypePromote3<T1,T2,T3,result_type_1>::temporary_type
        temporary_type;
    typedef typename temporary_type::value_type value_type;
    typedef typename temporary_type::cross_type cross_type;
    typedef et::ExprTraits<temporary_type> result_traits;
    typedef typename result_traits::size_tag size_tag;
    
    /**
     * NOTE: It seems that the equation for computing an intermediate
     * quaternion produces the same results regardless of whether 'standard'
     * or 'reverse' multiplication order is used (I haven't proved this -
     * I've just observed it). Still, just to be sure I've used a pair of
     * helper functions to ensure that the quaternions are multiplied in the
     * right order.
     */

    temporary_type result;
    detail::InterpResize(result, t1, size_tag());

    temporary_type t2_inverse = conjugate(t2);
    temporary_type temp1 = concatenate_quaternions(t1, t2_inverse, cross_type());
    temporary_type temp2 = concatenate_quaternions(t3, t2_inverse, cross_type());
    result = concatenate_quaternions(
        exp(-(log(temp1) + log(temp2)) * value_type(.25)), t2, cross_type());
    return result;
}

/**
 * NOTE: Construction of intermediate rotation matrices for use with squad
 * is currently implemented in terms of quaternions. This is pretty
 * inefficient (especially so in the 2-d case, which involves jumping through
 * a lot of hoops to get to 3-d and back), and is inelegant as well.
 *
 * I imagine this could be streamlined to work directly with the matrices, but
 * I'd need to dig a bit first (figure out the matrix equivalents of
 * quaternion exp() and log(), figure out what shortcuts can be taken in
 * 2-d, etc.), so for now it'll just have to remain as-is.
 *
 * In future versions of the CML, it might also be worth reconsidering
 * whether it's wise to support slerp and squad for matrices. Although it
 * can be done, it's not efficient, and may give the user a false sense of
 * security with respect to the efficiency of the underlying operations.
 */

template< class MatT_1, class MatT_2, class MatT_3, size_t N >
struct squad_intermediate_f;

template< class MatT_1, class MatT_2, class MatT_3 >
struct squad_intermediate_f<MatT_1,MatT_2,MatT_3,3>
{
    template< typename Real >
    typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
    operator()(
        const MatT_1& m1,
        const MatT_2& m2,
        const MatT_3& m3,
        Real tolerance)
    {
        typedef typename et::MatrixPromote3<
            MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;
        typedef typename temporary_type::value_type value_type;
        typedef quaternion< value_type > quaternion_type;

        quaternion_type q1, q2, q3;
        quaternion_rotation_matrix(q1, m1);
        quaternion_rotation_matrix(q2, m2);
        quaternion_rotation_matrix(q3, m3);

        quaternion_type q4 = squad_intermediate(q1, q2, q3, tolerance);

        temporary_type m;
        et::detail::Resize(m,3,3);
        
        matrix_rotation_quaternion(m, q4);
        
        return m;
    }
};

template< class MatT_1, class MatT_2, class MatT_3 >
struct squad_intermediate_f<MatT_1,MatT_2,MatT_3,2>
{
    template< typename Real >
    typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
    operator()(
        const MatT_1& m1,
        const MatT_2& m2,
        const MatT_3& m3,
        Real tolerance)
    {
        typedef typename et::MatrixPromote3<
            MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;
        typedef typename temporary_type::value_type value_type;
        typedef quaternion< value_type > quaternion_type;
        typedef vector< value_type, fixed<3> > vector_type;
        
        value_type angle1 = matrix_to_rotation_2D(m1);
        value_type angle2 = matrix_to_rotation_2D(m2);
        value_type angle3 = matrix_to_rotation_2D(m3);
        vector_type axis(value_type(0), value_type(0), value_type(1));
        
        quaternion_type q1, q2, q3;
        quaternion_rotation_axis_angle(q1, axis, angle1);
        quaternion_rotation_axis_angle(q2, axis, angle2);
        quaternion_rotation_axis_angle(q3, axis, angle3);

        quaternion_type q4 = squad_intermediate(q1, q2, q3, tolerance);
        
        value_type angle;
        quaternion_to_axis_angle(q4, axis, angle);

        temporary_type m;
        et::detail::Resize(m,2,2);
        
        matrix_rotation_2D(m, angle);
        
        return m;
    }
};

template< class MatT_1, class MatT_2, class MatT_3, typename Real >
typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
squad_intermediate(
    const MatT_1& m1,
    const MatT_2& m2,
    const MatT_3& m3,
    Real tolerance,
    et::matrix_result_tag,
    fixed_size_tag)
{
    return squad_intermediate_f<MatT_1,MatT_2,MatT_3,MatT_1::array_rows>()(
        m1,m2,m3,tolerance);
}

template< class MatT_1, class MatT_2, class MatT_3, typename Real >
typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
squad_intermediate(
    const MatT_1& m1,
    const MatT_2& m2,
    const MatT_3& m3,
    Real tolerance,
    et::matrix_result_tag,
    dynamic_size_tag)
{
    typedef typename et::MatrixPromote3<
        MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;

    temporary_type m;
    et::detail::Resize(m,m1.rows(),m1.cols());
    
    switch (m1.rows()) {
        case 3:
            m = squad_intermediate_f<MatT_1,MatT_2,MatT_3,3>()(m1,m2,m3,tolerance);
            break;
        case 2:
            m = squad_intermediate_f<MatT_1,MatT_2,MatT_3,2>()(m1,m2,m3,tolerance);
            break;
        default:
            throw std::invalid_argument(
                "matrix squad_intermediate_f() expects sizes 3x3 or 2x2");
            break;
    }
    return m;
}
#endif

//////////////////////////////////////////////////////////////////////////////
// Spherical linear interpolation of two vectors of any size
//////////////////////////////////////////////////////////////////////////////

template< class VecT_1, class VecT_2, typename Real, class SizeT >
typename detail::TypePromote<
    VecT_1,VecT_2,typename et::ExprTraits<VecT_1>::result_tag
>::temporary_type
slerp(
    const VecT_1& v1,
    const VecT_2& v2,
    Real t,
    Real tolerance,
    et::vector_result_tag,
    SizeT)
{
    typedef et::ExprTraits<VecT_1> type_traits;
    typedef typename type_traits::result_tag result_type;
    typedef typename
        detail::TypePromote<VecT_1,VecT_2,result_type>::temporary_type
            temporary_type;
    typedef typename temporary_type::value_type value_type;
    typedef et::ExprTraits<temporary_type> result_traits;
    typedef typename result_traits::size_tag size_tag;
            
    temporary_type result;
    detail::InterpResize(result, v1, size_tag());

    value_type omega = acos_safe(dot(v1,v2));
    value_type s = std::sin(omega);
    if (s < tolerance) {
        result = nlerp(v1,v2,t);
    } else {
        result = (value_type(std::sin((value_type(1)-t)*omega))*v1 +
            value_type(std::sin(t*omega))*v2) / s;
    }
    return result;
}

//////////////////////////////////////////////////////////////////////////////
// Spherical linear interpolation of two quaternions
//////////////////////////////////////////////////////////////////////////////

template< class QuatT_1, class QuatT_2, typename Real, class SizeT >
typename detail::TypePromote<
    QuatT_1,QuatT_2,typename et::ExprTraits<QuatT_1>::result_tag
>::temporary_type
slerp(
    const QuatT_1& q1,
    const QuatT_2& q2,
    Real t,
    Real tolerance,
    et::quaternion_result_tag,
    SizeT)
{
    typedef et::ExprTraits<QuatT_1> type_traits;
    typedef typename type_traits::result_tag result_type;
    typedef typename
        detail::TypePromote<QuatT_1,QuatT_2,result_type>::temporary_type
            temporary_type;
    typedef typename temporary_type::value_type value_type;

    temporary_type q3 = q2;
    value_type c = dot(q1,q3);
    if (c < value_type(0)) {
        // Turning this off temporarily to test squad...
        q3 = -q3;
        c = -c;
    }
    
    value_type omega = acos_safe(c);
    value_type s = std::sin(omega);

    return (s < tolerance) ?
        normalize(lerp(q1,q3,t)) :
        (value_type(std::sin((value_type(1) - t) * omega)) * q1+
            value_type(std::sin(t * omega)) * q3) / s;
}

//////////////////////////////////////////////////////////////////////////////
// Helper struct for spherical linear interpolation of 3x3 and 2x2 matrices
//////////////////////////////////////////////////////////////////////////////

template< class MatT_1, class MatT_2, size_t N > struct slerp_f;

template< class MatT_1, class MatT_2 > struct slerp_f<MatT_1,MatT_2,3>
{
    template< typename Real >
    typename detail::TypePromote<
        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
    >::temporary_type
    operator()(
        const MatT_1& m1,
        const MatT_2& m2,
        Real t,
        Real tolerance)
    {
        typedef typename detail::TypePromote<
            MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
        >::temporary_type temporary_type;

        temporary_type m;
        et::detail::Resize(m,3,3);
        m = matrix_rotation_difference(m1,m2);
        matrix_scale_rotation_angle(m,t,tolerance);
        m = detail::matrix_concat_rotations(m1,m);
        return m;
    }
};

template< class MatT_1, class MatT_2 > struct slerp_f<MatT_1,MatT_2,2>
{
    template< typename Real >
    typename detail::TypePromote<
        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
    >::temporary_type
    operator()(
        const MatT_1& m1,
        const MatT_2& m2,
        Real t,
        Real tolerance)
    {
        typedef typename detail::TypePromote<
            MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
        >::temporary_type temporary_type;

        temporary_type m;
        et::detail::Resize(m,2,2);
        m = matrix_rotation_difference_2D(m1,m2);
        matrix_scale_rotation_angle_2D(m,t,tolerance);
        m = detail::matrix_concat_rotations_2D(m1,m);
        return m;
    }
};

//////////////////////////////////////////////////////////////////////////////
// Spherical linear interpolation of two matrices of size 3x3 or 2x2
//////////////////////////////////////////////////////////////////////////////

template< class MatT_1, class MatT_2, typename Real >
typename detail::TypePromote<
    MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
>::temporary_type
slerp(
    const MatT_1& m1,
    const MatT_2& m2,
    Real t,
    Real tolerance,
    et::matrix_result_tag,
    fixed_size_tag)
{
    return slerp_f<MatT_1,MatT_2,MatT_1::array_rows>()(m1,m2,t,tolerance);
}

template< class MatT_1, class MatT_2, typename Real >
typename detail::TypePromote<
    MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
>::temporary_type
slerp(
    const MatT_1& m1,
    const MatT_2& m2,
    Real t,
    Real tolerance,
    et::matrix_result_tag,
    dynamic_size_tag)
{
    typedef typename detail::TypePromote<
        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
    >::temporary_type temporary_type;

    temporary_type m;
    et::detail::Resize(m,m1.rows(),m1.cols());
    
    switch (m1.rows()) {
        case 3:
            m = slerp_f<MatT_1,MatT_2,3>()(m1,m2,t,tolerance);
            break;
        case 2:
            m = slerp_f<MatT_1,MatT_2,2>()(m1,m2,t,tolerance);
            break;
        default:
            throw std::invalid_argument(
                "matrix slerp() expects sizes 3x3 or 2x2");
            break;
    }
    return m;
}

//////////////////////////////////////////////////////////////////////////////
// Normalized linear interpolation of two vectors of any size
//////////////////////////////////////////////////////////////////////////////

template< class VecT_1, class VecT_2, typename Real, class SizeT >
typename detail::TypePromote<
    VecT_1,VecT_2,typename et::ExprTraits<VecT_1>::result_tag
>::temporary_type
nlerp(
    const VecT_1& v1,
    const VecT_2& v2,
    Real t,
    et::vector_result_tag,
    SizeT)
{
    typedef et::ExprTraits<VecT_1> type_traits;
    typedef typename type_traits::result_tag result_type;
    typedef typename
        detail::TypePromote<VecT_1,VecT_2,result_type>::temporary_type
            temporary_type;
    typedef typename temporary_type::value_type value_type;
    typedef et::ExprTraits<temporary_type> result_traits;
    typedef typename result_traits::size_tag size_tag;
            
    temporary_type result;
    detail::InterpResize(result, v1, size_tag());

    result = (value_type(1)-t)*v1+t*v2;
    result.normalize();
    return result;
}

//////////////////////////////////////////////////////////////////////////////
// Normalized linear interpolation of two quaternions
//////////////////////////////////////////////////////////////////////////////

template< class QuatT_1, class QuatT_2, typename Real, class SizeT >
typename detail::TypePromote<
    QuatT_1,QuatT_2,typename et::ExprTraits<QuatT_1>::result_tag
>::temporary_type
nlerp(
    const QuatT_1& q1,
    const QuatT_2& q2,
    Real t,
    et::quaternion_result_tag,
    SizeT)
{
    typedef et::ExprTraits<QuatT_1> type_traits;
    typedef typename type_traits::result_tag result_type;
    typedef typename
        detail::TypePromote<QuatT_1,QuatT_2,result_type>::temporary_type
            temporary_type;
    typedef typename temporary_type::value_type value_type;

    return normalize(lerp(q1, (dot(q1,q2) < value_type(0)) ? -q2 : q2, t));
}

//////////////////////////////////////////////////////////////////////////////
// Helper struct for normalized linear interpolation of 3x3 and 2x2 matrices
//////////////////////////////////////////////////////////////////////////////

template< class MatT_1, class MatT_2, size_t N > struct nlerp_f;

template< class MatT_1, class MatT_2 > struct nlerp_f<MatT_1,MatT_2,3>
{
    template< typename Real >
    typename detail::TypePromote<
        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
    >::temporary_type
    operator()(
        const MatT_1& m1,
        const MatT_2& m2,
        Real t)
    {
        typedef typename detail::TypePromote<
            MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
        >::temporary_type temporary_type;
        typedef typename temporary_type::value_type value_type;

        temporary_type m;
        et::detail::Resize(m,3,3);
        m = lerp(m1,m2,t);
        matrix_orthogonalize_3x3(m);
        return m;
    }
};

template< class MatT_1, class MatT_2 > struct nlerp_f<MatT_1,MatT_2,2>
{
    template< typename Real >
    typename detail::TypePromote<
        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
    >::temporary_type
    operator()(
        const MatT_1& m1,
        const MatT_2& m2,
        Real t)
    {
        typedef typename detail::TypePromote<
            MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
        >::temporary_type temporary_type;
        typedef typename temporary_type::value_type value_type;

        temporary_type m;
        et::detail::Resize(m,2,2);
        m = lerp(m1,m2,t);
        matrix_orthogonalize_2x2(m);
        return m;
    }
};

//////////////////////////////////////////////////////////////////////////////
// Normalized linear interpolation of two matrices of size 3x3 or 2x2
//////////////////////////////////////////////////////////////////////////////

template< class MatT_1, class MatT_2, typename Real >
typename detail::TypePromote<
    MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
>::temporary_type
nlerp(
    const MatT_1& m1,
    const MatT_2& m2,
    Real t,
    et::matrix_result_tag,
    fixed_size_tag)
{
    return nlerp_f<MatT_1,MatT_2,MatT_1::array_rows>()(m1,m2,t);
}

template< class MatT_1, class MatT_2, typename Real >
typename detail::TypePromote<
    MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
>::temporary_type
nlerp(
    const MatT_1& m1,
    const MatT_2& m2,
    Real t,
    et::matrix_result_tag,
    dynamic_size_tag)
{
    typedef typename detail::TypePromote<
        MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
    >::temporary_type temporary_type;

    temporary_type m;
    et::detail::Resize(m,m1.rows(),m1.cols());
    
    switch (m1.rows()) {
        case 3:
            m = nlerp_f<MatT_1,MatT_2,3>()(m1,m2,t);
            break;
        case 2:
            m = nlerp_f<MatT_1,MatT_2,2>()(m1,m2,t);
            break;
        default:
            throw std::invalid_argument(
                "matrix nlerp() expects sizes 3x3 or 2x2");
            break;
    }
    return m;
}

} // namespace detail

//////////////////////////////////////////////////////////////////////////////
// Construction of 'intermediate' quaternions and matrices for use with squad
//////////////////////////////////////////////////////////////////////////////

/**
 * NOTE: Computation of intermediate rotation matrices for matrix 'squad'
 * doesn't seem to be working correctly. I'm not sure what the problem is
 * (it might have to do with q and -q representing the same rotation), but
 * in any case, I don't have time to get it sorted at the moment.
 *
 * In the meantime, I've just hacked in static assertions that will
 * restrict squad usage to quats. For anyone reading these comments, don't
 * worry: the quaternion verison of squad works just fine. However, you'll
 * just have to live without matrix squad for the time being (which is
 * probably just as well, given that matrix interpolation isn't terribly
 * efficient).
 */

#if 0
template< class T1, class T2, class T3 >
typename detail::TypePromote3<
    T1,T2,T3,typename et::ExprTraits<T1>::result_tag
>::temporary_type
squad_intermediate(
    const T1& t1,
    const T2& t2,
    const T3& t3,
    typename detail::TypePromote3<
        T1, T2, T3, typename et::ExprTraits<T1>::result_tag
    >::value_type tolerance =
    epsilon <
        typename detail::TypePromote3<
            T1, T2, T3, typename et::ExprTraits<T1>::result_tag
        >::value_type
    >::placeholder())
{
    // HACK: See note above...
    detail::CheckQuat(t1);
    detail::CheckQuat(t2);
    detail::CheckQuat(t3);

    typedef et::ExprTraits<T1> traits_1;
    typedef typename traits_1::result_tag result_type_1;

    typedef typename detail::TypePromote3<T1,T2,T3,result_type_1>::temporary_type
        temporary_type;
    typedef et::ExprTraits<temporary_type> result_traits;
    typedef typename result_traits::size_tag size_tag;

    temporary_type result;
    detail::InterpResize(result, t1, size_tag());

    result = detail::squad_intermediate(
        t1,t2,t3,tolerance,result_type_1(),size_tag());
    return result;
}

//////////////////////////////////////////////////////////////////////////////
// Spherical quadrangle interpolation of two quaternions or matrices
//////////////////////////////////////////////////////////////////////////////

/**
 * NOTE: The squad() impelementation is unfinished. I'm leaving the code
 * here (but preprocessor'ed out) for future reference.
 *
 * Currently, it seems that:
 *
 * 1. Computation of intermediate matrices is incorrect.
 * 2. The interpolated orientation sometimes 'jumps' while between nodes.
 *
 * I've observed that removing the 'shortest path' negation from the slerp
 * function eliminates the second problem. Also, in another implementation
 * of squad that I've seen, q1 and q2 are interpolated over the shortest
 * path, while the helper quaternions are not. I've never seen this
 * mentioned as a requirement of squad, but maybe they know something I
 * don't.
 *
 * For anyone who happens to read these comments, all of the other
 * interpolation functions (lerp, nlerp, slerp, etc.) should work fine -
 * it's just squad() that's on hold.
 */

template< class T1, class T2, class T3, class T4, typename Real >
typename detail::TypePromote4<
    T1,T2,T3,T4,typename et::ExprTraits<T1>::result_tag
>::temporary_type
squad(
    const T1& t1,
    const T2& t1_intermediate,
    const T3& t2_intermediate,
    const T4& t2,
    Real t,
    Real tolerance = epsilon<Real>::placeholder())
{
    // HACK: See note above...
    detail::CheckQuat(t1);
    detail::CheckQuat(t1_intermediate);
    detail::CheckQuat(t2_intermediate);
    detail::CheckQuat(t2);

    typedef et::ExprTraits<T1> traits_1;
    typedef typename traits_1::result_tag result_type_1;

    typedef typename detail::TypePromote4<
        T1,T2,T3,T4,result_type_1>::temporary_type temporary_type;
    typedef typename temporary_type::value_type value_type;
    typedef et::ExprTraits<temporary_type> result_traits;
    typedef typename result_traits::size_tag size_tag;

    temporary_type result;
    detail::InterpResize(result, t1, size_tag());
    
    result = slerp(
        slerp(t1, t2, t, tolerance),
        slerp(t1_intermediate, t2_intermediate, t, tolerance),
        value_type(2) * t * (value_type(1) - t),
        tolerance
    );

    return result;
}
#endif

//////////////////////////////////////////////////////////////////////////////
// Spherical linear interpolation of two vectors, quaternions or matrices
//////////////////////////////////////////////////////////////////////////////

template< class T1, class T2, typename Real >
typename detail::TypePromote<
    T1,T2,typename et::ExprTraits<T1>::result_tag
>::temporary_type
slerp(
    const T1& t1,
    const T2& t2,
    Real t,
    Real tolerance = epsilon<Real>::placeholder())
{
    typedef et::ExprTraits<T1> traits_1;
    typedef typename traits_1::result_tag result_type_1;

    typedef typename detail::TypePromote<T1,T2,result_type_1>::temporary_type
        temporary_type;
    typedef et::ExprTraits<temporary_type> result_traits;
    typedef typename result_traits::size_tag size_tag;

    temporary_type result;
    detail::InterpResize(result, t1, size_tag());

    result = detail::slerp(t1,t2,t,tolerance,result_type_1(),size_tag());
    return result;
}

//////////////////////////////////////////////////////////////////////////////
// Normalized linear interpolation of two vectors, quaternions or matrices
//////////////////////////////////////////////////////////////////////////////

template< class T1, class T2, typename Real >
typename detail::TypePromote<
    T1,T2,typename et::ExprTraits<T1>::result_tag
>::temporary_type
nlerp(const T1& t1, const T2& t2, Real t)
{
    typedef et::ExprTraits<T1> traits_1;
    typedef typename traits_1::result_tag result_type_1;

    typedef typename detail::TypePromote<T1,T2,result_type_1>::temporary_type
        temporary_type;
    typedef et::ExprTraits<temporary_type> result_traits;
    typedef typename result_traits::size_tag size_tag;

    temporary_type result;
    detail::InterpResize(result, t1, size_tag());

    result = detail::nlerp(t1,t2,t,result_type_1(),size_tag());
    return result;
}

//////////////////////////////////////////////////////////////////////////////
// Linear interpolation of two values of any qualified type
//////////////////////////////////////////////////////////////////////////////

/** Linear interpolation of 2 values.
 *
 * @note The data points are assumed to be sampled at u = 0 and u = 1, so
 * for interpolation u must lie between 0 and 1.
 */
template< class T1, class T2, typename Scalar >
typename detail::TypePromote<
    T1,T2,typename et::ExprTraits<T1>::result_tag
>::temporary_type
lerp(const T1& val0, const T2& val1, Scalar u)
{
    typedef
        typename detail::TypePromote<
            T1,T2,typename et::ExprTraits<T1>::result_tag
        >::temporary_type temporary_type;

    typedef et::ExprTraits<temporary_type> result_traits;
    typedef typename result_traits::size_tag size_tag;

    temporary_type result;
    detail::InterpResize(result, val1, size_tag());
    
    result = (Scalar(1) - u) * val0 + u * val1;
    return result;
}

//////////////////////////////////////////////////////////////////////////////
// Bilinear interpolation of four values of any qualified type
//////////////////////////////////////////////////////////////////////////////

template < class T1, class T2, class T3, class T4, typename Scalar >
typename detail::TypePromote<
    typename detail::TypePromote<
        T1,T2,typename et::ExprTraits<T1>::result_tag
    >::temporary_type,
    typename detail::TypePromote<
        T3,T4,typename et::ExprTraits<T3>::result_tag
    >::temporary_type,
    typename et::ExprTraits<T1>::result_tag
>::temporary_type
bilerp(const T1& val00, const T2& val10,
       const T3& val01, const T4& val11,
       Scalar u, Scalar v)
{
    typedef
        typename detail::TypePromote<
            typename detail::TypePromote<
                T1,T2,typename et::ExprTraits<T1>::result_tag
            >::temporary_type,
            typename detail::TypePromote<
                T3,T4,typename et::ExprTraits<T1>::result_tag
            >::temporary_type,
            typename et::ExprTraits<T1>::result_tag
        >::temporary_type temporary_type;

    typedef et::ExprTraits<temporary_type> result_traits;
    typedef typename result_traits::size_tag size_tag;

    temporary_type result;
    detail::InterpResize(result, val00, size_tag());

    Scalar uv = u * v;
    result = (
        (Scalar(1.0) - u - v + uv) * val00 +
                          (u - uv) * val10 +
                          (v - uv) * val01 +
                                uv * val11
    );
    return result;
}

//////////////////////////////////////////////////////////////////////////////
// Trilinear interpolation of eight values of any qualified type
//////////////////////////////////////////////////////////////////////////////

/** Trilinear interpolation of 8 values.
 *
 * @note The data values are assumed to be sampled at the corners of a unit
 * cube, so for interpolation, u, v, and w must lie between 0 and 1.
 */
template < class T1, class T2, class T3, class T4,
           class T5, class T6, class T7, class T8,
           typename Scalar >
typename detail::TypePromote<
    typename detail::TypePromote<
        typename detail::TypePromote<
            T1,T2,typename et::ExprTraits<T1>::result_tag
        >::temporary_type,
        typename detail::TypePromote<
            T3,T4,typename et::ExprTraits<T3>::result_tag
        >::temporary_type,
        typename et::ExprTraits<T1>::result_tag
    >::temporary_type,
    typename detail::TypePromote<
        typename detail::TypePromote<
            T5,T6,typename et::ExprTraits<T5>::result_tag
        >::temporary_type,
        typename detail::TypePromote<
            T7,T8,typename et::ExprTraits<T7>::result_tag
        >::temporary_type,
        typename et::ExprTraits<T1>::result_tag
    >::temporary_type,
    typename et::ExprTraits<T1>::result_tag
>::temporary_type
trilerp(const T1& val000, const T2& val100,
        const T3& val010, const T4& val110,
        const T5& val001, const T6& val101,
        const T7& val011, const T8& val111,
        Scalar u, Scalar v, Scalar w)
{
    typedef
        typename detail::TypePromote<
            typename detail::TypePromote<
                typename detail::TypePromote<
                    T1,T2,typename et::ExprTraits<T1>::result_tag
                >::temporary_type,
                typename detail::TypePromote<
                    T3,T4,typename et::ExprTraits<T1>::result_tag
                >::temporary_type,
                typename et::ExprTraits<T1>::result_tag
            >::temporary_type,
            typename detail::TypePromote<
                typename detail::TypePromote<
                    T5,T6,typename et::ExprTraits<T1>::result_tag
                >::temporary_type,
                typename detail::TypePromote<
                    T7,T8,typename et::ExprTraits<T1>::result_tag
                >::temporary_type,
                typename et::ExprTraits<T1>::result_tag
            >::temporary_type,
            typename et::ExprTraits<T1>::result_tag
        >::temporary_type temporary_type;

    typedef et::ExprTraits<temporary_type> result_traits;
    typedef typename result_traits::size_tag size_tag;

    temporary_type result;
    detail::InterpResize(result, val000, size_tag());

    Scalar uv = u * v;
    Scalar vw = v * w;
    Scalar wu = w * u;
    Scalar uvw = uv * w;
    
    result = (
        (Scalar(1.0) - u - v - w + uv + vw + wu - uvw) * val000 +
                                   (u - uv - wu + uvw) * val100 +
                                   (v - uv - vw + uvw) * val010 +
                                            (uv - uvw) * val110 +
                                   (w - vw - wu + uvw) * val001 +
                                            (wu - uvw) * val101 +
                                            (vw - uvw) * val011 +
                                                   uvw * val111
    );
    return result;
}

} // namespace cml

#endif