#include <algorithm>
#include <boost/numeric/bindings/lapack/computational/gbtrf.hpp>
#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/ublas/banded.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/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <cstddef>
#include <iostream>
#include <string>


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


template <
	typename ValueT,
	typename LayoutT,
	typename StorageT
>
ValueT* band_mat_to_lapack_vec(ublas::banded_matrix<ValueT,LayoutT,StorageT> const& A, ::fortran_int_t& n)
{
	typedef ublas::banded_matrix<ValueT,LayoutT,StorageT> matrix_type;
	typedef typename ublas::matrix_traits<matrix_type>::size_type size_type;

	const size_type kl = A.lower();
	const size_type ku = A.upper();
	const size_type nr = ublas::num_rows(A);
	const size_type nc = ublas::num_columns(A);

	n = (2*kl+ku+1)*nc;

	ValueT* v = new ValueT[n];

	for (size_type r = 0; r < nr; ++r)
	{
		for (size_type c = 0; c < nc; ++c)
		{
			size_type rr = kl+ku+r-c;
			size_type k = (2*kl+ku+1)*c+rr;

			if ((r == c) || (c <= (ku+r) && r <= (c+kl)))
			{
				v[k] = A(r,c);
			}
			else
			{
				v[k] = ValueT();
			}
		}
	}

	return v;
}


template <typename T>
void print_vector(std::string const& desc, T const* a, std::size_t n)
{
	std::cout << std::endl << desc << std::endl << "  [";
	for(std::size_t i = 0; i < n; ++i)
	{
		std::cout << " " << std::fixed << a[i];
	}
	std::cout << "]" << std::endl;
}


// OK TEST: directly use bindings::lapack::detail::gbtrf
template <
	typename ValueT,
	typename StorageT
>
void test_ok(ublas::banded_matrix<ValueT,ublas::column_major,StorageT> const& A)
{
	std::cout << "OK TEST" << std::endl;


	typedef ublas::banded_matrix<ValueT,ublas::column_major,StorageT> matrix_type;
	typedef typename ublas::matrix_traits<matrix_type>::value_type value_type; 
	typedef typename ublas::matrix_traits<matrix_type>::size_type size_type; 

	::fortran_int_t m = ublas::num_rows(A);
	::fortran_int_t n = ublas::num_columns(A);
	::fortran_int_t kl = A.lower();
	::fortran_int_t ku = A.upper();
	::fortran_int_t ldab = 2*kl+ku+1;
	::fortran_int_t k;

	// Create a vectorized form of A according to ?GBTRF doc and in col-major form.
	value_type* ab = band_mat_to_lapack_vec(A, k);

	print_vector("AB =", ab, k);

	::fortran_int_t* ipiv = new ::fortran_int_t[::std::min(m,n)];

	bindings::lapack::detail::gbtrf(
		m,
		n,
		kl,
		ku,
		ab,
		ldab,
		ipiv
	);

	print_vector("GBTRF -> AB =", ab, k);
	print_vector("GBTRF -> IPIV =", ipiv, n);

	delete[] ipiv;
	ipiv = 0;
	delete[] ab;
	ab = 0;
}


// KO TEST #1: use bindings::lapack::gbtrf
template <
	typename ValueT,
	typename StorageT
>
void test_ko_1(ublas::banded_matrix<ValueT,ublas::column_major,StorageT> const& A)
{
	std::cout << "KO TEST #1" << std::endl;


	typedef ublas::banded_matrix<ValueT,ublas::column_major,StorageT> matrix_type;
	typedef typename ublas::matrix_traits<matrix_type>::value_type value_type; 
	typedef typename ublas::matrix_traits<matrix_type>::size_type size_type; 
	typedef ublas::matrix<value_type, ublas::column_major> dense_matrix_type;

	// LAPACK work with dense matrices
	dense_matrix_type aux_AB(A);
	// ... but we need to "mark" the matrix as banded
	ublas::banded_adaptor<dense_matrix_type> AB(aux_AB, A.lower(), A.upper());

	std::cout << "AB = " << AB << std::endl;

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

	ublas::vector< ::fortran_int_t > ipiv(k);

	bindings::lapack::gbtrf(AB, ipiv);

	std::cout << "GBTRF -> AB = " << AB << std::endl;
	std::cout << "GBTRF -> IPIV = " << ipiv << std::endl;
}


