#define VIENNACL_WITH_OPENCL
#define VIENNACL_WITH_OPENMP
#define VIENNACL_HAVE_EIGEN

#include <typeinfo>

#include <sisl/sisl.hpp>
#include <sisl/lattice/bcc_odd.hpp>
#include <sisl/lattice/cartesian_odd.hpp>
#include <sisl/lattice/fcc_odd.hpp>

#include <Eigen/Sparse>

#include "viennacl/scalar.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/matrix_proxy.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/linalg/inner_prod.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/norm_2.hpp"

namespace sisl{
namespace utility{
extern const char * _bcc_optimization_kernel_dbl;
extern const char * _cc_optimization_kernel_dbl;
extern const char * _cc2d_optimization_kernel_dbl;

/**
 * Samples the basis function specified by
 * GF at locations specified by samplePts,
 * returns an Eigen::SparseMatrix that
 * corresponds to \mathbf{Phi} in the paper
 */
template <class L, class GF, class I, class O>
void createBigPhi(
		const std::vector<vector3<I>> &samplePts,
		L *l,
		vector3<I> latticeShift,
		viennacl::compressed_matrix<O> &phi,
		viennacl::compressed_matrix<O> &phiT) {

	int nSize = l->numberOfLatticeSites();
	std::vector<Eigen::Triplet<O>> eigenMap;
	std::vector<std::tuple<int,int,int>> st = GF::getSupport();
	I dh = l->getScale();

	// Sample the basis function to create the Xi vector
	for(unsigned int ptIndex = 0; ptIndex < samplePts.size(); ptIndex++) {
		vector3<I> pt = samplePts[ptIndex] + (latticeShift * dh);
		vector3<int> pIdx = l->getNearestIndex(pt);

		printf("%f %f %f;\r",pt.i, pt.j,pt.k );
		// Sample each lattice site
		for(unsigned int stIdx = 0; stIdx < st.size(); stIdx++) {
			std::tuple<int,int,int> s = st[stIdx];
			vector3<I> q = pt - l->getSitePosition(
				std::get<0>(s) + pIdx.i,
				std::get<1>(s) + pIdx.j,
				std::get<2>(s) + pIdx.k);

			int mindex = l->lIndex(
				std::get<0>(s) + pIdx.i,
				std::get<1>(s) + pIdx.j,
				std::get<2>(s) + pIdx.k);

			O value = GF::M(q.i/dh, q.j/dh, q.k/dh);

			if(value == 0 || mindex < 0) continue;
			eigenMap.push_back(Eigen::Triplet<O>(ptIndex, mindex, value));
		}
	}
	Eigen::SparseMatrix<O> cpuPhi(samplePts.size(), nSize);
	Eigen::SparseMatrix<O> cpuPhiT(nSize, samplePts.size());

	cpuPhi.setFromTriplets(eigenMap.begin(), eigenMap.end());
	cpuPhiT = Eigen::SparseMatrix<O>(cpuPhi.transpose());

	printf("\nCopying to gpu...\n");
	viennacl::copy(cpuPhi, phi);
	viennacl::copy(cpuPhiT, phiT);

	printf("Copying OK...\n");
}

template <class L, class GF, class I, class O>
void createBigPhi2D(
		const std::vector<vector2<I>> &samplePts,
		L *l,
		vector2<I> latticeShift,
		viennacl::compressed_matrix<O> &phi,
		viennacl::compressed_matrix<O> &phiT) {

	int nSize = l->numberOfLatticeSites();
	std::vector<Eigen::Triplet<O>> eigenMap;
	std::vector<std::tuple<int,int>> st = GF::getSupport();
	I dh = l->getScale();

	// Sample the basis function to create the Xi vector
	for(unsigned int ptIndex = 0; ptIndex < samplePts.size(); ptIndex++) {
		vector2<I> pt = samplePts[ptIndex] + (latticeShift * dh);
		vector2<int> pIdx = l->getNearestIndex(pt);

		// Sample each lattice site
		for(unsigned int stIdx = 0; stIdx < st.size(); stIdx++) {
			std::tuple<int,int> s = st[stIdx];
			vector2<I> q = pt - l->getSitePosition(
				std::get<0>(s) + pIdx.i,
				std::get<1>(s) + pIdx.j);

			int mindex = l->lIndex(
				std::get<0>(s) + pIdx.i,
				std::get<1>(s) + pIdx.j);

			O value = GF::M(q.i/dh, q.j/dh);

			if(value == 0 || mindex < 0) continue;
			eigenMap.push_back(Eigen::Triplet<O>(ptIndex, mindex, value));
		}
	}
	Eigen::SparseMatrix<O> cpuPhi(samplePts.size(), nSize);
	Eigen::SparseMatrix<O> cpuPhiT(nSize, samplePts.size());

	cpuPhi.setFromTriplets(eigenMap.begin(), eigenMap.end());
	cpuPhiT = Eigen::SparseMatrix<O>(cpuPhi.transpose());

	printf("\nCopying to gpu...\n");
	viennacl::copy(cpuPhi, phi);
	viennacl::copy(cpuPhiT, phiT);

	printf("Copying OK...\n");
}

template <class L, class GF, class I, class O>
void splatSamplesOntoLattice(
		const std::vector<vector3<I>> &samplePts,
		const std::vector<O> &samples,
		L *l,
		vector3<I> latticeShift) {

	int nSize = l->numberOfLatticeSites();
	std::vector<std::tuple<int,int,int>> st = GF::getSupport();
	I dh = l->getScale();

	// Sample the basis function to create the Xi vector
	for(unsigned int ptIndex = 0; ptIndex < samplePts.size(); ptIndex++) {
		vector3<I> pt = samplePts[ptIndex] + (latticeShift * dh);
		vector3<int> pIdx = l->getNearestIndex(pt);

		// Sample each lattice site
		//#pragma omp parallel for
		for(unsigned int stIdx = 0; stIdx < st.size(); stIdx++) {
			std::tuple<int,int,int> s = st[stIdx];
			vector3<I> q = pt - l->getSitePosition(
				std::get<0>(s) + pIdx.i,
				std::get<1>(s) + pIdx.j,
				std::get<2>(s) + pIdx.k);

			O value = GF::M(q.i/dh, q.j/dh, q.k/dh) * samples[ptIndex] + l->GV(
					std::get<0>(s) + pIdx.i,
					std::get<1>(s) + pIdx.j,
					std::get<2>(s) + pIdx.k);

			l->SV(
				std::get<0>(s) + pIdx.i,
				std::get<1>(s) + pIdx.j,
				std::get<2>(s) + pIdx.k, value);
		}
	}
}

template <class L, class GF, class I, class O>
void splatSamplesOntoLattice2D(
		const std::vector<vector2<I>> &samplePts,
		const std::vector<O> &samples,
		L *l,
		vector2<I> latticeShift) {

	int nSize = l->numberOfLatticeSites();
	std::vector<std::tuple<int,int>> st = GF::getSupport();
	I dh = l->getScale();

	// Sample the basis function to create the Xi vector
	for(unsigned int ptIndex = 0; ptIndex < samplePts.size(); ptIndex++) {
		vector2<I> pt = samplePts[ptIndex] + (latticeShift * dh);
		vector2<int> pIdx = l->getNearestIndex(pt);

		// Sample each lattice site
		//#pragma omp parallel for
		for(unsigned int stIdx = 0; stIdx < st.size(); stIdx++) {
			std::tuple<int,int> s = st[stIdx];
			vector2<I> q = pt - l->getSitePosition(
				std::get<0>(s) + pIdx.i,
				std::get<1>(s) + pIdx.j);

			O value = GF::M(q.i/dh, q.j/dh) * samples[ptIndex] + l->GV(
					std::get<0>(s) + pIdx.i,
					std::get<1>(s) + pIdx.j);

			l->SV(
				std::get<0>(s) + pIdx.i,
				std::get<1>(s) + pIdx.j, value);
		}
	}
}

template <class L, class GF, class I, class O>
bool ConjugateGradientBCC(
		viennacl::compressed_matrix<O> &phi,
		viennacl::compressed_matrix<O> &phiT,
		const std::vector<O> &samples,
		const std::vector<std::tuple<int,int,int,O>> &optimizationVector,
		L *l,
		O *err = NULL, int *iter = NULL) {

	int nSize = l->numberOfLatticeSites();
	int niter = 0;

/*	std::vector<double> cpuX(nSize);
	l->forEachLatticeSite([&](const int &i, const int &j, const int &k) {
		return cpuX[l->lIndex(i,j,k)] = l->GV(i,j,k);
	});*/

	// Setup the residual vectors
	viennacl::vector<O> residual(nSize), p(nSize), Ap(nSize), x(nSize);
	viennacl::vector<O> apTT(samples.size()), lsample(samples.size());

	// Initialize with zero
	residual.clear(); p.clear();
	Ap.clear(); x.clear(); apTT.clear();

//	viennacl::copy(cpuX, x);

	O resNew = 0;
	O resOld = 0;
	O alpha = 0;

	viennacl::copy(samples, lsample);

	// Do an initial iteration
	residual = viennacl::linalg::prod(phiT, lsample);
	p = viennacl::linalg::prod(phiT, lsample);
	resNew = viennacl::linalg::inner_prod(p,p);
	resOld = resNew;

	viennacl::ocl::handle<cl_mem> xr, yr, zr, blw;
	viennacl::ocl::program &normMtxPrgm = viennacl::ocl::current_context().add_program(sisl::utility::_bcc_optimization_kernel_dbl, "blCode");

	// Setup the opencl kernel for execution
	{
		std::vector<cl_int> xv, yv, zv;
		std::vector<cl_double> w;

		for(auto itr = optimizationVector.begin(); itr != optimizationVector.end(); ++itr) {
			auto t = (*itr);
			xv.push_back(std::get<0>(t));
			yv.push_back(std::get<1>(t));
			zv.push_back(std::get<2>(t));
			w.push_back(std::get<3>(t));
		}

		xr = viennacl::ocl::current_context().create_memory(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
						1024 * sizeof(cl_int), &(xv[0]));

		yr = viennacl::ocl::current_context().create_memory(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
						1024 * sizeof(cl_int), &(yv[0]));

		zr = viennacl::ocl::current_context().create_memory(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
						1024 * sizeof(cl_int), &(zv[0]));

		blw = viennacl::ocl::current_context().create_memory(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
						1024 * sizeof(cl_double), &(w[0]));
	}

	viennacl::ocl::kernel &blKern = normMtxPrgm.get_kernel("blnormbcc");
	//normMtxPrgm.add_kernel(blKern, "blnormbcc");

	for(niter = 0; niter < 5000; niter++) {
		alpha = 0;
		apTT = viennacl::linalg::prod(phi, p);
		Ap = viennacl::linalg::prod(phiT, apTT);

		viennacl::ocl::enqueue(
			blKern(p,
				xr, yr, zr, blw, Ap,
				static_cast<cl_uint>(nSize),
				static_cast<cl_uint>(l->getResolution()),
				static_cast<cl_double>(1)));
		viennacl::backend::finish();

		alpha = resOld/viennacl::linalg::inner_prod(p, Ap);
		x = x + alpha * p;
		residual = residual - (alpha * Ap);
		resNew = viennacl::linalg::inner_prod(residual, residual);

		if(sqrt(resNew) < 1e-5)
			break;

		printf("%d: %f...\r",niter, sqrt(resNew));
		std::cout.flush();

		p = residual + (resNew/resOld)*p;
		resOld=resNew;
	}

	residual.clear();
	p.clear();
	Ap.clear();
	apTT.clear();

	std::vector<double> cpuX(nSize);
	viennacl::copy(x, cpuX);
	x.clear();

	l->forEachLatticeSite([&](const int &i, const int &j, const int &k) {
		return cpuX[l->lIndex(i,j,k)];
	});
}

template <class L, class GF, class I, class O>
bool ConjugateGradientCC(
		viennacl::compressed_matrix<O> &phi,
		viennacl::compressed_matrix<O> &phiT,
		const std::vector<O> &samples,
		const std::vector<std::tuple<int,int,int,O>> &optimizationVector,
		L *l,
		O *err = NULL, int *iter = NULL) {

	int nSize = l->numberOfLatticeSites();
	int niter = 0;

	// Setup the residual vectors
	viennacl::vector<O> residual(nSize), p(nSize), Ap(nSize), x(nSize);
	viennacl::vector<O> apTT(samples.size()), lsample(samples.size());

	// Initialize with zero
	residual.clear(); p.clear();
	Ap.clear(); x.clear(); apTT.clear();

	O resNew = 0;
	O resOld = 0;
	O alpha = 0;

	viennacl::copy(samples, lsample);

	// Do an initial iteration
	residual = viennacl::linalg::prod(phiT, lsample);
	p = viennacl::linalg::prod(phiT, lsample);
	resNew = viennacl::linalg::inner_prod(p,p);
	resOld = resNew;

	viennacl::ocl::handle<cl_mem> xr, yr, zr, blw;
	viennacl::ocl::program &normMtxPrgm = viennacl::ocl::current_context().add_program(sisl::utility::_cc_optimization_kernel_dbl, "blCode");

	// Setup the opencl kernel for execution
	{
		std::vector<cl_int> xv, yv, zv;
		std::vector<cl_double> w;

		for(auto itr = optimizationVector.begin(); itr != optimizationVector.end(); ++itr) {
			auto t = (*itr);
			xv.push_back(std::get<0>(t));
			yv.push_back(std::get<1>(t));
			zv.push_back(std::get<2>(t));
			w.push_back(std::get<3>(t));
		}

		xr = viennacl::ocl::current_context().create_memory(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
						1024 * sizeof(cl_int), &(xv[0]));

		yr = viennacl::ocl::current_context().create_memory(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
						1024 * sizeof(cl_int), &(yv[0]));

		zr = viennacl::ocl::current_context().create_memory(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
						1024 * sizeof(cl_int), &(zv[0]));

		blw = viennacl::ocl::current_context().create_memory(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
						1024 * sizeof(cl_double), &(w[0]));
	}

	viennacl::ocl::kernel &blKern = normMtxPrgm.get_kernel("blnorm");
	//normMtxPrgm.add_kernel(blKern, "blnorm");

	for(niter = 0; niter < 5000; niter++) {
		alpha = 0;
		apTT = viennacl::linalg::prod(phi, p);
		Ap = viennacl::linalg::prod(phiT, apTT);

		viennacl::ocl::enqueue(
			blKern(p,
				xr, yr, zr, blw, Ap,
				static_cast<cl_uint>(nSize),
				static_cast<cl_uint>(l->getResolution()),
				static_cast<cl_uint>(l->getResolution()),
				static_cast<cl_uint>(l->getResolution()),
				static_cast<cl_double>(1)));
		viennacl::backend::finish();

		alpha = resOld/viennacl::linalg::inner_prod(p, Ap);
		x = x + alpha * p;
		residual = residual - (alpha * Ap);
		resNew = viennacl::linalg::inner_prod(residual, residual);

		if(sqrt(resNew) < 1e-5)
			break;

		printf("%d: %f...\r",niter, sqrt(resNew));
		std::cout.flush();

		p = residual + (resNew/resOld)*p;
		resOld=resNew;
	}

	residual.clear();
	p.clear();
	Ap.clear();
	apTT.clear();

	std::vector<double> cpuX(nSize);
	viennacl::copy(x, cpuX);
	x.clear();

	l->forEachLatticeSite([&](const int &i, const int &j, const int &k) {
		return cpuX[l->lIndex(i,j,k)];
	});
}

template <class L, class GF, class I, class O>
bool ConjugateGradientCC2(
		viennacl::compressed_matrix<O> &phi,
		viennacl::compressed_matrix<O> &phiT,
		const std::vector<O> &samples,
		const std::vector<std::tuple<int,int,O>> &optimizationVector,
		L *l,
		O *err = NULL, int *iter = NULL) {

	int nSize = l->numberOfLatticeSites();
	int niter = 0;

	// Setup the residual vectors
	viennacl::vector<O> residual(nSize), p(nSize), Ap(nSize), x(nSize);
	viennacl::vector<O> apTT(samples.size()), lsample(samples.size());

	// Initialize with zero
	residual.clear(); p.clear();
	Ap.clear(); x.clear(); apTT.clear();

	O resNew = 0;
	O resOld = 0;
	O alpha = 0;

	viennacl::copy(samples, lsample);

	// Do an initial iteration
	residual = viennacl::linalg::prod(phiT, lsample);
	p = viennacl::linalg::prod(phiT, lsample);
	resNew = viennacl::linalg::inner_prod(p,p);
	resOld = resNew;

	viennacl::ocl::handle<cl_mem> xr, yr, blw;
	viennacl::ocl::program &normMtxPrgm = viennacl::ocl::current_context().add_program(sisl::utility::_cc2d_optimization_kernel_dbl, "blCode");

	// Setup the opencl kernel for execution
	{
		std::vector<cl_int> xv, yv;
		std::vector<cl_double> w;

		for(auto itr = optimizationVector.begin(); itr != optimizationVector.end(); ++itr) {
			auto t = (*itr);
			xv.push_back(std::get<0>(t));
			yv.push_back(std::get<1>(t));
			w.push_back(std::get<2>(t));
 		}

		xr = viennacl::ocl::current_context().create_memory(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
						1024 * sizeof(cl_int), &(xv[0]));

		yr = viennacl::ocl::current_context().create_memory(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
						1024 * sizeof(cl_int), &(yv[0]));

		blw = viennacl::ocl::current_context().create_memory(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
						1024 * sizeof(cl_double), &(w[0]));
	}

	viennacl::ocl::kernel &blKern = normMtxPrgm.get_kernel("blnorm");

	for(niter = 0; niter < 5000; niter++) {
		alpha = 0;
		apTT = viennacl::linalg::prod(phi, p);
		Ap = viennacl::linalg::prod(phiT, apTT);

		viennacl::ocl::enqueue(
			blKern(p,
				xr, yr, blw, Ap,
				static_cast<cl_uint>(nSize),
				static_cast<cl_uint>(l->getResolution()),
				static_cast<cl_uint>(l->getResolution()),
				static_cast<cl_double>(1)));
		viennacl::backend::finish();

		alpha = resOld/viennacl::linalg::inner_prod(p, Ap);
		x = x + alpha * p;
		residual = residual - (alpha * Ap);
		resNew = viennacl::linalg::inner_prod(residual, residual);

		if(sqrt(resNew) < 1e-5)
			break;

		printf("%d: %f...\r",niter, sqrt(resNew));
		std::cout.flush();

		p = residual + (resNew/resOld)*p;
		resOld=resNew;
	}

	residual.clear();
	p.clear();
	Ap.clear();
	apTT.clear();

	std::vector<double> cpuX(nSize);
	viennacl::copy(x, cpuX);
	x.clear();

	l->forEachLatticeSite([&](const int &i, const int &j) {
		return cpuX[l->lIndex(i,j)];
	});
}

template <class L, class GF, class I, class O>
bool fitPoints(
		const std::vector<vector3<I>> &samplePts,
		const std::vector<O> &samples,
		const std::vector<std::tuple<int,int,int,O>> &optimizationVector,
		L *l,
		vector3<I> latticeShift,
		O *err = NULL, int *iter = NULL) {

	int nSize = l->numberOfLatticeSites();
	viennacl::compressed_matrix<O> phi(samplePts.size(), nSize), phiT(nSize, samplePts.size());
	createBigPhi<L,GF,I,O>(samplePts, l, latticeShift, phi, phiT);

	if(l->getLatticeName() == "bcc")
		ConjugateGradientBCC<L,GF,I,O>(phi, phiT, samples, optimizationVector, l, err, iter);
	else if(l->getLatticeName() == "cartesian")
		ConjugateGradientCC<L,GF,I,O>(phi, phiT, samples, optimizationVector, l, err, iter);
	else
		throw "fitPoints() - No cl kernel exists for this lattice type?";
}

template <class L, class GF, class I, class O>
bool fitPoints2D(
		const std::vector<vector2<I>> &samplePts,
		const std::vector<O> &samples,
		const std::vector<std::tuple<int,int,O>> &optimizationVector,
		L *l,
		vector2<I> latticeShift,
		O *err = NULL, int *iter = NULL) {

	int nSize = l->numberOfLatticeSites();
	viennacl::compressed_matrix<O> phi(samplePts.size(), nSize), phiT(nSize, samplePts.size());
	createBigPhi2D<L,GF,I,O>(samplePts, l, latticeShift, phi, phiT);

	if(l->getLatticeName() == "bcc")
		; //ConjugateGradientBCC<L,GF,I,O>(phi, phiT, samples, optimizationVector, l, err, iter);
	else if(l->getLatticeName() == "cartesian2")
		ConjugateGradientCC2<L,GF,I,O>(phi, phiT, samples, optimizationVector, l, err, iter);
	else
		throw "fitPoints() - No cl kernel exists for this lattice type?";
}

const char * _bcc_optimization_kernel_dbl =
"#pragma OPENCL EXTENSION cl_khr_fp64 : enable \n"
"#define MODP(x, d) (((x)%(d) + (d)) % (d)) \n"
"__kernel void blnormbcc(\n"
"		  __global const double * vectorIn,\n"
"		  __global const int * xOffset, \n"
"		  __global const int * yOffset, \n"
"		  __global const int * zOffset, \n"
"		  __global const double * weights,\n"
"		  __global double * result,\n"
"		  unsigned int size, \n"
"		  unsigned int res, \n"
"		  double lambda ) { \n"
"		int res2 = res * 2; \n"

"	for (unsigned int i = get_global_id(0); i < size; i += get_global_size(0)) { \n"
"		double temp = 0;	\n"

"		int z = (i % res2); \n"
"		int y = ((i - z)/res2) % res; \n"
"		int x = (((i - z)/res2) - y) / res; \n"
"		x = (x*2 + (z%2));\n"
"		y = (y*2 + (z%2));\n"

"		for(unsigned int j = 0; j < 175; j++) { \n"
"			int xx = (xOffset[j] + x); \n"
"			int yy = (yOffset[j] + y); \n"
"			int zz = (zOffset[j] + z); \n"
"			xx = (xx - abs(zz % 2))/ 2; \n"
"			yy = (yy - abs(zz % 2))/2; \n"

"			int index = zz + res2*(yy+res*xx); \n"

"			index = (xx < 0 || yy < 0 || zz < 0 || xx >= res2 || yy >= res2 || zz >= res2 || index >= size) ? -1 : index; \n"
"			temp += (index>=0)?vectorIn[index]*weights[j]:0; \n"
"		}\n"
"		result[i] += temp; \n"
"	}"
"};\n";

const char * _cc_optimization_kernel_dbl =
"#pragma OPENCL EXTENSION cl_khr_fp64 : enable \n"
"__kernel void blnorm(\n"
"       __global const double * vectorIn,\n"
"       __global const int * xOffset, \n"
"       __global const int * yOffset, \n"
"       __global const int * zOffset, \n"
"       __global const double * weights,\n"
"       __global double * result,\n"
"       unsigned int size, \n"
"		unsigned int nx, \n"
"		unsigned int ny, \n"
"		unsigned int nz, \n"
"		double lambda) { \n"
"	for (unsigned int i = get_global_id(0); i < size; i += get_global_size(0)) { \n"
"		double temp = 0;	\n"
"		int z = i % nz; \n"
"		int y = ((i - z)/nz) % ny; \n"
"		int x = (((i - z)/nz) - y) / ny; \n"
"		for(unsigned int j = 0; j < 343; j++) { \n"
"			int xx = xOffset[j] + x; \n"
"			int yy = yOffset[j] + y; \n"
"			int zz = zOffset[j] + z; \n"
"			int index = zz + nx*(yy+ny*xx); \n"
"			if(xx < 0 || yy < 0 || zz < 0 || xx >= nx || yy >= ny || zz >= nz || index >= size) continue; \n"
"			temp += vectorIn[index]*weights[j]; \n"
"		}\n"
"		result[i] += temp; \n"
"	}"
"};\n";

const char * _cc2d_optimization_kernel_dbl =
"#pragma OPENCL EXTENSION cl_khr_fp64 : enable \n"
"__kernel void blnorm(\n"
"       __global const double * vectorIn,\n"
"       __global const int * xOffset, \n"
"       __global const int * yOffset, \n"
"       __global const double * weights,\n"
"       __global double * result,\n"
"       unsigned int size, \n"
"		unsigned int nx, \n"
"		unsigned int ny, \n"
"		double lambda) { \n"
"	for (unsigned int i = get_global_id(0); i < size; i += get_global_size(0)) { \n"
"		double temp = 0;	\n"
"		int y = i % ny; \n"
"		int x = ((i - y)/ny) % ny; \n"
"		for(unsigned int j = 0; j < 50; j++) { \n"
"			int xx = xOffset[j] + x; \n"
"			int yy = yOffset[j] + y; \n"
"			int index = yy + ny*xx; \n"
"			if(xx < 0 || yy < 0 || xx >= nx || yy >= ny || index >= size) continue; \n"
"			temp += vectorIn[index]*weights[j]; \n"
"		}\n"
"		result[i] += temp; \n"
"	}"
"};\n";
};
};


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_NEGS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_NEGS_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/negs.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::negs_, tag::cpu_
                            , (A0)
                            , ((generic_<signed_<A0> >))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return if_else(eq(a0, Valmin<result_type>()),Valmax<result_type>(),unary_minus(a0));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::negs_, tag::cpu_
                                   , (A0)
                                   , ((generic_<floating_<A0> >))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1) { return unary_minus(a0); }
  };
} } }

