#include <boost/numeric/bindings/lapack/computational/ormql.hpp>
#include <boost/numeric/bindings/lapack/computational/geqlf.hpp>
#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/bindings/tag.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation/num_columns.hpp>
#include <boost/numeric/ublas/operation/num_rows.hpp>
#include <boost/numeric/ublas/operation/size.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <cstddef>
#include <iostream>


namespace ublas = boost::numeric::ublas;
namespace bindings = boost::numeric::bindings;

/**
 * \brief Perform the QL factorization of \a A and returns the result in \a QL
 *  and \a tau.
 */
template <typename MatrixT, typename VectorT>
void ql_decompose(MatrixT const& A, MatrixT& QL, VectorT& tau)
{
	typedef typename ublas::matrix_traits<MatrixT>::value_type size_type;

	size_type m = ublas::num_rows(A);
	size_type n = ublas::num_columns(A);
	size_type k = std::min(m, n);

	QL = A;

	if (ublas::size(tau) != k)
	{
		tau.resize(k, false);
	}

	bindings::lapack::geqlf(QL, tau);
}


/**
 * Multiplies the QL factorization of A by matrix C.
 * If
 * - left==true and trans==false  ==> Q*C
 * - left==true and trans==true   ==> Q'*C
 * - left==false and trans==false ==> C*Q
 * - left==false and trans==true  ==> C*Q'
 * .
 */
template <typename MatrixT, typename VectorT>
MatrixT ql_prod(MatrixT const& QL, VectorT const& tau, MatrixT const& C, bool left, bool trans)
{
	MatrixT X(C);

	// Perform product
	if (left)
	{
		if (trans)
		{
			// DO NOT COMPILE
			bindings::lapack::ormql(
				bindings::tag::left(),
				bindings::trans(QL),
				tau,
				X
			);
		}
		else
		{
			bindings::lapack::ormql(
				bindings::tag::left(),
				QL,
				tau,
				X
			);
		}
	}
	else
	{
		if (trans)
		{
			// DO NOT COMPILE
			bindings::lapack::ormql(
				bindings::tag::right(),
				bindings::trans(QL),
				tau,
				X
			);
		}
		else
		{
			bindings::lapack::ormql(
				bindings::tag::right(),
				QL,
				tau,
				X
			);
		}
	}

	return X;
}


int main()
{
	typedef double value_type;
	typedef ublas::matrix<value_type, ublas::column_major> matrix_type;
	typedef ublas::vector<value_type> vector_type;

	// Set-up the matrix to be factorized

	const std::size_t A_nr(6);
	const std::size_t A_nc(4);

	matrix_type A(A_nr,A_nc);

	A(0,0) = -0.57; A(0,1) = -1.28; A(0,2) = -0.39; A(0,3) =  0.25;
	A(1,0) = -1.93; A(1,1) =  1.08; A(1,2) = -0.31; A(1,3) = -2.14;
	A(2,0) =  2.30; A(2,1) =  0.24; A(2,2) =  0.40; A(2,3) = -0.35;
	A(3,0) = -1.93; A(3,1) =  0.64; A(3,2) = -0.66; A(3,3) =  0.08;
	A(4,0) =  0.15; A(4,1) =  0.30; A(4,2) =  0.15; A(4,3) = -2.13;
	A(5,0) = -0.02; A(5,1) =  1.03; A(5,2) = -1.43; A(5,3) =  0.50;


	// Perform the QL factorization of A

	matrix_type QL;
	vector_type tau;

	ql_decompose(A, QL, tau);
	std::cout << "QL = " << QL << std::endl;
	std::cout << "tau = " << tau << std::endl;


	// Set-up matrices for QL product

	const std::size_t C1_nr(6);
	const std::size_t C1_nc(2);

	matrix_type C1(C1_nr, C1_nc);

	C1(0,0) = -2.67; C1(0,1) =  0.41;
	C1(1,0) = -0.55; C1(1,1) = -3.10;
	C1(2,0) =  3.34; C1(2,1) = -4.01;
	C1(3,0) = -0.77; C1(3,1) =  2.76;
	C1(4,0) =  0.48; C1(4,1) = -6.17;
	C1(5,0) =  4.10; C1(5,1) =  0.21;

	matrix_type C2(ublas::trans(C1));


	// Perform QL products

	matrix_type X; // the output matrix

	X = ql_prod(QL, tau, C1, true, false);
	std::cout << "Q*C1 = " << X << std::endl;

	X = ql_prod(QL, tau, C1, true, true);
	std::cout << "Q'*C1 = " << X << std::endl;

	X = ql_prod(QL, tau, C2, false, false);
	std::cout << "C2*Q = " << X << std::endl;

	X = ql_prod(QL, tau, C2, false, true);
	std::cout << "C2*Q' = " << X << std::endl;
}