// KO TEST #2: use bindings::lapack::gbtrf
template <
	typename ValueT,
	typename StorageT
>
void test_ko_2(ublas::banded_matrix<ValueT,ublas::column_major,StorageT> const& A)
{
	std::cout << "KO TEST #2" << std::endl;


	typedef ublas::banded_matrix<ValueT,ublas::column_major,StorageT> matrix_type;
	typedef typename ublas::matrix_traits<matrix_type>::value_type value_type; 
	typedef typename ublas::matrix_traits<matrix_type>::size_type size_type; 
	typedef ublas::matrix<value_type, ublas::column_major> dense_matrix_type;

	size_type nr = ublas::num_rows(A);
	size_type nc = ublas::num_columns(A);
	size_type kl = A.lower();
	size_type ku = A.upper();
	size_type ldab = 2*kl+ku+1;

	// Create a copy of matrix A with a form described in ?GBTRF doc
	dense_matrix_type AB(ldab, nc, value_type());
	for (size_type r = 0; r < nr; ++r)
	{
		for (size_type c = 0; c < nc; ++c)
		{
			size_type rr = kl+ku+r-c;
			if ((r == c) || (c <= (r+ku) && r <= (c+kl)))
			{
				AB(rr,c) = A(r,c);
			}
		}
	}

	std::cout << "AB = " << AB << std::endl;

	ublas::vector< ::fortran_int_t > ipiv(std::min(nr, nc));

	bindings::lapack::gbtrf(AB, ipiv); //DO NOT COMPILE!

	std::cout << "GBTRF -> AB = " << AB << std::endl;
	std::cout << "GBTRF -> IPIV = " << ipiv << std::endl;
}


// KO TEST #2: use bindings::lapack::gbtrf
template <
	typename ValueT,
	typename StorageT
>
void test_ko_3(ublas::banded_matrix<ValueT,ublas::column_major,StorageT> const& A)
{
	std::cout << "KO TEST #3" << std::endl;


	typedef ublas::banded_matrix<ValueT,ublas::column_major,StorageT> matrix_type;
	typedef typename ublas::matrix_traits<matrix_type>::value_type value_type; 
	typedef typename ublas::matrix_traits<matrix_type>::size_type size_type; 
	typedef ublas::matrix<value_type, ublas::column_major> dense_matrix_type;

	size_type nr = ublas::num_rows(A);
	size_type nc = ublas::num_columns(A);
	size_type kl = A.lower();
	size_type ku = A.upper();
	size_type ldab = 2*kl+ku+1;

	// Create a copy of matrix A with a form described in ?GBTRF doc
	dense_matrix_type aux_AB(ldab, nc, value_type());
	for (size_type r = 0; r < nr; ++r)
	{
		for (size_type c = 0; c < nc; ++c)
		{
			size_type rr = kl+ku+r-c;
			if ((r == c) || (c <= (r+ku) && r <= (c+kl)))
			{
				aux_AB(rr,c) = A(r,c);
			}
		}
	}

	// Make it appear as a banded matrix (even it is not!!!!)
	ublas::banded_adaptor<dense_matrix_type> AB(aux_AB, kl, ku);

	std::cout << "AB = " << AB << std::endl;

	ublas::vector< ::fortran_int_t > ipiv(std::min(nr, nc));

	bindings::lapack::gbtrf(AB, ipiv); //DO NOT COMPILE!

	std::cout << "GBTRF -> AB = " << AB << std::endl;
	std::cout << "GBTRF -> IPIV = " << ipiv << std::endl;
}


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

	const std::size_t nr = 4;
	const std::size_t nc = 4;
	const std::size_t l = 1;
	const std::size_t u = 2;

    matrix_type A(nr,nc,l,u);
    A(0,0) = -0.23; A(0,1) = 2.54; A(0,2) = -3.66;         /* 0 */ // 1st row
    A(1,0) = -6.98; A(1,1) = 2.46; A(1,2) = -2.73; A(1,3) = -2.13; // 2nd row
            /* 0 */ A(2,1) = 2.56; A(2,2) =  2.46; A(2,3) =  4.07; // 3rd row
            /* 0 */        /* 0 */ A(3,2) = -4.78; A(3,3) = -3.82; // 4th row

	test_ok(A);
	test_ko_1(A);
	//test_ko_2(A); // DO NOT COMPILE (cause: line 193)
	test_ko_3(A);
}