#endif


// Copyright Louis Dionne 2016
// Distributed under the Boost Software License, Version 1.0.

#define BOOST_HANA_CONFIG_ENABLE_STRING_UDL
#include <boost/hana.hpp>

#include <cassert>
#include <iostream>
#include <string>
namespace hana = boost::hana;
using namespace hana::literals;

// sample(setup)
template <typename T, typename U, typename V>
struct Triple {
    T first;
    U second;
    V third;
};

struct A { char data; };
struct B { int data; };
struct C { double data; };

// What's the most packed layout?
// end-sample

// sample(main)
template <typename Types>
auto packed_triple(Types const& types) {
    auto sorted = hana::sort(types, [](auto t, auto u) {
        return hana::alignof_(t) > hana::alignof_(u);
    });

    auto triple = hana::unpack(sorted, [](auto ...t) {
        using Answer = Triple<typename decltype(t)::type...>;
        return hana::type<Answer>{};
    });

    return triple;
}

int main() {
    hana::tuple<hana::type<A>, hana::type<B>, hana::type<C>> types;
    using Packed = decltype(packed_triple(types))::type;
    Packed triple{/* ... */};
}
// end-sample


//
//  post-solver.hpp
//  Parametrize
//
//  Created by Jingwei on 2/5/18.
//

