// Copyright Andrew Little 2005
//
// 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)
//
// See QUAN_ROOT/quan_matters/index.html for documentation.

/*
    in place matrix multiply. Final version ? :-)
*/

#define FUSION_MAX_VECTOR_SIZE 20
#include <quan/fusion/boost/matrix_column_sequence.hpp>
#include <quan/fusion/boost/matrix_row_sequence.hpp>
#include <quan/fusion/dot_product.hpp>
#include <quan/matrix/rc_matrix_def.hpp>
#include <quan/static.hpp>
#include <quan/matrix/out/matrix.hpp>

//build the result_type
template <typename RhsSequence>
struct push_back_dot_product{

    template <typename LhsSequence,typename InputSequence>
    struct result{
   
        typedef typename boost::fusion::result_of::push_back<
            InputSequence const ,
            typename quan::fusion::dot_product::result<
                LhsSequence,RhsSequence
            >::type
        >::type type;
    };
};

template <typename SeqOfSeq>
struct fold_seq_of_seq_by_seq{

    template <typename Sequence, typename InputSequence>
    struct result{
        typedef typename boost::fusion::result_of::fold<
            SeqOfSeq,
            InputSequence,
            push_back_dot_product<Sequence>
        >::type type;
    };
};

struct matrix_mux_elements{

    template <typename SeqOfSequenceL, typename SeqOfSequenceR>
    struct result{
        typedef fold_seq_of_seq_by_seq<SeqOfSequenceR> const start;
        typedef typename boost::fusion::result_of::fold<
            SeqOfSequenceL,
            boost::fusion::vector0,
            start
        >::type type; 
    };
};

template <int R, int C>
struct eval_dot{

    template <typename Matrix1, typename Matrix2>
    struct result{
        typedef boost::fusion::matrix_row<Matrix1,R> row;
        typedef boost::fusion::matrix_column<Matrix2,C> col;
        typedef typename row::sequence_type  row_seq;
        typedef typename col::sequence_type  col_seq; 
        typedef typename quan::fusion::dot_product:: template result<
            row_seq,col_seq
        >::type type;
    };

    template <typename Matrix1, typename Matrix2>
    typename result<const Matrix1,const Matrix2>::type
    operator() (Matrix1 const & m1, Matrix2 const & m2) const
    {
        typename result< const Matrix1, const Matrix2>::row row(m1);
        typename result< const Matrix1, const Matrix2>::col col(m2);
        quan::fusion::dot_product dot;
        return dot(row.m_sequence,col.m_sequence);
    }
};

template <int R, int C, typename Matrix1, typename Matrix2,typename ResultMatrix>
struct eval_rc;

template < typename Matrix1, typename Matrix2,typename ResultMatrix>
struct eval_rc<0,0,Matrix1, Matrix2,ResultMatrix>{

    void operator()(Matrix1 const & m1, Matrix2 const & m2,ResultMatrix & r)const
    {
        eval_dot<0,0> dot;
        r.at<0,0>() = dot(m1,m2);
    }
};

template <int R, typename Matrix1, typename Matrix2, typename ResultMatrix>
struct eval_rc<R,0,Matrix1, Matrix2,ResultMatrix>{

    void operator()(Matrix1 const & m1, Matrix2 const & m2,ResultMatrix & r)const
    {
        eval_rc<R-1,Matrix2::cols-1,Matrix1, Matrix2,ResultMatrix> prev;
        prev(m1,m2,r);
        
        eval_dot<R,0> dot;
        r.at<R,0>() = dot(m1,m2) ;
    }
};

template <int R, int C, typename Matrix1, typename Matrix2, typename ResultMatrix>
struct eval_rc{

    void operator()(Matrix1 const & m1, Matrix2 const & m2,ResultMatrix & r)const
    {
        eval_rc<R,C-1,Matrix1, Matrix2,ResultMatrix> prev;
        prev(m1,m2,r); 

        eval_dot<R,C> dot;
        r.at<R,C>() = dot(m1,m2) ;
    }
};

// matrix x matrix multiply

struct matrix_mux1{

    template <typename Matrix1, typename Matrix2>
    struct result{

        typedef boost::fusion::matrix_row_sequence<Matrix1> seqL;
        typedef boost::fusion::matrix_column_sequence<Matrix2> seqR;
        
        typedef typename boost::fusion::result_of::as_vector<
            typename matrix_mux_elements::template result<seqL,seqR>::type
        >::type elements_type;

        typedef quan::rc_matrix<Matrix1::rows,Matrix2::cols,elements_type> type;
    };

    template <typename Matrix1, typename Matrix2>
    typename result<Matrix1,Matrix2>::type
    operator()(Matrix1 const & m1, Matrix2 const & m2)const
    {
        BOOST_STATIC_ASSERT((Matrix1::cols == Matrix2::rows));
        typedef  typename result<Matrix1,Matrix2>::type result_type;
        eval_rc<
            result_type::rows -1,result_type::cols -1,Matrix1,Matrix2,result_type
        > eval;
        result_type result;
        eval(m1,m2,result);
        return result;
    }
};

#include <quan/angle.hpp>
#include <quan_matters/src/constant.cpp>
int main()
{
    using quan::static_;

    typedef static_<double,0>::type zero;
    typedef static_<double,1>::type one;

    typedef boost::fusion::vector16<
        double, double, zero, zero,
        double, double, zero, zero,
        zero,     zero,  one, zero,
        zero,     zero, zero,  one
    > elements1;

    typedef quan::rc_matrix<4,4,elements1> matrix_type1;

    quan::angle::rad theta = quan::angle::deg(90);
    matrix_type1 matrix1(
        elements1(
            quan::cos(theta) ,quan::sin(theta),  zero() , zero(),
            -quan::sin(theta),quan::cos(theta),  zero() , zero(),
            zero()           ,          zero(),   one() , zero(),
            zero()           ,          zero(),  zero() ,  one()
        )
    );

    typedef boost::fusion::vector4<
        double,zero,zero, one
    > elements2;

    typedef quan::rc_matrix<1,4,elements2> coord_type;

    coord_type vect(elements2(1,zero(),zero(),one()));
    
    matrix_mux1 mux;
    std::cout.setf(std::ios_base::fixed,std::ios_base::floatfield);
    
    std::cout.precision(3);
    std::cout << "input  coordinate : " << vect<<'\n';
    std::cout << "output coordinate : " << mux(vect,matrix1) << '\n';
   
}