#ifndef post_solver_h
#define post_solver_h

#include <Eigen/Core>
#include <vector>
#include "qflow/disajoint-tree.hpp"

namespace qflow {

using namespace Eigen;

/*
 * TODO: Optimize O_quad, and possibly N_quad
 * Input:
 *  O_quad[i]: initialized i-th vertex position of the quad mesh
 *  N_quad[i]: initialized i-th vertex normal of the quad mesh
 *  Q_quad[i]: initialized i-th vertex orientation of the quad mesh, guaranteed to be orthogonal to
 *             N_quad[i]
 *  F_quad[i]: 4 vertex index of the i-th quad face
 *
 *  Concept: i-th directed edge is the (i%4)-th edge of the (i/4)-th face of the quad mesh
 *    V2E_quad[i]: one directed edge from i-th vertex of the quad mesh
 *    E2E_quad[i]: the reverse directed edge's index of the i-th directed edge of the quad mesh
 *
 *  V.col(i): i-th vertex position of the triangle mesh
 *  N.col(i): i-th vertex normal of the triangle mesh
 *  Q.col(i): i-th vertex orientation of the triangle mesh, guaranteed to be orthogonal to N.col(i)
 *  O.col(i): "quad position" associated with the i-th vertex in the triangle mesh (see InstantMesh
 *            position field)
 *  F.col(i): i-th triangle of the triangle mesh
 *
 *  V2E[i]: one directed edge from the i-th vertex of the triangle mesh
 *  E2E[i]: the reverse directed edge's index of the i-th directed edge of the triangle mesh
 *
 *  j = disajoint_tree.Index(i)
 *      the j-th vertex of the quad mesh is corresponding to the i-th vertex of the triangle mesh
 *      the relation is one-to-multiple
 *      O_quad can be viewed as an average of corresponding O
 *      N_quad can be viewed as an average of corresponding N
 *      Q_quad can be viewed as aggregation of corresponding Q
 *          Method that aggregates qi to qj with weights wi and wj:
 *              value = compat_orientation_extrinsic_4(qj, nj, qi, ni)
 *              result = (value.first * wj + value.second * wi).normalized()
 *
 * Output:
 *  Optimized O_quad, (possibly N_quad)
 */
void optimize_quad_positions(std::vector<Vector3d>& O_quad, std::vector<Vector3d>& N_quad,
                             std::vector<Vector3d>& Q_quad, std::vector<Vector4i>& F_quad,
                             VectorXi& V2E_quad, std::vector<int>& E2E_quad, MatrixXd& V, MatrixXd& N,
                             MatrixXd& Q, MatrixXd& O, MatrixXi& F, VectorXi& V2E, VectorXi& E2E,
                             DisajointTree& disajoint_tree, double reference_length,
                             bool just_serialize = true);

} // namespace qflow

#endif /* post_solver_h */


#include <armadillo>
#include <string>
#include "TSP.h"

using namespace std;

int main(int argc, char **argv) {
  string fname;

  if (argc > 1) {
    fname = argv[1];
  }

  auto dist = get_dist_tsp(fname);
  int cities = dist.n_rows;
  cout <<dist;

  ivec sol = new_solution(cities);
  double fit = fitness(sol, dist);
  cout <<"Fitness: " <<fit <<endl;

  printf("Hello, world\n");
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_GENERAL_LOGM_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_GENERAL_LOGM_HPP_INCLUDED

#include <nt2/linalg/functions/logm.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/atanh.hpp>
#include <nt2/include/functions/ceil.hpp>
#include <nt2/include/functions/cons.hpp>
#include <nt2/include/functions/ctranspose.hpp>
#include <nt2/include/functions/colon.hpp>
#include <nt2/include/functions/diag_of.hpp>
#include <nt2/include/functions/divides.hpp>
#include <nt2/include/functions/dot.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/find.hpp>
#include <nt2/include/functions/from_diag.hpp>
#include <nt2/include/functions/globalnone.hpp>
#include <nt2/include/functions/isdiagonal.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/is_lez.hpp>
#include <nt2/include/functions/length.hpp>
#include <nt2/include/functions/linsolve.hpp>
#include <nt2/include/functions/log.hpp>
#include <nt2/include/functions/logical_and.hpp>
#include <nt2/include/functions/minusone.hpp>
#include <nt2/include/functions/mtimes.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/include/functions/mnorm1.hpp>
#include <nt2/include/functions/oneplus.hpp>
#include <nt2/include/functions/schur.hpp>
#include <nt2/include/functions/size.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/symeig.hpp>
#include <nt2/include/functions/transpose.hpp>
#include <nt2/include/functions/twopower.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/isempty.hpp>
#include <nt2/include/functions/horzcat.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/sdk/complex/meta/is_complex.hpp>
#include <boost/assert.hpp>

namespace nt2
{
  namespace ext
  {
    BOOST_DISPATCH_IMPLEMENT  ( logm_, tag::cpu_
                              , (A0)
                              , (scalar_< unspecified_<A0> >)
                              )
    {
      typedef A0 result_type;
      NT2_FUNCTOR_CALL(1)
      {
        return nt2::log(a0);
      }
    };

    // logm tag only used for matrix
    template<class Domain, int N, class Expr>
    struct size_of<tag::logm_,Domain,N,Expr>
    {
      typedef typename boost::proto::result_of::child_c<Expr&,0>::value_type  c0_t;
      typedef _2D                                                      result_type;
      BOOST_FORCEINLINE result_type operator()(Expr& e) const
      {
        BOOST_ASSERT_MSG(issquare(boost::proto::child_c<0>(e)),
                         "logm needs a square matrix or a scalar");
        return nt2::extent(boost::proto::child_c<0>(e));
      }
    };

    BOOST_DISPATCH_IMPLEMENT  ( run_assign_, tag::cpu_
                                , (A0)(A1)
                                , ((ast_<A0, nt2::container::domain>))
                                  ((node_<A1, nt2::tag::logm_, boost::mpl::long_<1>, nt2::container::domain>))
                                )
    {
    typedef void                                                    result_type;
    typedef typename A1::value_type                                  value_type;
    typedef typename boost::proto::result_of::child_c<A1&,0>::type         Out0;
    typedef typename meta::as_real<value_type>::type                     r_type;
    typedef typename meta::as_complex<r_type>::type                   cplx_type;
    typedef typename meta::as_integer<r_type>::type                      i_type;
    typedef nt2::table<value_type>                                        tab_t;
    typedef nt2::table<r_type>                                           btab_t;
    typedef table<cplx_type>                                             ctab_t;
    typedef table<i_type>                                                itab_t;
    typedef typename A0::index_type                                  index_type;

    BOOST_FORCEINLINE result_type operator()(A0& a0, const A1& a1) const
    {
      compute_logm(boost::proto::child_c<0>(a1), a0);
    }

    private:
      template < class T >
      BOOST_FORCEINLINE static void compute_logm(const T& a0, A0& res)
      {
        //u, t and r are complex arrays
        res.resize(extent(a0));
        ctab_t u, t;
        nt2::tie(u, t) = schur(a0, nt2::cmplx_);
        BOOST_ASSERT_MSG(nt2::globalnone(is_eqz(nt2::diag_of(t))), "a0 has null eigenvalue(s)");
        BOOST_ASSERT_MSG(nt2::globalnone(nt2::logical_and(is_eqz(imag(nt2::diag_of(t))),
                                                          is_lez(real(nt2::diag_of(t))))), "a0 has non positive real eigenvalue(s)");
        if (isdiagonal(t))
        {
          t = nt2::from_diag(nt2::log(nt2::diag_of(t)));
          BOOST_AUTO_TPL(r, nt2::mtimes(u, nt2::mtimes(t, nt2::ctrans(u))));
          transtype(res, r, typename nt2::meta::is_complex<value_type>::type());
          return;
        }
        else
        {
          ctab_t r = zeros(extent(a0), meta::as_<cplx_type>());
          r_type delta = 0.1;
          itab_t  ord(nt2::of_size(2u, nt2::size(a0,1)));
          blocking(diag_of(t),delta, ord);
          uint32_t lord = nt2::size(ord, 2);
          for(uint32_t col=1; col <= lord ; ++col)
          {
            BOOST_AUTO_TPL(j, nt2::_(ord(1, col), ord(2, col)));
            uint32_t maxsqrt = 100;
            itab_t terms(nt2::of_size(1, lord));
            uint32_t nj =  length(j);
            ctab_t rj(nt2::of_size(nj, nj));
            terms(col) = logm_triang(a0(j, j), maxsqrt, rj);
            r(j, j) = rj;
            for(uint32_t row=col-1; row >= 1; --row)
            {
              BOOST_AUTO_TPL(i, nt2::_(ord(1, row), ord(2, row)));
              if (length(i) == 1 && length(j) == 1)
              {
                size_t ii = i(1), jj = j(1);
                BOOST_AUTO_TPL(k, nt2::_(ii+1, jj-1));
                cplx_type temp = a0(ii,jj)*(r(ii,ii) - r(jj,jj));
                if (!isempty(k)) temp += dot(a0(k,jj), nt2::ctrans(r(ii,k)))- dot(a0(ii,k), nt2::ctrans(r(k,jj)));
                r(ii,jj) = temp/(a0(ii,ii)-a0(jj,jj));
              }
              else
              {
                itab_t k(nt2::of_size(1, 0));
                for(uint32_t l = row+1; l < col; ++l)
                {
                  itab_t k1 = horzcat(k, nt2::_(ord(1, l), ord(2, l)));
                  k = k1;
                }
                ctab_t rhs =  mtimes(r(i,i), a0(i,j)) - mtimes(a0(i,j), r(j,j));
                if(!isempty(k)) rhs += mtimes(r(i,k), a0(k,j)) -  mtimes(a0(i,k), r(k,j));
                r(i,j) = sylv_tri(a0(i,i),-a0(j,j),rhs);
              }
            }
            ctab_t z =  mtimes(mtimes(u, r), ctrans(u));
            transtype(res, z, typename nt2::meta::is_complex<value_type>::type());
          }
        }
      }

      template < class T, class U, class B >
      static inline ctab_t sylv_tri(const T& t,const U& u, const B& b)
      {
        // sylv_tri    solve triangular sylvester equation.
        //    x = sylv_tri(t,u,b) solves the sylvester equation
        //  t*x + x*u = b, where t and u are square upper triangular matrices.

        uint32_t m = length(t);
        uint32_t n = length(u);
        ctab_t x = zeros(m,n, nt2::meta::as_<cplx_type>());
        for(uint32_t i = 1;  i <= n; ++i)
        {
          ctab_t bb = b(nt2::_,i);
          BOOST_AUTO_TPL(ii, nt2::_(1u, i-1));
          if(!isempty(ii)) bb -= mtimes(x(nt2::_,ii),u(ii,i));
          x(nt2::_,i) = linsolve(t + u(i,i)*eye(m, meta::as_<cplx_type>()),
                                 bb);
        }
        return x;
      }

      template < class T1, class T2 >
      BOOST_FORCEINLINE static void transtype(T1& r, T2& z, boost::mpl::false_ const &)
      {
        r =  real(z);
      }

      template < class T1, class T2 >
      BOOST_FORCEINLINE static void transtype(T1& r, T2& z, boost::mpl::true_ const &)
      {
        r =  z;
      }
      template < class D >
      static inline void blocking(const D& a0, const r_type& delta, itab_t & ord)
      {
        uint32_t n = nt2::size(a0, 1);
        uint32_t j = 1;
        ord(1, 1) = 1;
        for(uint32_t i=2; i <= n; ++i)
        {
          if (nt2::abs(a0(i-1)-a0(i)) >= delta)
          {
            ord(2, j) = i-1;
            ord(1, ++j) = i;
          }
        }
        ord(2, j) = n;
        ord.resize(nt2::of_size(2u, j));
      }

      template < class D1, class D2 > static inline
      uint32_t logm_triang(const D1& a0, uint32_t maxsqrt,
                           D2& rj)
      {
        uint32_t term = 0;
        if (nt2::size(a0, 1) == 1)
        {
          rj(1) = log(a0(1));
        }
        else if(nt2::size(a0, 1) == 2)
        {
          logm2by2(a0, maxsqrt, rj);
        }
        else
        {
          logm_tr(a0, maxsqrt, rj, term);
        }
        return term;
      }

      template < class D1, class D2 > static inline
      void logm2by2(const D1& a0,
                    uint32_t /*maxsqrt*/,
                    D2& rj)
      {
        value_type a1 = a0(1,1);
        value_type a2 = a0(2,2);
        value_type loga1 = log(a1);
        value_type loga2 = log(a2);
        rj = nt2::cons(nt2::of_size(2, 2), loga1, Zero<value_type>(), Zero<value_type>(), loga2);
        if (a1 == a2)
        {
          rj(1,2) = a0(1,2)/a1;
        }
        else if ((nt2::abs(a1) < nt2::Half<r_type>()*nt2::abs(a2)) ||
                 (nt2::abs(a2) < nt2::Half<r_type>()*nt2::abs(a1)))
        {

          rj(1,2) =  a0(1,2) * (loga2 - loga1) / (a2 - a1);
        }
        else // Close eigenvalues.
        {
          //dd = (2*atanh((a2-a1)/(a2+a1)))
          //  dd=(dd+ 2*pi*1i*unwinding(loga2-loga1)) / (a2-a1)
          value_type dd = Two<value_type>()*nt2::atanh(nt2::divides((a2-a1), (a2+a1)));
          dd += unwind(loga2-loga1, meta::is_complex<value_type>());
          dd /= a2-a1;
          rj(1,2) = nt2::multiplies(a0(1,2), dd);
        }
      }
      static inline r_type unwind(const value_type& /*z*/, boost::mpl::false_)
      {
        return Zero<r_type>();
      }
      static inline cplx_type unwind(const value_type& z, boost::mpl::true_)
      {
        static const r_type _2pi = nt2::Two<r_type>()*nt2::Pi<r_type>();
        return cplx_type(Zero<r_type>(), _2pi*nt2::ceil((imag(z)-nt2::Pi<r_type>())/_2pi));
      }

      template < class D1, class D2 > static inline
      void logm_tr(const D1& a0,
                  uint32_t maxsqrt,
                  D2& rj,
                  uint32_t& /*term*/)
      {
        btab_t xvals = nt2::cons<r_type>(1.6206284795015669e-002,   // m = 3
                                         5.3873532631381268e-002,   // m = 4
                                         1.1352802267628663e-001,   // m = 5
                                         1.8662860613541296e-001,   // m = 6
                                         2.6429608311114350e-001    // m = 7
                                        );
        ctab_t t = a0;
        uint32_t k = 0, p = 0;
        uint32_t n = nt2::size(t, 1);
        uint32_t m = 0;
        while (true)
        {
          r_type normdiff = nt2::mnorm1(t-eye(n, meta::as_<value_type>()));
          if (normdiff <= xvals(end_))
          {
            ++p;
            BOOST_AUTO_TPL( tj1, nt2::find(nt2::le(normdiff, xvals)));
            uint32_t j1 = tj1(1)+2;
            normdiff /= 2;
            BOOST_AUTO_TPL( tj2, nt2::find(nt2::le(normdiff, xvals)));
            uint32_t j2 = tj2(1)+2;
            if ((j1-j2 < 2) || (p == 2))
            {
              m = j1;
              break;
            }
          }

          if (k == maxsqrt)
          {
            m = 16; break;
          }
          sqrtm_tri(t);
          ++k;
        }
        logm_pf(t-eye(n, meta::as_<cplx_type>()),m, rj);
        rj *= r_type(twopower(k));
      }
      template < class T > static inline
      void sqrtm_tri( T& t )
      {
        uint32_t n = nt2::size(t, 1);
        ctab_t r = nt2::zeros(nt2::of_size(n, n), meta::as_<cplx_type>());
        for (uint32_t j=1; j <= n; ++j)
        {
          r(j,j) = nt2::sqrt(t(j,j));
          for(uint32_t  i=j-1;  i >= 1; --i)
          {
            r(i,j) = t(i,j);
            if (i+1 <= j-1)
            {
              ctab_t z1 = r(i,nt2::_(i+1, j-1));
              ctab_t z2 = r(nt2::_(i+1, j-1),j);
              ctab_t z3 = mtimes(z1, z2);
              r(i, j) -= z3;
            }
            r(i,j) /= (r(i,i) + r(j,j));
          }
        }
        t = r;
      }

      template < class D, class S > static inline
      void logm_pf(const D& a0,
                   uint32_t m,
                   S& s)
      {
        //    LOGM_PF   Pade approximation to matrix log by partial fraction expansion.
        //    LOGM_PF(A,m) is an [m/m] Pade approximant to LOG(EYE(SIZE(A))+A).

        uint32_t n = nt2::size(a0, 1);
        btab_t nodes, wts;
        gauss_legendre(m, nodes, wts);
        //  Convert from [-1,1] to [0,1].
        nodes = oneplus(nodes)/nt2::Two<r_type>();
        wts /= Two<r_type>();
        s = zeros(n, meta::as_<cplx_type>());
        ctab_t p(nt2::of_size(n, n));
        for(uint32_t j=1;  j <= m; ++j)
        {
          p = trans(linsolve(trans(eye(n, meta::as_<r_type>())+nodes(j)*a0), trans(a0)));
          s +=  wts(j)*p;
        }
      }

      template < class X, class W > static inline
      void gauss_legendre( uint32_t n, X& x, W& w )
      {
        //gauss_legendre  nodes and weights for gauss-legendre quadrature.
        //  gauss_legendre(n, x, w) computes the nodes x and weights w
        //  for n-point gauss-legendre quadrature.
        //G. H. Golub and J. H. Welsch, calculation of gauss quadrature
        //rules, math. comp., 23(106):221-230, 1969.

        BOOST_AUTO_TPL(i,  nt2::_(nt2::One<r_type>(), r_type(n-1)));
        btab_t v = i/nt2::sqrt(nt2::minusone(nt2::sqr(nt2::Two<r_type>()*i)));
        btab_t vv;
        nt2::tie(x, vv) = nt2::symeig(from_diag(v,-1)+from_diag(v,1), nt2::vector_);
        w = nt2::Two<r_type>()*nt2::trans(nt2::sqr(vv(1,nt2::_)));
      }
    };

  }
}

#endif


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HANA_TEST_LAWS_RING_HPP
#define BOOST_HANA_TEST_LAWS_RING_HPP

#include <boost/hana/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/concept/comparable.hpp>
#include <boost/hana/concept/constant.hpp>
#include <boost/hana/concept/monoid.hpp>
#include <boost/hana/concept/ring.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/functional/capture.hpp>
#include <boost/hana/lazy.hpp>
#include <boost/hana/mult.hpp>
#include <boost/hana/not_equal.hpp>
#include <boost/hana/one.hpp>
#include <boost/hana/plus.hpp>
#include <boost/hana/power.hpp>
#include <boost/hana/value.hpp>

#include <laws/base.hpp>

namespace boost { namespace hana { namespace test {
    template <typename R, typename = when<true>>
    struct TestRing : TestRing<R, laws> {
        using TestRing<R, laws>::TestRing;
    };

    template <typename R>
    struct TestRing<R, laws> {
        template <typename Xs>
        TestRing(Xs xs) {
            hana::for_each(xs, hana::capture(xs)([](auto xs, auto x) {
                static_assert(Ring<decltype(x)>{}, "");

                foreach2(xs, hana::capture(x)([](auto x, auto y, auto z) {
                    // associativity
                    BOOST_HANA_CHECK(hana::equal(
                        hana::mult(x, hana::mult(y, z)),
                        hana::mult(hana::mult(x, y), z)
                    ));

                    // distributivity
                    BOOST_HANA_CHECK(hana::equal(
                        hana::mult(x, hana::plus(y, z)),
                        hana::plus(hana::mult(x, y), hana::mult(x, z))
                    ));
                }));

                // right identity
                BOOST_HANA_CHECK(hana::equal(
                    hana::mult(x, one<R>()), x
                ));

                // left identity
                BOOST_HANA_CHECK(hana::equal(
                    hana::mult(one<R>(), x), x
                ));

                // power
                BOOST_HANA_CHECK(hana::equal(
                    hana::power(x, int_c<0>),
                    one<R>()
                ));

                BOOST_HANA_CHECK(hana::equal(
                    hana::power(x, int_c<1>),
                    x
                ));

                BOOST_HANA_CHECK(hana::equal(
                    hana::power(x, int_c<2>),
                    hana::mult(x, x)
                ));

                BOOST_HANA_CHECK(hana::equal(
                    hana::power(x, int_c<3>),
                    hana::mult(hana::mult(x, x), x)
                ));

                BOOST_HANA_CHECK(hana::equal(
                    hana::power(x, int_c<4>),
                    hana::mult(hana::mult(hana::mult(x, x), x), x)
                ));

                BOOST_HANA_CHECK(hana::equal(
                    hana::power(x, int_c<5>),
                    hana::mult(hana::mult(hana::mult(hana::mult(x, x), x), x), x)
                ));

            }));
        }
    };

    template <typename C>
    struct TestRing<C, when<Constant<C>::value>>
        : TestRing<C, laws>
    {
        template <typename Xs>
        TestRing(Xs xs) : TestRing<C, laws>{xs} {
            BOOST_HANA_CHECK(hana::equal(
                hana::value(one<C>()),
                one<typename C::value_type>()
            ));

            foreach2(xs, [](auto x, auto y) {
                BOOST_HANA_CHECK(hana::equal(
                    hana::mult(hana::value(x), hana::value(y)),
                    hana::value(hana::mult(x, y))
                ));
            });
        }
    };
}}} // end namespace boost::hana::test

#endif // !BOOST_HANA_TEST_LAWS_RING_HPP


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/
#include "SiconosAlgebraProd.hpp"
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include "SiconosMatrix.hpp"
#include "SimpleMatrix.hpp"
#include "BlockVector.hpp"
#include "SiconosVector.hpp"
#include "SiconosException.hpp"

void prod(const SiconosMatrix& A, const SiconosVector& x, BlockVector& y, bool init)
{
  assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

  unsigned int startRow = 0;
  VectorOfVectors::const_iterator it;
  // For Each subvector of y, y[i], private_prod computes y[i] = subA x, subA being a submatrix of A corresponding to y[i] position.
  //       // private_prod takes into account the fact that x and y[i] may be block vectors.
  for(it = y.begin(); it != y.end(); ++it)
  {
    A.private_prod(startRow, x, **it, init);
    startRow += (*it)->size();
  }

}

void prod(const SiconosMatrix& A, const BlockVector& x, SiconosVector& y, bool init)
{

  assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

  if(init)
    y.zero();
  unsigned int startRow = 0;
  unsigned int startCol = 0;
  // In private_addprod, the sum of all blocks of x, x[i], is computed: y = Sum_i (subA x[i]), with subA a submatrix of A,
  // starting from position startRow in rows and startCol in columns.
  // private_prod takes also into account the fact that each block of x can also be a block.
  VectorOfVectors::const_iterator it;
  for(it = x.begin(); it != x.end(); ++it)
  {
    A.private_addprod(startRow, startCol, **it, y);
    startCol += (*it)->size();
  }
}

void prod(const SiconosMatrix& A, const SiconosVector& x, SiconosVector& y, bool init)
{
  // To compute y = A * x in an "optimized" way (in comparison with y = prod(A,x) )
  // or y += A*x if init = false.
  assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

  if(A.size(1) != x.size())
    THROW_EXCEPTION("inconsistent sizes between A and x.")

  if(A.size(0) != y.size())
    THROW_EXCEPTION("inconsistent sizes between A and y.");

  Siconos::UBLAS_TYPE numA = A.num();
  Siconos::UBLAS_TYPE numX = x.num();
  Siconos::UBLAS_TYPE numY = y.num();

  if(numA == Siconos::BLOCK)
    THROW_EXCEPTION("not yet implemented for block matrices.");

  if(numA == Siconos::ZERO)
  {
    if(init)
      y.zero();
    //else nothing
  }

  else if(numA == Siconos::IDENTITY)
  {
    if(!init)
      y += x;
    else
    {
      if(&x != &y) y = x ;  // if x and y do not share memory (ie are different objects)
      // else nothing
    }
  }

  else // A is not 0 or identity
  {

    // === First case: y is not a block vector ===
    if(init)
    {
      if(&x != &y)  // if no common memory between x and y.
      {
        if(numX == Siconos::DENSE)
        {
          if(numY != Siconos::DENSE)
            THROW_EXCEPTION("y (output) must be a dense vector.");

          assert(y.dense() != x.dense());

          if(numA == Siconos::DENSE)
            noalias(*y.dense()) = ublas::prod(*A.dense(), *x.dense());
          else if(numA == Siconos::TRIANGULAR)
            noalias(*y.dense()) = ublas::prod(*A.triang(), *x.dense());
          else if(numA == Siconos::SYMMETRIC)
            noalias(*y.dense()) = ublas::prod(*A.sym(), *x.dense());
          else if(numA == Siconos::SPARSE)
            noalias(*y.dense()) = ublas::prod(*A.sparse(), *x.dense());
          else //if(numA==Siconos::BANDED)
            noalias(*y.dense()) = ublas::prod(*A.banded(), *x.dense());
        }
        else //if(numX == Siconos::SPARSE)
        {
          if(numY != Siconos::DENSE && numA != Siconos::SPARSE)
            THROW_EXCEPTION(" y (output) must be a dense vector or A a sparse matrix.");

          if(numA == Siconos::DENSE)
            noalias(*y.dense()) = ublas::prod(*A.dense(), *x.sparse());
          else if(numA == Siconos::TRIANGULAR)
            noalias(*y.dense()) = ublas::prod(*A.triang(), *x.sparse());
          else if(numA == Siconos::SYMMETRIC)
            noalias(*y.dense()) = ublas::prod(*A.sym(), *x.sparse());
          else if(numA == Siconos::SPARSE)
          {
            if(numY == Siconos::DENSE)
              noalias(*y.dense()) = ublas::prod(*A.sparse(), *x.sparse());
            else
              noalias(*y.sparse()) = ublas::prod(*A.sparse(), *x.sparse());
          }
          else //if(numA==Siconos::BANDED)
            noalias(*y.dense()) = ublas::prod(*A.banded(), *x.sparse());
        }
      }
      else // if x and y are the same object => alias
      {
        if(numX == Siconos::DENSE)
        {
          if(numA == Siconos::DENSE)
            *y.dense() = ublas::prod(*A.dense(), *x.dense());
          else if(numA == Siconos::TRIANGULAR)
            *y.dense() = ublas::prod(*A.triang(), *x.dense());
          else if(numA == Siconos::SYMMETRIC)
            *y.dense() = ublas::prod(*A.sym(), *x.dense());
          else if(numA == Siconos::SPARSE)
            *y.dense() = ublas::prod(*A.sparse(), *x.dense());
          else //if(numA==Siconos::BANDED)
            *y.dense() = ublas::prod(*A.banded(), *x.dense());
        }
        else //if(numX == Siconos::SPARSE)
        {
          if(numA == Siconos::DENSE)
            *y.sparse() = ublas::prod(*A.dense(), *x.sparse());
          else if(numA == Siconos::TRIANGULAR)
            *y.sparse() = ublas::prod(*A.triang(), *x.sparse());
          else if(numA == Siconos::SYMMETRIC)
            *y.sparse() = ublas::prod(*A.sym(), *x.sparse());
          else if(numA == Siconos::SPARSE)
            *y.sparse() = ublas::prod(*A.sparse(), *x.sparse());
          else //if(numA==Siconos::BANDED)
            *y.sparse() = ublas::prod(*A.banded(), *x.sparse());
        }
      }
    }
    else // += case
    {
      if(&x != &y)  // if no common memory between x and y.
      {
        if(numX == Siconos::DENSE)
        {
          if(numY != Siconos::DENSE)
            THROW_EXCEPTION("y (output) must be a dense vector.");

          if(numA == Siconos::DENSE)
            noalias(*y.dense()) += ublas::prod(*A.dense(), *x.dense());
          else if(numA == Siconos::TRIANGULAR)
            noalias(*y.dense()) += ublas::prod(*A.triang(), *x.dense());
          else if(numA == Siconos::SYMMETRIC)
            noalias(*y.dense()) += ublas::prod(*A.sym(), *x.dense());
          else if(numA == Siconos::SPARSE)
            noalias(*y.dense()) += ublas::prod(*A.sparse(), *x.dense());
          else //if(numA==Siconos::BANDED)
            noalias(*y.dense()) += ublas::prod(*A.banded(), *x.dense());
        }
        else //if(numX == Siconos::SPARSE)
        {
          if(numY != Siconos::DENSE && numA != Siconos::SPARSE)
            THROW_EXCEPTION("y (output) must be a dense vector or A a sparse matrix.");

          if(numA == Siconos::DENSE)
            noalias(*y.dense()) += ublas::prod(*A.dense(), *x.sparse());
          else if(numA == Siconos::TRIANGULAR)
            noalias(*y.dense()) += ublas::prod(*A.triang(), *x.sparse());
          else if(numA == Siconos::SYMMETRIC)
            noalias(*y.dense()) += ublas::prod(*A.sym(), *x.sparse());
          else if(numA == Siconos::SPARSE)
          {
            if(numY == Siconos::DENSE)
              noalias(*y.dense()) += ublas::prod(*A.sparse(), *x.sparse());
            else
              noalias(*y.sparse()) += ublas::prod(*A.sparse(), *x.sparse());
          }
          else //if(numA==Siconos::BANDED)
            noalias(*y.dense()) += ublas::prod(*A.banded(), *x.sparse());
        }
      }
      else // if x and y are the same object => alias
      {
        if(numX == Siconos::DENSE)
        {
          if(numA == Siconos::DENSE)
            *y.dense() += ublas::prod(*A.dense(), *x.dense());
          else if(numA == Siconos::TRIANGULAR)
            *y.dense() += ublas::prod(*A.triang(), *x.dense());
          else if(numA == Siconos::SYMMETRIC)
            *y.dense() += ublas::prod(*A.sym(), *x.dense());
          else if(numA == Siconos::SPARSE)
            *y.dense() += ublas::prod(*A.sparse(), *x.dense());
          else //if(numA==Siconos::BANDED)
            *y.dense() += ublas::prod(*A.banded(), *x.dense());
        }
        else //if(numX == Siconos::SPARSE)
        {
          if(numA == Siconos::DENSE)
            *y.sparse() += ublas::prod(*A.dense(), *x.sparse());
          else if(numA == Siconos::TRIANGULAR)
            *y.sparse() += ublas::prod(*A.triang(), *x.sparse());
          else if(numA == Siconos::SYMMETRIC)
            *y.sparse() += ublas::prod(*A.sym(), *x.sparse());
          else if(numA == Siconos::SPARSE)
            *y.sparse() += ublas::prod(*A.sparse(), *x.sparse());
          else //if(numA==Siconos::BANDED)
            *y.sparse() += ublas::prod(*A.banded(), *x.sparse());
        }
      }
    }
  }
}

void prod(const SiconosVector& x, const SiconosMatrix& A, SiconosVector& y, bool init)
{
  // To compute y = trans(A) * x in an "optimized" way, if init = true
  // (or y = trans(A) * x + y if init = false
  assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

  if(A.size(0) != x.size())
    THROW_EXCEPTION("inconsistent sizes between A and x.");

  if(A.size(1) != y.size())
    THROW_EXCEPTION("inconsistent sizes between A and y.");

  Siconos::UBLAS_TYPE numA = A.num();
  Siconos::UBLAS_TYPE numX = x.num();
  Siconos::UBLAS_TYPE numY = y.num();

  if(numA == Siconos::BLOCK)
    THROW_EXCEPTION("not yet implemented for block matrices.");

  if(numA == Siconos::ZERO)  // A = 0
  {
    if(init)
      y.zero();
    // else nothing
  }

  else if(numA == Siconos::IDENTITY)  // A = identity
  {
    if(!init)
      y += x;
    else
    {
      if(&x != &y) y = x ;  // if x and y do not share memory (ie are different objects)
      // else nothing
    }
  }

  else // A is not 0 or identity
  {
    {
      if(init)
      {

        if(&x != &y)  // if no common memory between x and y.
        {
          if(numX == Siconos::DENSE)
          {
            if(numY != Siconos::DENSE)
              THROW_EXCEPTION("y (output) must be a dense vector.");

            if(numA == Siconos::DENSE)
              noalias(*y.dense()) = ublas::prod(trans(*A.dense()), *x.dense());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*y.dense()) = ublas::prod(trans(*A.triang()), *x.dense());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*y.dense()) = ublas::prod(trans(*A.sym()), *x.dense());
            else if(numA == Siconos::SPARSE)
              noalias(*y.dense()) = ublas::prod(trans(*A.sparse()), *x.dense());
            else //if(numA==Siconos::BANDED)
              noalias(*y.dense()) = ublas::prod(trans(*A.banded()), *x.dense());
          }
          else //if(numX == Siconos::SPARSE)
          {
            if(numY != Siconos::DENSE && numA != Siconos::SPARSE)
              THROW_EXCEPTION("y (output) must be a dense vector or A a sparse matrix.");
            if(numA == Siconos::DENSE)
              noalias(*y.dense()) = ublas::prod(trans(*A.dense()), *x.sparse());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*y.dense()) = ublas::prod(trans(*A.triang()), *x.sparse());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*y.dense()) = ublas::prod(trans(*A.sym()), *x.sparse());
            else if(numA == Siconos::SPARSE)
            {
              if(numY == Siconos::DENSE)
                noalias(*y.dense()) = ublas::prod(trans(*A.sparse()), *x.sparse());
              else
                noalias(*y.sparse()) = ublas::prod(trans(*A.sparse()), *x.sparse());
            }
            else //if(numA==Siconos::BANDED)
              noalias(*y.dense()) = ublas::prod(trans(*A.banded()), *x.sparse());
          }
        }
        else // if x and y are the same object => alias
        {
          if(numX == Siconos::DENSE)
          {
            if(numA == Siconos::DENSE)
              *y.dense() = ublas::prod(trans(*A.dense()), *x.dense());
            else if(numA == Siconos::TRIANGULAR)
              *y.dense() = ublas::prod(trans(*A.triang()), *x.dense());
            else if(numA == Siconos::SYMMETRIC)
              *y.dense() = ublas::prod(trans(*A.sym()), *x.dense());
            else if(numA == Siconos::SPARSE)
              *y.dense() = ublas::prod(trans(*A.sparse()), *x.dense());
            else //if(numA==Siconos::BANDED)
              *y.dense() = ublas::prod(trans(*A.banded()), *x.dense());
          }
          else //if(numX == Siconos::SPARSE)
          {
            if(numA == Siconos::DENSE)
              *y.sparse() = ublas::prod(trans(*A.dense()), *x.sparse());
            else if(numA == Siconos::TRIANGULAR)
              *y.sparse() = ublas::prod(trans(*A.triang()), *x.sparse());
            else if(numA == Siconos::SYMMETRIC)
              *y.sparse() = ublas::prod(trans(*A.sym()), *x.sparse());
            else if(numA == Siconos::SPARSE)
              *y.sparse() = ublas::prod(trans(*A.sparse()), *x.sparse());
            else //if(numA==Siconos::BANDED)
              *y.sparse() = ublas::prod(trans(*A.banded()), *x.sparse());
          }
        }
      }
      else // += case
      {

        if(&x != &y)  // if no common memory between x and y.
        {
          if(numX == Siconos::DENSE)
          {
            if(numY != Siconos::DENSE)
              THROW_EXCEPTION("y (output) must be a dense vector.");

            if(numA == Siconos::DENSE)
              noalias(*y.dense()) += ublas::prod(trans(*A.dense()), *x.dense());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*y.dense()) += ublas::prod(trans(*A.triang()), *x.dense());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*y.dense()) += ublas::prod(trans(*A.sym()), *x.dense());
            else if(numA == Siconos::SPARSE)
              noalias(*y.dense()) += ublas::prod(trans(*A.sparse()), *x.dense());
            else //if(numA==Siconos::BANDED)
              noalias(*y.dense()) += ublas::prod(trans(*A.banded()), *x.dense());
          }
          else //if(numX == Siconos::SPARSE)
          {
            if(numY != Siconos::DENSE && numA != Siconos::SPARSE)
              THROW_EXCEPTION("y (output) must be a dense vector or A a sparse matrix.");

            if(numA == Siconos::DENSE)
              noalias(*y.dense()) += ublas::prod(trans(*A.dense()), *x.sparse());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*y.dense()) += ublas::prod(trans(*A.triang()), *x.sparse());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*y.dense()) += ublas::prod(trans(*A.sym()), *x.sparse());
            else if(numA == Siconos::SPARSE)
            {
              if(numY == Siconos::DENSE)
                noalias(*y.dense()) += ublas::prod(trans(*A.sparse()), *x.sparse());
              else
                noalias(*y.sparse()) += ublas::prod(trans(*A.sparse()), *x.sparse());
            }
            else //if(numA==Siconos::BANDED)
              noalias(*y.dense()) += ublas::prod(trans(*A.banded()), *x.sparse());
          }
        }
        else // if x and y are the same object => alias
        {
          if(numX == Siconos::DENSE)
          {
            if(numA == Siconos::DENSE)
              *y.dense() += ublas::prod(trans(*A.dense()), *x.dense());
            else if(numA == Siconos::TRIANGULAR)
              *y.dense() += ublas::prod(trans(*A.triang()), *x.dense());
            else if(numA == Siconos::SYMMETRIC)
              *y.dense() += ublas::prod(trans(*A.sym()), *x.dense());
            else if(numA == Siconos::SPARSE)
              *y.dense() += ublas::prod(trans(*A.sparse()), *x.dense());
            else //if(numA==Siconos::BANDED)
              *y.dense() += ublas::prod(trans(*A.banded()), *x.dense());
          }
          else //if(numX == Siconos::SPARSE)
          {
            if(numA == Siconos::DENSE)
              *y.sparse() += ublas::prod(trans(*A.dense()), *x.sparse());
            else if(numA == Siconos::TRIANGULAR)
              *y.sparse() += ublas::prod(trans(*A.triang()), *x.sparse());
            else if(numA == Siconos::SYMMETRIC)
              *y.sparse() += ublas::prod(trans(*A.sym()), *x.sparse());
            else if(numA == Siconos::SPARSE)
              *y.sparse() += ublas::prod(trans(*A.sparse()), *x.sparse());
            else //if(numA==Siconos::BANDED)
              *y.sparse() += ublas::prod(trans(*A.banded()), *x.sparse());
          }
        }
      }
    }
  }
}

void prod(const SiconosVector& x, const SiconosMatrix& A, BlockVector& y, bool init)
{
  assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

  if(A.size(0) != x.size())
    THROW_EXCEPTION("inconsistent sizes between A and x.");

  if(A.size(1) != y.size())
    THROW_EXCEPTION("inconsistent sizes between A and y.");

  unsigned int pos = 0;
  VectorOfVectors::const_iterator it;
  // For Each subvector of y, y[i], computes y[i] = transpose(subA) x, subA being a submatrix of A corresponding to y[i] position.
  // private_prod takes into account the fact that x and y[i] may be block vectors.
  for(it = y.begin(); it != y.end(); ++it)
  {
    taxpy(createSPtrConstSiconosVector(x), createSPtrConstSiconosMatrix(A), pos, 0, *it, init);
    pos += (*it)->size();
  }
}

// ========== Products matrix - vector

SiconosVector prod(const SiconosMatrix& A, const SiconosVector& x)
{
  // To compute y = A * x
  assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

  if(A.size(1) != x.size())
    THROW_EXCEPTION("inconsistent sizes between A and x.");

  Siconos::UBLAS_TYPE numA = A.num();
  Siconos::UBLAS_TYPE numX = x.num();

  if(numA == Siconos::BLOCK)  // if A is block ...
    THROW_EXCEPTION("Not implemented for block matrices.");

  if(numA == Siconos::ZERO)  // A = 0
    return (DenseVect)(ublas::zero_vector<double>(x.size()));

  else if(numA == Siconos::IDENTITY)  // A = Identity
    return x;

  else
  {
    if(numX == Siconos::DENSE)
    {
      if(numA == Siconos::DENSE)
        return (DenseVect)(prod(*A.dense(), *x.dense()));
      else if(numA == Siconos::TRIANGULAR)
        return (DenseVect)(prod(*A.triang(), *x.dense()));
      else if(numA == Siconos::SYMMETRIC)
        return (DenseVect)(prod(*A.sym(), *x.dense()));
      else if(numA == Siconos::SPARSE)
        return (DenseVect)(prod(*A.sparse(), *x.dense()));
      else // if(numA==Siconos::BANDED)
        return (DenseVect)(prod(*A.banded(), *x.dense()));
    }
    else //if(numX == Siconos::SPARSE)
    {
      if(numA == Siconos::DENSE)
        return (DenseVect)(prod(*A.dense(), *x.sparse()));
      else if(numA == Siconos::TRIANGULAR)
        return (DenseVect)(prod(*A.triang(), *x.sparse()));
      else if(numA == Siconos::SYMMETRIC)
        return (DenseVect)(prod(*A.sym(), *x.sparse()));
      else if(numA == Siconos::SPARSE)
        return (DenseVect)(prod(*A.sparse(), *x.sparse()));
      else // if(numA==Siconos::BANDED)
        return (DenseVect)(prod(*A.banded(), *x.sparse()));
    }
  }
}

const SimpleMatrix  prod(const SiconosMatrix& A, const SiconosMatrix& B)
{
  Siconos::UBLAS_TYPE numA = A.num();
  Siconos::UBLAS_TYPE numB = B.num();

  if (numA == numB)
  {
    SimpleMatrix  C(A.size(0),B.size(1), numA);
    prod(A, B, C);
    return C;
  }
  else
  {
    SimpleMatrix  C(A.size(0),B.size(1));
    prod(A, B, C);
    return C;
  }
}
void prod(const SiconosMatrix& A, const SiconosMatrix& B, SiconosMatrix& C, bool init)
{
  // To compute C = A * B
  assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");
  assert(!(B.isPLUFactorizedInPlace()) && "B is PLUFactorizedInPlace in prod !!");
  if(!C.isBlock())
    C.resetFactorizationFlags();

  if((A.size(1) != B.size(0)))
    THROW_EXCEPTION("inconsistent sizes between A and B");

  if(A.size(0) != C.size(0) || B.size(1) != C.size(1))
    THROW_EXCEPTION("inconsistent sizes between A and C or B and C.");

  Siconos::UBLAS_TYPE numA = A.num();
  Siconos::UBLAS_TYPE numB = B.num();
  Siconos::UBLAS_TYPE numC = C.num();

  // == TODO: implement block product ==
  if(numA == Siconos::BLOCK || numB == Siconos::BLOCK)
    THROW_EXCEPTION("not yet implemented for block matrices");

  // === if C is zero or identity => read-only ===
  if(numC == Siconos::ZERO || numC == Siconos::IDENTITY)
    THROW_EXCEPTION("wrong type for resulting matrix C (read-only: zero or identity).");

  if(numA == Siconos::IDENTITY)  // A = identity ...
  {
    if(init)
    {
      if(&C != &B) C = B;  // if C and B are two different objects.
      // else nothing
    }
    else
      C += B;
  }

  else if(numB == Siconos::IDENTITY)  // B = identity
  {
    if(init)
    {
      if(&C != &A) C = A;  // if C and A are two different objects.
      // else nothing
    }
    else
      C += A;
  }

  else if(numA == Siconos::ZERO || numB == Siconos::ZERO)  // if A or B = 0
  {
    if(init)
      C.zero();
    //else nothing
  }
  else if(numC == Siconos::BLOCK)  // if C is Block - Temp. solution
  {
    SimpleMatrix tmp(C);
    prod(A, B, tmp, init);
    C = tmp;
  }
  else // neither A or B is equal to identity or zero.
  {
    if(init)
    {
      if(&C == &A)  // if common memory between A and C
      {
        switch(numA)
        {
        case Siconos::DENSE:
          if(numB == Siconos::DENSE)
          {
            *C.dense()  = prod(*A.dense(), *B.dense());
            //siconosBindings::blas::gemm(1.0, *A.dense(), *B.dense(), 0.0, *C.dense());
          }
          else if(numB == Siconos::TRIANGULAR)
            *C.dense()  = prod(*A.dense(), *B.triang());
          else if(numB == Siconos::SYMMETRIC)
            *C.dense()  = prod(*A.dense(), *B.sym());
          else if(numB == Siconos::SPARSE)
            *C.dense()  = prod(*A.dense(), *B.sparse());
          else //if(numB==Siconos::BANDED)
            *C.dense() = prod(*A.dense(), *B.banded());
          break;
        case Siconos::TRIANGULAR:
          if(numB != Siconos::TRIANGULAR)
            THROW_EXCEPTION("wrong type for B (according to A type).");
          *C.triang() = prod(*A.triang(), *B.triang());
          break;
        case Siconos::SYMMETRIC:
          if(numB != Siconos::SYMMETRIC)
            THROW_EXCEPTION("wrong type for B (according to A type).");
          *C.sym() = prod(*A.sym(), *B.sym());
          break;
        case Siconos::SPARSE:
          if(numB != Siconos::SPARSE)
            THROW_EXCEPTION("wrong type for B (according to A type).");
          *C.sparse() = prod(*A.sparse(), *B.sparse());
          break;
        default:
          THROW_EXCEPTION("not implemented for A type.");
        }
      }
      else if(&C == &B)
      {
        switch(numB)
        {
        case Siconos::DENSE:
          if(numA == Siconos::DENSE)
            *C.dense() = prod(*A.dense(), *B.dense());
          else if(numA == Siconos::TRIANGULAR)
            *C.dense()  = prod(*A.triang(), *B.dense());
          else if(numA == Siconos::SYMMETRIC)
            *C.dense()  = prod(*A.sym(), *B.dense());
          else if(numA == Siconos::SPARSE)
            *C.dense()  = prod(*A.sparse(), *B.dense());
          else //if(numB==Siconos::BANDED)
            *C.dense() = prod(*A.banded(), *B.dense());
          break;
        case Siconos::TRIANGULAR:
          if(numA != Siconos::TRIANGULAR)
            THROW_EXCEPTION("wrong type for A (according to B type).");
          *C.triang() = prod(*A.triang(), *B.triang());
          break;
        case Siconos::SYMMETRIC:
          if(numA != Siconos::SYMMETRIC)
            THROW_EXCEPTION("wrong type for A (according to B type).");
          *C.sym() = prod(*A.sym(), *B.sym());
          break;
        case Siconos::SPARSE:
          if(numA != Siconos::SPARSE)
            THROW_EXCEPTION("wrong type for A (according to B type).");
          *C.sparse() = prod(*A.sparse(), *B.sparse());
          break;
        default:
          THROW_EXCEPTION("not implemented for B type.");
        }
      }
      else // if no alias between C and A or B.
      {
        switch(numC)
        {
        case Siconos::DENSE:
          if(numB == Siconos::DENSE)
          {
            if(numA == Siconos::DENSE)
              noalias(*C.dense()) = prod(*A.dense(), *B.dense());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*C.dense()) = prod(*A.triang(), *B.dense());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*C.dense())  = prod(*A.sym(), *B.dense());
            else if(numA == Siconos::SPARSE)
              noalias(*C.dense()) = prod(*A.sparse(), *B.dense());
            else// if(numA==Siconos::BANDED)
              noalias(*C.dense())  = prod(*A.banded(), *B.dense());
          }
          else if(numB == Siconos::TRIANGULAR)
          {
            if(numA == Siconos::DENSE)
              noalias(*C.dense())  = prod(*A.dense(), *B.triang());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*C.dense())  = prod(*A.triang(), *B.triang());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*C.dense())  = prod(*A.sym(), *B.triang());
            else if(numA == Siconos::SPARSE)
              noalias(*C.dense())  = prod(*A.sparse(), *B.triang());
            else //if(numA==Siconos::BANDED)
              noalias(*C.dense())  = prod(*A.banded(), *B.triang());
          }
          else if(numB == Siconos::SYMMETRIC)
          {
            if(numA == Siconos::DENSE)
              noalias(*C.dense())  = prod(*A.dense(), *B.sym());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*C.dense())  = prod(*A.triang(), *B.sym());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*C.dense())  = prod(*A.sym(), *B.sym());
            else if(numA == Siconos::SPARSE)
              noalias(*C.dense())  = prod(*A.sparse(), *B.sym());
            else // if (numA == Siconos::BANDED)
              noalias(*C.dense())  = prod(*A.banded(), *B.sym());
          }
          else if(numB == Siconos::SPARSE)
          {
            if(numA == Siconos::DENSE)
              noalias(*C.dense()) = prod(*A.dense(), *B.sparse());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*C.dense()) = prod(*A.triang(), *B.sparse());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*C.dense()) = prod(*A.sym(), *B.sparse());
            else if(numA == Siconos::SPARSE)
              noalias(*C.dense()) = prod(*A.sparse(), *B.sparse());
            else //if(numA==Siconos::BANDED){
              noalias(*C.dense()) = prod(*A.banded(), *B.sparse());
          }
          else //if(numB==Siconos::BANDED)
          {
            if(numA == Siconos::DENSE)
              noalias(*C.dense()) = prod(*A.dense(), *B.banded());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*C.dense()) = prod(*A.triang(), *B.banded());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*C.dense()) = prod(*A.sym(), *B.banded());
            else if(numA == Siconos::SPARSE)
              noalias(*C.dense()) = prod(*A.sparse(), *B.banded());
            else //if(numA==Siconos::BANDED)
              noalias(*C.dense()) = prod(*A.banded(), *B.banded());
          }
          break;
        case Siconos::TRIANGULAR:
          if(numA != Siconos::TRIANGULAR || numB != Siconos::TRIANGULAR)
            THROW_EXCEPTION("wrong type for A or B (according to C type).");
          noalias(*C.triang()) = prod(*A.triang(), *B.triang());
          break;
        case Siconos::SYMMETRIC:
          if(numA != Siconos::SYMMETRIC || numB != Siconos::SYMMETRIC)
            THROW_EXCEPTION("wrong type for A or B (according to C type).");
          noalias(*C.sym()) = prod(*A.sym(), *B.sym());
          break;
        case Siconos::SPARSE:
          if(numA != Siconos::SPARSE || numB != Siconos::SPARSE)
            THROW_EXCEPTION("wrong type for A or B (according to C type).");
          noalias(*C.sparse()) = prod(*A.sparse(), *B.sparse());
          break;
        default:
          THROW_EXCEPTION("not implemented for C type.");
        }
      }
    }
    else // += case
    {
      if(&C == &A)  // if common memory between A and C
      {
        switch(numA)
        {
        case Siconos::DENSE:
          if(numB == Siconos::DENSE)
            *C.dense() += prod(*A.dense(), *B.dense());
          else if(numB == Siconos::TRIANGULAR)
            *C.dense()  += prod(*A.dense(), *B.triang());
          else if(numB == Siconos::SYMMETRIC)
            *C.dense()  += prod(*A.dense(), *B.sym());
          else if(numB == Siconos::SPARSE)
            *C.dense()  += prod(*A.dense(), *B.sparse());
          else //if(numB==Siconos::BANDED)
            *C.dense() += prod(*A.dense(), *B.banded());
          break;
        case Siconos::TRIANGULAR:
          if(numB != Siconos::TRIANGULAR)
            THROW_EXCEPTION("wrong type for B (according to A type).");
          *C.triang() += prod(*A.triang(), *B.triang());
          break;
        case Siconos::SYMMETRIC:
          if(numB != Siconos::SYMMETRIC)
            THROW_EXCEPTION("wrong type for B (according to A type).");
          *C.sym() += prod(*A.sym(), *B.sym());
          break;
        case Siconos::SPARSE:
          if(numB != Siconos::SPARSE)
            THROW_EXCEPTION("wrong type for B (according to A type).");
          *C.sparse() += prod(*A.sparse(), *B.sparse());
          break;
        default:
          THROW_EXCEPTION("not implemented for A type.");
        }
      }
      else if(&C == &B)
      {
        switch(numB)
        {
        case Siconos::DENSE:
          if(numA == Siconos::DENSE)
            *C.dense() += prod(*A.dense(), *B.dense());
          else if(numA == Siconos::TRIANGULAR)
            *C.dense()  += prod(*A.triang(), *B.dense());
          else if(numA == Siconos::SYMMETRIC)
            *C.dense()  += prod(*A.sym(), *B.dense());
          else if(numA == Siconos::SPARSE)
            *C.dense()  += prod(*A.sparse(), *B.dense());
          else //if(numB==Siconos::BANDED)
            *C.dense() += prod(*A.banded(), *B.dense());
          break;
        case Siconos::TRIANGULAR:
          if(numA != Siconos::TRIANGULAR)
            THROW_EXCEPTION("wrong type for A (according to B type).");
          *C.triang() += prod(*A.triang(), *B.triang());
          break;
        case Siconos::SYMMETRIC:
          if(numA != Siconos::SYMMETRIC)
            THROW_EXCEPTION("wrong type for A (according to B type).");
          *C.sym() += prod(*A.sym(), *B.sym());
          break;
        case Siconos::SPARSE:
          if(numA != Siconos::SPARSE)
            THROW_EXCEPTION("wrong type for A (according to B type).");
          *C.sparse() += prod(*A.sparse(), *B.sparse());
          break;
        default:
            THROW_EXCEPTION("not yet implemented for A type.");
        }
      }
      else // if no alias between C and A or B.
      {
        switch(numC)
        {
        case Siconos::DENSE:
          if(numB == Siconos::DENSE)
          {
            if(numA == Siconos::DENSE)
              noalias(*C.dense()) += prod(*A.dense(), *B.dense());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*C.dense()) += prod(*A.triang(), *B.dense());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*C.dense())  += prod(*A.sym(), *B.dense());
            else if(numA == Siconos::SPARSE)
              noalias(*C.dense()) += prod(*A.sparse(), *B.dense());
            else// if(numA==Siconos::BANDED)
              noalias(*C.dense())  += prod(*A.banded(), *B.dense());
          }
          else if(numB == Siconos::TRIANGULAR)
          {
            if(numA == Siconos::DENSE)
              noalias(*C.dense())  += prod(*A.dense(), *B.triang());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*C.dense())  += prod(*A.triang(), *B.triang());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*C.dense())  += prod(*A.sym(), *B.triang());
            else if(numA == Siconos::SPARSE)
              noalias(*C.dense())  += prod(*A.sparse(), *B.triang());
            else //if(numA==Siconos::BANDED)
              noalias(*C.dense())  += prod(*A.banded(), *B.triang());
          }
          else if(numB == Siconos::SYMMETRIC)
          {
            if(numA == Siconos::DENSE)
              noalias(*C.dense())  += prod(*A.dense(), *B.sym());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*C.dense())  += prod(*A.triang(), *B.sym());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*C.dense())  += prod(*A.sym(), *B.sym());
            else if(numA == Siconos::SPARSE)
              noalias(*C.dense())  += prod(*A.sparse(), *B.sym());
            else // if (numA == BANDED)
              noalias(*C.dense())  += prod(*A.banded(), *B.sym());
          }
          else if(numB == Siconos::SPARSE)
          {
            if(numA == Siconos::DENSE)
              noalias(*C.dense()) += prod(*A.dense(), *B.sparse());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*C.dense()) += prod(*A.triang(), *B.sparse());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*C.dense()) += prod(*A.sym(), *B.sparse());
            else if(numA == Siconos::SPARSE)
              noalias(*C.dense()) += prod(*A.sparse(), *B.sparse());
            else //if(numA==Siconos::BANDED){
              noalias(*C.dense()) += prod(*A.banded(), *B.sparse());
          }
          else //if(numB==Siconos::BANDED)
          {
            if(numA == Siconos::DENSE)
              noalias(*C.dense()) += prod(*A.dense(), *B.banded());
            else if(numA == Siconos::TRIANGULAR)
              noalias(*C.dense()) += prod(*A.triang(), *B.banded());
            else if(numA == Siconos::SYMMETRIC)
              noalias(*C.dense()) += prod(*A.sym(), *B.banded());
            else if(numA == Siconos::SPARSE)
              noalias(*C.dense()) += prod(*A.sparse(), *B.banded());
            else //if(numA==Siconos::BANDED)
              noalias(*C.dense()) += prod(*A.banded(), *B.banded());
          }
          break;
        case Siconos::TRIANGULAR:
          if(numA != Siconos::TRIANGULAR || numB != Siconos::TRIANGULAR)
            THROW_EXCEPTION("wrong type for A or B (according to C type).");
          noalias(*C.triang()) += prod(*A.triang(), *B.triang());
          break;
        case Siconos::SYMMETRIC:
          if(numA != Siconos::SYMMETRIC || numB != Siconos::SYMMETRIC)
            THROW_EXCEPTION("wrong type for A or B (according to C type).");
          noalias(*C.sym()) += prod(*A.sym(), *B.sym());
          break;
        case Siconos::SPARSE:
          if(numA != Siconos::SPARSE || numB != Siconos::SPARSE)
            THROW_EXCEPTION("wrong type for A or B (according to C type).");
          noalias(*C.sparse()) += prod(*A.sparse(), *B.sparse());
          break;
        default:
            THROW_EXCEPTION("not implemented for C type).");
        }
      }
    }
    if(!C.isBlock())
      C.resetFactorizationFlags();
  }
}

void prod(double a, const SiconosMatrix& A, const SiconosVector& x, SiconosVector& y, bool init)
{
  // To compute y = a*A * x in an "optimized" way (in comparison with y = prod(A,x) )
  // or y += a*A*x if init = false.
  assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

  if(A.size(1) != x.size())
    THROW_EXCEPTION("inconsistent sizes between A and x.");

  if(A.size(0) != y.size())
    THROW_EXCEPTION("inconsistent sizes between A and y.");

  Siconos::UBLAS_TYPE numA = A.num();
  Siconos::UBLAS_TYPE numX = x.num();
  Siconos::UBLAS_TYPE numY = y.num();

  if(numA == Siconos::BLOCK)  // If A is Block
    THROW_EXCEPTION("not yet implemented for block matrices.");

  if(numA == Siconos::ZERO)  // A = 0
  {
    if(init)
      y.zero();
    //else nothing
  }

  else if(numA == Siconos::IDENTITY)  // A = identity
  {
    scal(a, x, y, init);
  }

  else // A is not 0 or identity
  {

    // === First case: y is not a block vector ===
    {
      {
        if(init)
        {
          if(&x != &y)  // if no common memory between x and y.
          {
            if(numX == Siconos::DENSE)
            {
              if(numY != Siconos::DENSE)
                THROW_EXCEPTION("y (output) must be a dense vector.");

              if(numA == Siconos::DENSE)
                noalias(*y.dense()) = a * ublas::prod(*A.dense(), *x.dense());
              else if(numA == Siconos::TRIANGULAR)
                noalias(*y.dense()) = a * ublas::prod(*A.triang(), *x.dense());
              else if(numA == Siconos::SYMMETRIC)
                noalias(*y.dense()) = a * ublas::prod(*A.sym(), *x.dense());
              else if(numA == Siconos::SPARSE)
                noalias(*y.dense()) = a * ublas::prod(*A.sparse(), *x.dense());
              else //if(numA==Siconos::BANDED)
                noalias(*y.dense()) = a * ublas::prod(*A.banded(), *x.dense());
            }
            else //if(numX == Siconos::SPARSE)
            {
              if(numY != Siconos::DENSE && numA != Siconos::SPARSE)
               THROW_EXCEPTION("y (output) must be a dense vector or A a sparse matrix.");

              if(numA == Siconos::DENSE)
                noalias(*y.dense()) = a * ublas::prod(*A.dense(), *x.sparse());
              else if(numA == Siconos::TRIANGULAR)
                noalias(*y.dense()) = a * ublas::prod(*A.triang(), *x.sparse());
              else if(numA == Siconos::SYMMETRIC)
                noalias(*y.dense()) = a * ublas::prod(*A.sym(), *x.sparse());
              else if(numA == Siconos::SPARSE)
              {
                if(numY == Siconos::DENSE)
                  noalias(*y.dense()) = a * ublas::prod(*A.sparse(), *x.sparse());
                else
                  noalias(*y.sparse()) = a * ublas::prod(*A.sparse(), *x.sparse());
              }
              else //if(numA==Siconos::BANDED)
                noalias(*y.dense()) = a * ublas::prod(*A.banded(), *x.sparse());
            }
          }
          else // if x and y are the same object => alias
          {
            if(numX == Siconos::DENSE)
            {
              if(numA == Siconos::DENSE)
                *y.dense() = a * ublas::prod(*A.dense(), *x.dense());
              else if(numA == Siconos::TRIANGULAR)
                *y.dense() = a * ublas::prod(*A.triang(), *x.dense());
              else if(numA == Siconos::SYMMETRIC)
                *y.dense() = a * ublas::prod(*A.sym(), *x.dense());
              else if(numA == Siconos::SPARSE)
                *y.dense() = a * ublas::prod(*A.sparse(), *x.dense());
              else //if(numA==Siconos::BANDED)
                *y.dense() = a * ublas::prod(*A.banded(), *x.dense());
            }
            else //if(numX == Siconos::SPARSE)
            {
              if(numA == Siconos::DENSE)
                *y.sparse() = a * ublas::prod(*A.dense(), *x.sparse());
              else if(numA == Siconos::TRIANGULAR)
                *y.sparse() = a * ublas::prod(*A.triang(), *x.sparse());
              else if(numA == Siconos::SYMMETRIC)
                *y.sparse() = a * ublas::prod(*A.sym(), *x.sparse());
              else if(numA == Siconos::SPARSE)
                *y.sparse() = a * ublas::prod(*A.sparse(), *x.sparse());
              else //if(numA==Siconos::BANDED)
                *y.sparse() = a * ublas::prod(*A.banded(), *x.sparse());
            }
          }
        }
        else // += case
        {
          if(&x != &y)  // if no common memory between x and y.
          {
            if(numX == Siconos::DENSE)
            {
              if(numY != Siconos::DENSE)
               THROW_EXCEPTION("y (output) must be a dense vector.");

              if(numA == Siconos::DENSE)
                noalias(*y.dense()) += a * ublas::prod(*A.dense(), *x.dense());
              else if(numA == Siconos::TRIANGULAR)
                noalias(*y.dense()) += a * ublas::prod(*A.triang(), *x.dense());
              else if(numA == Siconos::SYMMETRIC)
                noalias(*y.dense()) += a * ublas::prod(*A.sym(), *x.dense());
              else if(numA == Siconos::SPARSE)
                noalias(*y.dense()) += a * ublas::prod(*A.sparse(), *x.dense());
              else //if(numA==Siconos::BANDED)
                noalias(*y.dense()) += a * ublas::prod(*A.banded(), *x.dense());
            }
            else //if(numX == Siconos::SPARSE)
            {
              if(numY != Siconos::DENSE && numA != Siconos::SPARSE)
                THROW_EXCEPTION("y (output) must be a dense vector or A a sparse matrix.");

              if(numA == Siconos::DENSE)
                noalias(*y.dense()) += a * ublas::prod(*A.dense(), *x.sparse());
              else if(numA == Siconos::TRIANGULAR)
                noalias(*y.dense()) += a * ublas::prod(*A.triang(), *x.sparse());
              else if(numA == Siconos::SYMMETRIC)
                noalias(*y.dense()) += a * ublas::prod(*A.sym(), *x.sparse());
              else if(numA == Siconos::SPARSE)
              {
                if(numY == Siconos::DENSE)
                  noalias(*y.dense()) += a * ublas::prod(*A.sparse(), *x.sparse());
                else
                  noalias(*y.sparse()) += a * ublas::prod(*A.sparse(), *x.sparse());
              }
              else //if(numA==Siconos::BANDED)
                noalias(*y.dense()) += a * ublas::prod(*A.banded(), *x.sparse());
            }
          }
          else // if x and y are the same object => alias
          {
            if(numX == Siconos::DENSE)
            {
              if(numA == Siconos::DENSE)
                *y.dense() += a * ublas::prod(*A.dense(), *x.dense());
              else if(numA == Siconos::TRIANGULAR)
                *y.dense() += a * ublas::prod(*A.triang(), *x.dense());
              else if(numA == Siconos::SYMMETRIC)
                *y.dense() += a * ublas::prod(*A.sym(), *x.dense());
              else if(numA == Siconos::SPARSE)
                *y.dense() += a * ublas::prod(*A.sparse(), *x.dense());
              else //if(numA==Siconos::BANDED)
                *y.dense() += a * ublas::prod(*A.banded(), *x.dense());
            }
            else //if(numX == Siconos::SPARSE)
            {
              if(numA == Siconos::DENSE)
                *y.sparse() += a * ublas::prod(*A.dense(), *x.sparse());
              else if(numA == Siconos::TRIANGULAR)
                *y.sparse() += a * ublas::prod(*A.triang(), *x.sparse());
              else if(numA == Siconos::SYMMETRIC)
                *y.sparse() += a * ublas::prod(*A.sym(), *x.sparse());
              else if(numA == Siconos::SPARSE)
                *y.sparse() += a * ublas::prod(*A.sparse(), *x.sparse());
              else //if(numA==Siconos::BANDED)
                *y.sparse() += a * ublas::prod(*A.banded(), *x.sparse());
            }
          }
        }
      }
    }
  }
}

void taxpy(SPC::SiconosVector x, SPC::SiconosMatrix A, unsigned int startRow, unsigned int startCol, SP::SiconosVector y, bool init)
{
  assert(!(A->isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");
  // Computes y = subA *x (or += if init = false), subA being a sub-matrix of trans(A), between el. of A of index (col) startCol and startCol + sizeY
  if(init)  // y = subA * x , else y += subA * x
    y->zero();

  if(A->isBlock())
    THROW_EXCEPTION("not yet implemented for block matrix.");

  // we take a submatrix subA of A, starting from row startRow to row (startRow+sizeY) and between columns startCol and (startCol+sizeX).
  // Then computation of y = subA*x + y.
  Siconos::UBLAS_TYPE numA = A->num();
  Siconos::UBLAS_TYPE numY = y->num();
  Siconos::UBLAS_TYPE numX = x->num();
  unsigned int sizeX = x->size();
  unsigned int sizeY = y->size();

  if(numX != numY)
    THROW_EXCEPTION("not yet implemented for x and y of different types.");

  if(numY == Siconos::DENSE && numX == Siconos::DENSE)
  {

    assert(y->dense() != x->dense());

    if(numA == Siconos::DENSE)
      noalias(*y->dense()) += prod(ublas::subrange(trans(*A->dense()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if(numA == Siconos::TRIANGULAR)
      noalias(*y->dense()) += prod(ublas::subrange(trans(*A->triang()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if(numA == Siconos::SYMMETRIC)
      noalias(*y->dense()) += prod(ublas::subrange(trans(*A->sym()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else if(numA == Siconos::SPARSE)
      noalias(*y->dense()) += prod(ublas::subrange(trans(*A->sparse()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
    else //if(numA==Siconos::BANDED)
      noalias(*y->dense()) += prod(ublas::subrange(trans(*A->banded()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->dense());
  }
  else // x and y sparse
  {
    if(numA == Siconos::SPARSE)
      *y->sparse() += prod(ublas::subrange(trans(*A->sparse()), startRow, startRow + sizeY, startCol, startCol + sizeX), *x->sparse());
    else
      THROW_EXCEPTION("not yet implemented for x, y  sparse and A not sparse.");
  }
}


//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to 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)

#ifndef BOOST_MATH_CCMATH_ISINF
#define BOOST_MATH_CCMATH_ISINF

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>

namespace boost::math::ccmath {

template <typename T>
inline constexpr bool isinf(T x)
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(x))
    {
        return x == std::numeric_limits<T>::infinity() || -x == std::numeric_limits<T>::infinity();
    }
    else
    {
        using std::isinf;

        if constexpr (!std::is_integral_v<T>)
        {
            return isinf(x);
        }
        else
        {
            return isinf(static_cast<double>(x));
        }
    }
}

}

#endif // BOOST_MATH_CCMATH_ISINF


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Boost.SpatialIndex - geometry helper functions
//
// Copyright 2008 Federico J. Fernandez.
// Use, modification and distribution is subject to 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)

#ifndef BOOST_GEOMETRY_GGL_INDEX_RTREE_HELPERS_HPP
#define BOOST_GEOMETRY_GGL_INDEX_RTREE_HELPERS_HPP

#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/core/point_type.hpp>

namespace boost { namespace geometry { namespace index {

/**
 * \brief Given two boxes, returns the minimal box that contains them
 */
// TODO: use geometry::expand
template <typename Box>
inline Box enlarge_box(Box const& b1, Box const& b2)
{
    // TODO: mloskot - Refactor to readable form. Fix VC++8.0 min/max warnings:
    //  warning C4002: too many actual parameters for macro 'min

    typedef typename geometry::point_type<Box>::type point_type;

    point_type pmin(
        geometry::get<min_corner, 0>(b1) < geometry::get<min_corner, 0>(b2)
            ? geometry::get<min_corner, 0>(b1) : geometry::get<min_corner, 0>(b2),
        geometry::get<min_corner, 1>(b1) < geometry::get<min_corner, 1>(b2)
            ? geometry::get<min_corner, 1>(b1) : geometry::get<min_corner, 1>(b2));

    point_type pmax(
        geometry::get<max_corner, 0>(b1) > geometry::get<max_corner, 0>(b2)
            ? geometry::get<max_corner, 0>(b1) : geometry::get<max_corner, 0>(b2),
        geometry::get<max_corner, 1>(b1) > geometry::get<max_corner, 1>(b2)
            ? geometry::get<max_corner, 1>(b1) : geometry::get<max_corner, 1>(b2));

    return Box(pmin, pmax);
}

/**
 * \brief Compute the area of the union of b1 and b2
 */
template <typename Box>
inline typename default_area_result<Box>::type compute_union_area(Box const& b1, Box const& b2)
{
    Box enlarged_box = enlarge_box(b1, b2);
    return geometry::area(enlarged_box);
}

/**
 * \brief Checks if boxes intersects
 */
// TODO: move to geometry::intersects
template <typename Box>
inline bool is_overlapping(Box const& b1, Box const& b2)
{
    return ! geometry::disjoint(b1, b2);
}

}}} // namespace boost::geometry::index

#endif // BOOST_GEOMETRY_GGL_INDEX_RTREE_HELPERS_HPP