#ifndef RED_UARAP
#define RED_UARAP

#include <igl/polar_svd.h>
#include "mesh.h"
#include <Eigen/LU>
#include <iostream>
#include <string>
#include <igl/Timer.h>

using namespace Eigen;
using namespace std;

typedef Eigen::Triplet<double> Trip;
typedef Matrix<double, 12, 1> Vector12d;
typedef Matrix<double, 9, 1> Vector9d;

class Reduced_UArap
{

protected:
	SparseLU<SparseMatrix<double>>  aARAPKKTSparseSolver;
	SparseLU<SparseMatrix<double>> ajacLU;

	FullPivLU<MatrixXd>  aARAPKKTSolver;
	Eigen::VectorXd aPAx0, aEr, aEr_max, aEs, aEs_max, aEx, aDEDs, aFPAx0;
	SparseMatrix<double>  aExs_max, aExr_max, aErr_max, aErs_max, aPA;
	MatrixXd aExx, aExs, aExr, aErr, aErs, aPAG, aCG;
	SparseMatrix<double> aJacKKTSparse, aJacConstrainsSparse;
	MatrixXd aJacKKT, aJacConstrains;

	MatrixXd aPAx0DS;
	std::vector<Trip> aExx_trips, aExr_max_trips, aExr_trips, aErr_trips, aErs_trips, aErr_max_trips, aExs_trips, aExs_max_trips, aErs_max_trips, aC_trips;
	double jacLUanalyzed = false;

	SparseMatrix<double> adjointP, a_Wr, a_Ww;
	std::vector<MatrixXd> aePAx;
	std::vector<MatrixXd> aeUSUtPAx0;

	double aPAx0squaredNorm =0;
	VectorXd aPAx0tPAG;
	MatrixXd aPAx0tRSPAx0;
	MatrixXd aRSPAx0tRSPAx0;

	std::vector<MatrixXd> aFASTARAPDenseTerms;
	std::vector<MatrixXd> aFASTARAPSparseTerms;
	std::vector<MatrixXd> aFASTARAPCubeGtAtPtRSPAx0;
	std::vector<MatrixXd> aFASTARAPItRTerms1;
	std::vector<VectorXd> aFASTARAPItRTerms2;
	MatrixXd aFastEsTerm1;
	MatrixXd aFastEsTerm2;
	std::vector<MatrixXd> aFastEsTerm3s;

	igl::Timer atimer;

public:
	Reduced_UArap(Mesh& m){
		int r_size = m.red_w().size();
		int z_size = m.red_x().size();
		int s_size = m.red_s().size();
		int t_size = m.T().rows();
		int v_size = m.V().rows();

		aFPAx0.resize(12*m.T().rows());
		aFPAx0.setZero();

		aExx = (m.P()*m.A()*m.G()).transpose()*(m.P()*m.A()*m.G());

		print("arap 2");
		aErr_max.resize(3*t_size, 3*t_size);
		aErs_max.resize(3*t_size, 6*t_size);
		aExr_max.resize(3*v_size, 3*t_size);
		aExs_max.resize(3*v_size, 6*t_size);
		aEr_max.resize(3*t_size);
		aEs_max.resize(6*t_size);

		aExs.resize(z_size, s_size);
		aErr.resize(r_size, r_size);
		aErs.resize(r_size, s_size);
		aExr.resize(z_size, r_size);
		aEr.resize(r_size);
		aEs.resize(s_size);

		aPAx0 = m.P()*m.A()*m.x0();
		aPA = m.P()*m.A();
		aPAG = m.P()*m.A()*m.G();
		aCG = m.AB().transpose()*m.G();
		print("rarap 4");
		MatrixXd& YC = m.JointY();
		MatrixXd ARAPKKTmat = MatrixXd::Zero(aExx.rows() + YC.rows(), aExx.cols() + YC.rows());
		ARAPKKTmat.block(0,0,aExx.rows(), aExx.cols()) = aExx;
		ARAPKKTmat.block(0, aExx.cols(), YC.cols(), YC.rows()) = YC.transpose();
		ARAPKKTmat.block(aExx.rows(), 0, YC.rows(), YC.cols()) = YC;
		aARAPKKTSolver.compute(ARAPKKTmat);

		print("rarap 5");
		aJacKKT.resize(z_size+r_size+aCG.rows(), z_size+r_size+aCG.rows());
		aJacConstrains.resize(z_size+r_size+aCG.rows() ,s_size);
		print("rarap 6");
		setupAdjointP();
		print("pre-processing");
		m.constTimeFPAx0(aFPAx0);
		setupWrWw(m);
		// setupFASTARAPTerms(m);
		// setupFastPAx0DSTerm(m);
		// setupFastEsTerms(m);
		// setupFastEnergyTerms(m);
		setupFastItR(m);

		print("Jacobian solve pre-processing");
		aJacKKT.block(0,0,aExx.rows(), aExx.cols()) = Exx();
		aJacKKT.block(aExx.rows()+aExr.cols(), 0, aCG.rows(), aCG.cols()) = aCG;
		aJacKKT.block(0, aExx.cols()+aExr.cols(), aCG.cols(), aCG.rows())= aCG.transpose();
	}

	void setupFastItR(Mesh& m){
		std::map<int, std::vector<int>>& c_e_map = m.r_cluster_elem_map();
		for (int i=0; i<m.red_w().size()/3; i++){
			std::vector<int> cluster_elem = c_e_map[i];
			aePAx.push_back(MatrixXd::Zero(4*cluster_elem.size(), 3));
			aeUSUtPAx0.push_back(MatrixXd::Zero(4*cluster_elem.size(), 3));
		}
	}

	void setupFastEnergyTerms(Mesh& m){
		aPAx0squaredNorm = aPAx0.transpose()*aPAx0;
		aPAx0tPAG = aPAx0.transpose()*aPAG;

			std::vector<Trip> SPAx0_trips;
			for(int t=0; t<m.T().rows(); t++){
				for(int j=0; j<4; j++){
					Vector3d PAx0 = aPAx0.segment<3>(12*t + 3*j);
					SPAx0_trips.push_back(Trip( 12*t+3*j+0, 6*t+0 , PAx0[0]));
					SPAx0_trips.push_back(Trip( 12*t+3*j+0, 6*t+3 , PAx0[1]));
					SPAx0_trips.push_back(Trip( 12*t+3*j+0, 6*t+4 , PAx0[2]));

					SPAx0_trips.push_back(Trip( 12*t+3*j+1, 6*t+3 , PAx0[0]));
					SPAx0_trips.push_back(Trip( 12*t+3*j+1, 6*t+1 , PAx0[1]));
					SPAx0_trips.push_back(Trip( 12*t+3*j+1, 6*t+5 , PAx0[2]));

					SPAx0_trips.push_back(Trip( 12*t+3*j+2, 6*t+4 , PAx0[0]));
					SPAx0_trips.push_back(Trip( 12*t+3*j+2, 6*t+5 , PAx0[1]));
					SPAx0_trips.push_back(Trip( 12*t+3*j+2, 6*t+2 , PAx0[2]));

				}
			}
			SparseMatrix<double> SMPAx0(12*m.T().rows(), 6*m.T().rows());
			SMPAx0.setFromTriplets(SPAx0_trips.begin(), SPAx0_trips.end());
		aRSPAx0tRSPAx0 = (SMPAx0*m.sW()).transpose()*(SMPAx0*m.sW());

			std::vector<Trip> PAx0tR_trips;
			for(int t=0; t<m.T().rows(); t++){
				for(int j=0; j<4; j++){
					Vector3d PAx0 = aPAx0.segment<3>(12*t + 3*j);
					PAx0tR_trips.push_back(Trip(12*t+3*j+0, 9*t+0, PAx0[0]));
					PAx0tR_trips.push_back(Trip(12*t+3*j+0, 9*t+3, PAx0[1]));
					PAx0tR_trips.push_back(Trip(12*t+3*j+0, 9*t+6, PAx0[2]));

					PAx0tR_trips.push_back(Trip(12*t+3*j+1, 9*t+1, PAx0[0]));
					PAx0tR_trips.push_back(Trip(12*t+3*j+1, 9*t+4, PAx0[1]));
					PAx0tR_trips.push_back(Trip(12*t+3*j+1, 9*t+7, PAx0[2]));

					PAx0tR_trips.push_back(Trip(12*t+3*j+2, 9*t+2, PAx0[0]));
					PAx0tR_trips.push_back(Trip(12*t+3*j+2, 9*t+5, PAx0[1]));
					PAx0tR_trips.push_back(Trip(12*t+3*j+2, 9*t+8, PAx0[2]));
				}
			}
			SparseMatrix<double> RMPAx0(12*m.T().rows(), 9*m.T().rows());
			RMPAx0.setFromTriplets(PAx0tR_trips.begin(), PAx0tR_trips.end());
		aPAx0tRSPAx0 = (RMPAx0*a_Wr).transpose()*(SMPAx0*m.sW());
	}

	void setupFASTARAPTerms(Mesh& m){
		print("setupFASTARAPTerms");

		VectorXd AtPtPAx0 = (aPAG).transpose()*(aPAx0);
		aFASTARAPDenseTerms.push_back(AtPtPAx0);
		std::vector<Trip> PAx0_trips;
		for(int t=0; t<m.T().rows(); t++){
			for(int j=0; j<4; j++){
				Vector3d PAx0 = aPAx0.segment<3>(12*t + 3*j);
				PAx0_trips.push_back(Trip( 12*t+3*j+0, 6*t+0 , PAx0[0]));
				PAx0_trips.push_back(Trip( 12*t+3*j+0, 6*t+3 , PAx0[1]));
				PAx0_trips.push_back(Trip( 12*t+3*j+0, 6*t+4 , PAx0[2]));

				PAx0_trips.push_back(Trip( 12*t+3*j+1, 6*t+1 , PAx0[1]));
				PAx0_trips.push_back(Trip( 12*t+3*j+1, 6*t+3 , PAx0[0]));
				PAx0_trips.push_back(Trip( 12*t+3*j+1, 6*t+5 , PAx0[2]));

				PAx0_trips.push_back(Trip( 12*t+3*j+2, 6*t+2 , PAx0[2]));
				PAx0_trips.push_back(Trip( 12*t+3*j+2, 6*t+4 , PAx0[0]));
				PAx0_trips.push_back(Trip( 12*t+3*j+2, 6*t+5 , PAx0[1]));

			}
		}
		SparseMatrix<double> MPAx0(12*m.T().rows(), 6*m.T().rows());
		MPAx0.setFromTriplets(PAx0_trips.begin(), PAx0_trips.end());
		MatrixXd MPAx0sW = MPAx0*m.sW();

		MatrixXd& sW = m.sW();
		for(int s=0; s<sW.cols(); s++){
			VectorXd p = MPAx0sW.col(s);
			std::vector<Trip> BigCube_s_trips;
			for(int t=0; t<m.T().rows(); t++){
				for(int jj=0; jj<4; jj++){
					BigCube_s_trips.push_back(Trip(12*t+3*jj+0, 9*t+0, p[12*t+3*jj+0]));
					BigCube_s_trips.push_back(Trip(12*t+3*jj+0, 9*t+1, p[12*t+3*jj+1]));
					BigCube_s_trips.push_back(Trip(12*t+3*jj+0, 9*t+2, p[12*t+3*jj+2]));

					BigCube_s_trips.push_back(Trip(12*t+3*jj+1, 9*t+3, p[12*t+3*jj+0]));
					BigCube_s_trips.push_back(Trip(12*t+3*jj+1, 9*t+4, p[12*t+3*jj+1]));
					BigCube_s_trips.push_back(Trip(12*t+3*jj+1, 9*t+5, p[12*t+3*jj+2]));

					BigCube_s_trips.push_back(Trip(12*t+3*jj+2, 9*t+6, p[12*t+3*jj+0]));
					BigCube_s_trips.push_back(Trip(12*t+3*jj+2, 9*t+7, p[12*t+3*jj+1]));
					BigCube_s_trips.push_back(Trip(12*t+3*jj+2, 9*t+8, p[12*t+3*jj+2]));
				}
			}

			SparseMatrix<double> BigCube_s(12*m.T().rows(), 9*m.T().rows());
			BigCube_s.setFromTriplets(BigCube_s_trips.begin(), BigCube_s_trips.end());

			MatrixXd ClusteredCube_s = (aPAG).transpose()*BigCube_s*a_Wr;
			aFASTARAPCubeGtAtPtRSPAx0.push_back(ClusteredCube_s);
		}

		for(int g=0; g<m.red_r().size()/9; g++){
			std::vector<Trip> try_trips1;
			std::vector<Trip> try_trips2;
			std::vector<Trip> try_trips3;
			SparseMatrix<double>& B = m.RotBLOCK()[g];
			for(int i=0; i< B.cols()/3; i++){
				try_trips1.push_back(Trip(i, 3*i+0, 1));
				try_trips2.push_back(Trip(i, 3*i+1, 1));
				try_trips3.push_back(Trip(i, 3*i+2, 1));

			}

			SparseMatrix<double> try1(B.cols()/3, B.cols());
			SparseMatrix<double> try2(B.cols()/3, B.cols());
			SparseMatrix<double> try3(B.cols()/3, B.cols());
			try1.setFromTriplets(try_trips1.begin(), try_trips1.end());
			try2.setFromTriplets(try_trips2.begin(), try_trips2.end());
			try3.setFromTriplets(try_trips3.begin(), try_trips3.end());

			MatrixXd BtPAG = B.transpose()*aPAG;
			MatrixXd BtPAx0 = B.transpose()*aPAx0;
			MatrixXd BtMPAx0sW = B.transpose()*MPAx0sW;

			MatrixXd BtPAG1 = try1*BtPAG;
			MatrixXd BtPAG2 = try2*BtPAG;
			MatrixXd BtPAG3 = try3*BtPAG;

			VectorXd BtPAx0_1 = try1*BtPAx0;
			VectorXd BtPAx0_2 = try2*BtPAx0;
			VectorXd BtPAx0_3 = try3*BtPAx0;

			MatrixXd BtMPAx0sW1 = try1*BtMPAx0sW;
			MatrixXd BtMPAx0sW2 = try2*BtMPAx0sW;
			MatrixXd BtMPAx0sW3 = try3*BtMPAx0sW;

			MatrixXd zs11 = BtPAG1.transpose()*BtMPAx0sW1;
			MatrixXd zs12 = BtPAG1.transpose()*BtMPAx0sW2;
			MatrixXd zs13 = BtPAG1.transpose()*BtMPAx0sW3;
			MatrixXd zs21 = BtPAG2.transpose()*BtMPAx0sW1;
			MatrixXd zs22 = BtPAG2.transpose()*BtMPAx0sW2;
			MatrixXd zs23 = BtPAG2.transpose()*BtMPAx0sW3;
			MatrixXd zs31 = BtPAG3.transpose()*BtMPAx0sW1;
			MatrixXd zs32 = BtPAG3.transpose()*BtMPAx0sW2;
			MatrixXd zs33 = BtPAG3.transpose()*BtMPAx0sW3;

			VectorXd ps11 = BtPAx0_1.transpose()*BtMPAx0sW1;
			VectorXd ps12 = BtPAx0_1.transpose()*BtMPAx0sW2;
			VectorXd ps13 = BtPAx0_1.transpose()*BtMPAx0sW3;
			VectorXd ps21 = BtPAx0_2.transpose()*BtMPAx0sW1;
			VectorXd ps22 = BtPAx0_2.transpose()*BtMPAx0sW2;
			VectorXd ps23 = BtPAx0_2.transpose()*BtMPAx0sW3;
			VectorXd ps31 = BtPAx0_3.transpose()*BtMPAx0sW1;
			VectorXd ps32 = BtPAx0_3.transpose()*BtMPAx0sW2;
			VectorXd ps33 = BtPAx0_3.transpose()*BtMPAx0sW3;

			aFASTARAPItRTerms1.push_back(zs11);
			aFASTARAPItRTerms1.push_back(zs12);
			aFASTARAPItRTerms1.push_back(zs13);
			aFASTARAPItRTerms1.push_back(zs21);
			aFASTARAPItRTerms1.push_back(zs22);
			aFASTARAPItRTerms1.push_back(zs23);
			aFASTARAPItRTerms1.push_back(zs31);
			aFASTARAPItRTerms1.push_back(zs32);
			aFASTARAPItRTerms1.push_back(zs33);

			aFASTARAPItRTerms2.push_back(ps11);
			aFASTARAPItRTerms2.push_back(ps12);
			aFASTARAPItRTerms2.push_back(ps13);
			aFASTARAPItRTerms2.push_back(ps21);
			aFASTARAPItRTerms2.push_back(ps22);
			aFASTARAPItRTerms2.push_back(ps23);
			aFASTARAPItRTerms2.push_back(ps31);
			aFASTARAPItRTerms2.push_back(ps32);
			aFASTARAPItRTerms2.push_back(ps33);

		}
	}

	void setupAdjointP(){
		adjointP.resize(aExx.rows()+aErr.rows(), aExx.rows()+aErr.rows()+aCG.rows());
		for(int i=0; i<aExx.rows()+aErr.rows(); i++){
			adjointP.coeffRef(i,i) = 1;
		}
	}

	void setupWrWw(Mesh& m){
		std::vector<Trip> wr_trips;
		std::vector<Trip> ww_trips;

		std::map<int, std::vector<int>>& c_e_map = m.r_cluster_elem_map();
		for (int i=0; i<m.red_r().size()/9; i++){
			std::vector<int> cluster_elem = c_e_map[i];
			for(int e=0; e<cluster_elem.size(); e++){
				wr_trips.push_back(Trip(9*cluster_elem[e]+0, 9*i+0, 1));
				wr_trips.push_back(Trip(9*cluster_elem[e]+1, 9*i+1, 1));
				wr_trips.push_back(Trip(9*cluster_elem[e]+2, 9*i+2, 1));
				wr_trips.push_back(Trip(9*cluster_elem[e]+3, 9*i+3, 1));
				wr_trips.push_back(Trip(9*cluster_elem[e]+4, 9*i+4, 1));
				wr_trips.push_back(Trip(9*cluster_elem[e]+5, 9*i+5, 1));
				wr_trips.push_back(Trip(9*cluster_elem[e]+6, 9*i+6, 1));
				wr_trips.push_back(Trip(9*cluster_elem[e]+7, 9*i+7, 1));
				wr_trips.push_back(Trip(9*cluster_elem[e]+8, 9*i+8, 1));

				ww_trips.push_back(Trip(3*cluster_elem[e]+0, 3*i+0, 1));
				ww_trips.push_back(Trip(3*cluster_elem[e]+1, 3*i+1, 1));
				ww_trips.push_back(Trip(3*cluster_elem[e]+2, 3*i+2, 1));

			}
		}

		a_Wr.resize( 9*m.T().rows(), m.red_r().size());
		a_Wr.setFromTriplets(wr_trips.begin(), wr_trips.end());

		a_Ww.resize( 3*m.T().rows(), m.red_w().size());
		a_Ww.setFromTriplets(ww_trips.begin(), ww_trips.end());
	}

	void setupFastPAx0DSTerm(Mesh& m){
		aPAx0DS = MatrixXd::Zero(aPAx0.size(), m.red_s().size());
		for(int s=0; s<m.red_s().size()/6; s++){
			VectorXd sWx = m.sW().col(6*s+0);
			VectorXd sWy = m.sW().col(6*s+1);
			VectorXd sWz = m.sW().col(6*s+2);
			VectorXd sW01 = m.sW().col(6*s+3);
			VectorXd sW02 = m.sW().col(6*s+4);
			VectorXd sW12 = m.sW().col(6*s+5);

			VectorXd diag_x = VectorXd::Zero(12*m.T().rows());
			VectorXd diag_y = VectorXd::Zero(12*m.T().rows());
			VectorXd diag_z = VectorXd::Zero(12*m.T().rows());
			VectorXd diag_1 = VectorXd::Zero(12*m.T().rows());
			VectorXd diag_2 = VectorXd::Zero(12*m.T().rows());
			VectorXd diag_3 = VectorXd::Zero(12*m.T().rows());
			for(int i=0; i<m.T().rows(); i++){
				diag_x[12*i+0] = sWx[6*i];
				diag_x[12*i+3] = sWx[6*i];
				diag_x[12*i+6] = sWx[6*i];
				diag_x[12*i+9] = sWx[6*i];

				diag_y[12*i+0+1] = sWy[6*i+1];
				diag_y[12*i+3+1] = sWy[6*i+1];
				diag_y[12*i+6+1] = sWy[6*i+1];
				diag_y[12*i+9+1] = sWy[6*i+1];

				diag_z[12*i+0+2] = sWz[6*i+2];
				diag_z[12*i+3+2] = sWz[6*i+2];
				diag_z[12*i+6+2] = sWz[6*i+2];
				diag_z[12*i+9+2] = sWz[6*i+2];

				diag_1[12*i+0] = sW01[6*i+3];
				diag_1[12*i+3] = sW01[6*i+3];
				diag_1[12*i+6] = sW01[6*i+3];
				diag_1[12*i+9] = sW01[6*i+3];

				diag_2[12*i+0+1] = sW02[6*i+4];
				diag_2[12*i+3+1] = sW02[6*i+4];
				diag_2[12*i+6+1] = sW02[6*i+4];
				diag_2[12*i+9+1] = sW02[6*i+4];

				diag_3[12*i+0] = sW12[6*i+5];
				diag_3[12*i+3] = sW12[6*i+5];
				diag_3[12*i+6] = sW12[6*i+5];
				diag_3[12*i+9] = sW12[6*i+5];
			}

			aPAx0DS.col(6*s+0) += aPAx0.cwiseProduct(diag_x);
			aPAx0DS.col(6*s+1) += aPAx0.cwiseProduct(diag_y);
			aPAx0DS.col(6*s+2) += aPAx0.cwiseProduct(diag_z);

			aPAx0DS.col(6*s+3).tail(aPAx0.size()-1) += aPAx0.head(aPAx0.size()-1).cwiseProduct(diag_1.head(aPAx0.size()-1));
			aPAx0DS.col(6*s+3).head(aPAx0.size()-1) += aPAx0.tail(aPAx0.size()-1).cwiseProduct(diag_1.head(aPAx0.size()-1));

			aPAx0DS.col(6*s+4).tail(aPAx0.size()-2) += aPAx0.head(aPAx0.size()-2).cwiseProduct(diag_3.head(aPAx0.size()-2));
			aPAx0DS.col(6*s+4).head(aPAx0.size()-2) += aPAx0.tail(aPAx0.size()-2).cwiseProduct(diag_3.head(aPAx0.size()-2));

			aPAx0DS.col(6*s+5).tail(aPAx0.size()-1) += aPAx0.head(aPAx0.size()-1).cwiseProduct(diag_2.head(aPAx0.size()-1));
			aPAx0DS.col(6*s+5).head(aPAx0.size()-1) += aPAx0.tail(aPAx0.size()-1).cwiseProduct(diag_2.head(aPAx0.size()-1));
		}
	}

	void setupFastEsTerms(Mesh& m){
		std::vector<Trip> SPAx0_trips;
		for(int t=0; t<m.T().rows(); t++){
			for(int j=0; j<4; j++){
				Vector3d PAx0 = aPAx0.segment<3>(12*t + 3*j);
				SPAx0_trips.push_back(Trip( 12*t+3*j+0, 6*t+0 , PAx0[0]));
				SPAx0_trips.push_back(Trip( 12*t+3*j+0, 6*t+3 , PAx0[1]));
				SPAx0_trips.push_back(Trip( 12*t+3*j+0, 6*t+4 , PAx0[2]));

				SPAx0_trips.push_back(Trip( 12*t+3*j+1, 6*t+1 , PAx0[1]));
				SPAx0_trips.push_back(Trip( 12*t+3*j+1, 6*t+3 , PAx0[0]));
				SPAx0_trips.push_back(Trip( 12*t+3*j+1, 6*t+5 , PAx0[2]));

				SPAx0_trips.push_back(Trip( 12*t+3*j+2, 6*t+2 , PAx0[2]));
				SPAx0_trips.push_back(Trip( 12*t+3*j+2, 6*t+4 , PAx0[0]));
				SPAx0_trips.push_back(Trip( 12*t+3*j+2, 6*t+5 , PAx0[1]));

			}
		}
		SparseMatrix<double> SMPAx0(12*m.T().rows(), 6*m.T().rows());
		SMPAx0.setFromTriplets(SPAx0_trips.begin(), SPAx0_trips.end());
		MatrixXd SMPAx0sW = SMPAx0*m.sW();
		aFastEsTerm1 = SMPAx0sW.transpose()*aPAx0DS;

		std::vector<Trip> RtPAx0_trips;
		for(int t=0; t<m.T().rows(); t++){
			for(int j=0; j<4; j++){
				Vector3d PAx0 = aPAx0.segment<3>(12*t + 3*j);
				RtPAx0_trips.push_back(Trip( 12*t+3*j+0, 9*t+0 , PAx0[0]));
				RtPAx0_trips.push_back(Trip( 12*t+3*j+0, 9*t+3 , PAx0[1]));
				RtPAx0_trips.push_back(Trip( 12*t+3*j+0, 9*t+6 , PAx0[2]));

				RtPAx0_trips.push_back(Trip( 12*t+3*j+1, 9*t+1 , PAx0[0]));
				RtPAx0_trips.push_back(Trip( 12*t+3*j+1, 9*t+4 , PAx0[1]));
				RtPAx0_trips.push_back(Trip( 12*t+3*j+1, 9*t+7 , PAx0[2]));

				RtPAx0_trips.push_back(Trip( 12*t+3*j+2, 9*t+2 , PAx0[0]));
				RtPAx0_trips.push_back(Trip( 12*t+3*j+2, 9*t+5 , PAx0[1]));
				RtPAx0_trips.push_back(Trip( 12*t+3*j+2, 9*t+8 , PAx0[2]));

			}
		}
		SparseMatrix<double> RMPAx0(12*m.T().rows(), 9*m.T().rows());
		RMPAx0.setFromTriplets(RtPAx0_trips.begin(), RtPAx0_trips.end());
		SparseMatrix<double> RMPAx0Wr= RMPAx0*a_Wr;
		aFastEsTerm2 = RMPAx0Wr.transpose()*aPAx0DS;

		for(int g=0; g<m.red_x().size(); g++){
			std::vector<Trip> RtPAGzi_trips;
			for(int t=0; t<m.T().rows(); t++){
				for(int j=0; j<4; j++){
					Vector3d PAGi = aPAG.col(g).segment<3>(12*t + 3*j);
					RtPAGzi_trips.push_back(Trip( 12*t+3*j+0, 9*t+0 , PAGi[0]));
					RtPAGzi_trips.push_back(Trip( 12*t+3*j+0, 9*t+3 , PAGi[1]));
					RtPAGzi_trips.push_back(Trip( 12*t+3*j+0, 9*t+6 , PAGi[2]));

					RtPAGzi_trips.push_back(Trip( 12*t+3*j+1, 9*t+1 , PAGi[0]));
					RtPAGzi_trips.push_back(Trip( 12*t+3*j+1, 9*t+4 , PAGi[1]));
					RtPAGzi_trips.push_back(Trip( 12*t+3*j+1, 9*t+7 , PAGi[2]));

					RtPAGzi_trips.push_back(Trip( 12*t+3*j+2, 9*t+2 , PAGi[0]));
					RtPAGzi_trips.push_back(Trip( 12*t+3*j+2, 9*t+5 , PAGi[1]));
					RtPAGzi_trips.push_back(Trip( 12*t+3*j+2, 9*t+8 , PAGi[2]));

				}
			}
			SparseMatrix<double> RMPAGzi(12*m.T().rows(), 9*m.T().rows());
			RMPAGzi.setFromTriplets(RtPAGzi_trips.begin(), RtPAGzi_trips.end());
			SparseMatrix<double> RMPAGzi0Wr= RMPAGzi*a_Wr;
			MatrixXd EsTerm3 = RMPAGzi0Wr.transpose()*aPAx0DS;
			aFastEsTerm3s.push_back(EsTerm3);
		}
	}

	double Energy(Mesh& m){
		VectorXd PAx = aPA*m.red_x() + aPAx0;
		m.constTimeFPAx0(aFPAx0);
		double En = 0.5*(PAx - aFPAx0).squaredNorm();
		return En;
	}

	double Energy(Mesh& m, VectorXd& z, VectorXd& redw, VectorXd& redr, VectorXd& reds){
		VectorXd ms = m.sW()*reds;
		VectorXd mr = a_Wr*redr;
		VectorXd mw = a_Ww*redw;
		VectorXd PAx = aPAG*z + aPAx0;

		VectorXd FPAx0(PAx.size());
		for(int i=0; i<m.T().rows(); i++){
            Matrix3d ri = Map<Matrix3d>(mr.segment<9>(9*i).data()).transpose();
            Matrix3d r;
            Vector3d w;
            w<<mw(3*i+0),mw(3*i+1),mw(3*i+2);
            double wlen = w.norm();
            if (wlen>1e-9){
                double wX = w(0);
                double wY = w(1);
                double wZ = w(2);
                Matrix3d cross;
                cross<<0, -wZ, wY,
                        wZ, 0, -wX,
                        -wY, wX, 0;
                Matrix3d Rot = cross.exp();
                r = ri*Rot;
            }else{
                r = ri;
            }

            Matrix3d s;
            s<< ms[6*i + 0], ms[6*i + 3], ms[6*i + 4],
                ms[6*i + 3], ms[6*i + 1], ms[6*i + 5],
                ms[6*i + 4], ms[6*i + 5], ms[6*i + 2];

            Matrix3d rs = r*s;
            FPAx0.segment<3>(12*i+0) = rs*aPAx0.segment<3>(12*i+0);
            FPAx0.segment<3>(12*i+3) = rs*aPAx0.segment<3>(12*i+3);
            FPAx0.segment<3>(12*i+6) = rs*aPAx0.segment<3>(12*i+6);
            FPAx0.segment<3>(12*i+9) = rs*aPAx0.segment<3>(12*i+9);
        }
		return 0.5*(PAx - FPAx0).squaredNorm();
	}

	int Gradients(Mesh& m){

		// print("			+ Gradients");
		// print("		Ex");
		m.constTimeFPAx0(aFPAx0);
		aEx = dEdx(m);

		// print("		Er");
		aEr.setZero();
		Matrix3d Jx = cross_prod_mat(1,0,0);
		Matrix3d Jy = cross_prod_mat(0,1,0);
		Matrix3d Jz = cross_prod_mat(0,0,1);

		VectorXd PAg = aPAG*m.red_x() + aPAx0;
		VectorXd ms = m.sW()*m.red_s();
		VectorXd mr = a_Wr*m.red_r();

		for(int t=0; t<m.T().rows(); t++){
			Matrix3d s;
			s<< ms[6*t + 0], ms[6*t + 3], ms[6*t + 4],
				ms[6*t + 3], ms[6*t + 1], ms[6*t + 5],
				ms[6*t + 4], ms[6*t + 5], ms[6*t + 2];

			Matrix3d r0= Map<Matrix3d>(mr.segment<9>(9*t).data()).transpose();
			Matrix3d r1 = r0*Jx;
			Matrix3d r2 = r0*Jy;
			Matrix3d r3 = r0*Jz;

			Matrix3d p1 = -PAg.segment<3>(12*t+0)*(s*aPAx0.segment<3>(12*t+0)).transpose();
			Matrix3d p2 = -PAg.segment<3>(12*t+3)*(s*aPAx0.segment<3>(12*t+3)).transpose();
			Matrix3d p3 = -PAg.segment<3>(12*t+6)*(s*aPAx0.segment<3>(12*t+6)).transpose();
			Matrix3d p4 = -PAg.segment<3>(12*t+9)*(s*aPAx0.segment<3>(12*t+9)).transpose();

			double Er1 = p1.cwiseProduct(r1).sum() + p2.cwiseProduct(r1).sum() + p3.cwiseProduct(r1).sum() + p4.cwiseProduct(r1).sum();
			double Er2 = p1.cwiseProduct(r2).sum() + p2.cwiseProduct(r2).sum() + p3.cwiseProduct(r2).sum() + p4.cwiseProduct(r2).sum();
			double Er3 = p1.cwiseProduct(r3).sum() + p2.cwiseProduct(r3).sum() + p3.cwiseProduct(r3).sum() + p4.cwiseProduct(r3).sum();

			aEr_max[3*t+0] = Er1;
			aEr_max[3*t+1] = Er2;
			aEr_max[3*t+2] = Er3;

		}

		aEr = a_Ww.transpose()*aEr_max;

		// print("		Es");
		aEs_max.setZero();
		for(int t=0; t<m.T().rows(); t++){
			Matrix3d rt = Map<Matrix3d>(mr.segment<9>(9*t).data());
			Matrix3d s;
			s<< ms[6*t + 0], ms[6*t + 3], ms[6*t + 4],
				ms[6*t + 3], ms[6*t + 1], ms[6*t + 5],
				ms[6*t + 4], ms[6*t + 5], ms[6*t + 2];

			Matrix3d p1 = s*aPAx0.segment<3>(12*t+0)*aPAx0.segment<3>(12*t+0).transpose() - (rt*PAg.segment<3>(12*t+0))*aPAx0.segment<3>(12*t+0).transpose();
			Matrix3d p2 = s*aPAx0.segment<3>(12*t+3)*aPAx0.segment<3>(12*t+3).transpose() - (rt*PAg.segment<3>(12*t+3))*aPAx0.segment<3>(12*t+3).transpose();
			Matrix3d p3 = s*aPAx0.segment<3>(12*t+6)*aPAx0.segment<3>(12*t+6).transpose() - (rt*PAg.segment<3>(12*t+6))*aPAx0.segment<3>(12*t+6).transpose();
			Matrix3d p4 = s*aPAx0.segment<3>(12*t+9)*aPAx0.segment<3>(12*t+9).transpose() - (rt*PAg.segment<3>(12*t+9))*aPAx0.segment<3>(12*t+9).transpose();

			double Es1 = p1(0,0) + p2(0,0) + p3(0,0) + p4(0,0);
			double Es2 = p1(1,1) + p2(1,1) + p3(1,1) + p4(1,1);
			double Es3 = p1(2,2) + p2(2,2) + p3(2,2) + p4(2,2);
			double Es4 = p1(0,1) + p2(0,1) + p3(0,1) + p4(0,1)+ p1(1,0) + p2(1,0) + p3(1,0) + p4(1,0);
			double Es5 = p1(0,2) + p2(0,2) + p3(0,2) + p4(0,2)+ p1(2,0) + p2(2,0) + p3(2,0) + p4(2,0);
			double Es6 = p1(2,1) + p2(2,1) + p3(2,1) + p4(2,1)+ p1(1,2) + p2(1,2) + p3(1,2) + p4(1,2);
			aEs_max[6*t+0] = Es1;
			aEs_max[6*t+1] = Es2;
			aEs_max[6*t+2] = Es3;
			aEs_max[6*t+3] = Es4;
			aEs_max[6*t+4] = Es5;
			aEs_max[6*t+5] = Es6;

		}
		aEs = m.sW().transpose()*aEs_max;

		// print("			- Gradients");
		return 1;
	}

	VectorXd fastEs(Mesh& m){
		// print("		Es");
		aEs_max.setZero();
		for(int t=0; t<m.T().rows(); t++){
			Matrix3d rt = Map<Matrix3d>(mr.segment<9>(9*t).data());
			Matrix3d ut = m.U().block<3,3>(3*t,0).transpose();
			Matrix3d s;
			s<< ms[6*t + 0], ms[6*t + 3], ms[6*t + 4],
				ms[6*t + 3], ms[6*t + 1], ms[6*t + 5],
				ms[6*t + 4], ms[6*t + 5], ms[6*t + 2];

			Matrix3d p1 = s*ut*aPAx0.segment<3>(12*t+0)*(ut*aPAx0.segment<3>(12*t+0)).transpose() - (ut*rt*PAg.segment<3>(12*t+0))*(ut*aPAx0.segment<3>(12*t+0)).transpose();
			Matrix3d p2 = s*ut*aPAx0.segment<3>(12*t+3)*(ut*aPAx0.segment<3>(12*t+3)).transpose() - (ut*rt*PAg.segment<3>(12*t+3))*(ut*aPAx0.segment<3>(12*t+3)).transpose();
			Matrix3d p3 = s*ut*aPAx0.segment<3>(12*t+6)*(ut*aPAx0.segment<3>(12*t+6)).transpose() - (ut*rt*PAg.segment<3>(12*t+6))*(ut*aPAx0.segment<3>(12*t+6)).transpose();
			Matrix3d p4 = s*ut*aPAx0.segment<3>(12*t+9)*(ut*aPAx0.segment<3>(12*t+9)).transpose() - (ut*rt*PAg.segment<3>(12*t+9))*(ut*aPAx0.segment<3>(12*t+9)).transpose();

			double Es1 = p1(0,0) + p2(0,0) + p3(0,0) + p4(0,0);
			double Es2 = p1(1,1) + p2(1,1) + p3(1,1) + p4(1,1);
			double Es3 = p1(2,2) + p2(2,2) + p3(2,2) + p4(2,2);
			double Es4 = p1(0,1) + p2(0,1) + p3(0,1) + p4(0,1)+ p1(1,0) + p2(1,0) + p3(1,0) + p4(1,0);
			double Es5 = p1(0,2) + p2(0,2) + p3(0,2) + p4(0,2)+ p1(2,0) + p2(2,0) + p3(2,0) + p4(2,0);
			double Es6 = p1(2,1) + p2(2,1) + p3(2,1) + p4(2,1)+ p1(1,2) + p2(1,2) + p3(1,2) + p4(1,2);
			aEs_max[6*t+0] = Es1;
			aEs_max[6*t+1] = Es2;
			aEs_max[6*t+2] = Es3;
			aEs_max[6*t+3] = Es4;
			aEs_max[6*t+4] = Es5;
			aEs_max[6*t+5] = Es6;

		}
		aEs = m.sW().transpose()*aEs_max;
		return aEs;
	}

	VectorXd dEdx(Mesh& m){
		VectorXd PAx = aPA*m.red_x() + aPAx0;
		m.constTimeFPAx0(aFPAx0);
		VectorXd res = (aPA).transpose()*(PAx - aFPAx0);
		return res;
	}

	void itT(Mesh& m){
		//TODO DENSIFY
		VectorXd AtPtFPAx0 = (aPAG).transpose()*aFPAx0;
		VectorXd AtPtPAx0 = (aPAG).transpose()*(aPAx0);
		VectorXd gb = AtPtFPAx0 - AtPtPAx0;

		VectorXd zer = VectorXd::Zero(m.JointY().rows());

		VectorXd gd (gb.size() + zer.size());
		gd<<gb, zer;

		VectorXd result = aARAPKKTSolver.solve(gd);
		VectorXd gu = result.head(gb.size());
		m.red_x(gu);
 	}

	void itR(Mesh& m){

		VectorXd PAx = aPAG*m.red_x() + aPAx0;
		VectorXd& mr =m.red_r();

		std::map<int, std::vector<int>>& c_e_map = m.r_cluster_elem_map();
		for (int i=0; i<mr.size()/9; i++){
			std::vector<int> cluster_elem = c_e_map[i];
			for(int c=0; c<cluster_elem.size(); c++){
				aePAx[i].row(4*c+0) = PAx.segment<3>(12*cluster_elem[c]);
				aePAx[i].row(4*c+1) = PAx.segment<3>(12*cluster_elem[c]+3);
				aePAx[i].row(4*c+2) = PAx.segment<3>(12*cluster_elem[c]+6);
				aePAx[i].row(4*c+3) = PAx.segment<3>(12*cluster_elem[c]+9);

				aeUSUtPAx0[i].row(4*c+0) = USUtPAx0.segment<3>(12*cluster_elem[c]);
				aeUSUtPAx0[i].row(4*c+1) = USUtPAx0.segment<3>(12*cluster_elem[c]+3);
				aeUSUtPAx0[i].row(4*c+2) = USUtPAx0.segment<3>(12*cluster_elem[c]+6);
				aeUSUtPAx0[i].row(4*c+3) = USUtPAx0.segment<3>(12*cluster_elem[c]+9);
			}

			Matrix3d F = aePAx[i].transpose()*aeUSUtPAx0[i];
			Matrix3d ri,ti,ui,vi;
     		Vector3d _;
      		igl::polar_svd(F,ri,ti,ui,_,vi);

      		mr[9*i+0] = ri(0,0);
      		mr[9*i+1] = ri(0,1);
      		mr[9*i+2] = ri(0,2);
      		mr[9*i+3] = ri(1,0);
      		mr[9*i+4] = ri(1,1);
      		mr[9*i+5] = ri(1,2);
      		mr[9*i+6] = ri(2,0);
      		mr[9*i+7] = ri(2,1);
      		mr[9*i+8] = ri(2,2);
		}
	}

	bool minimize(Mesh& m){
		print("		+ ARAP minimize");

		double itRTimes = 0;
		double itTTimes = 0;

		double previous5ItE = Energy(m);
		double oldE = Energy(m);
		double newE;
		for(int i=1; i< 1000; i++){
			// atimer.start();
			itT(m);
			// atimer.stop();
			// itTTimes += atimer.getElapsedTimeInMicroSec();
			// atimer.start();
			itR(m);
			// atimer.stop();
			// itRTimes += atimer.getElapsedTimeInMicroSec();
			// m.constTimeFPAx0(aFPAx0);

			newE = Energy(m);
			// cout<<i<<", "<<newE-oldE<<endl;
			// VectorXd newEx = dEdx(m);
			// if((newE - oldE)>1e-5 && i>1){
			// 	print("Reduced_Arap::minimize() error. ARAP should monotonically decrease.");
			// 	print(i);
			// 	print(oldE);
			// 	print(newE);
			// 	exit(0);
			// }

			if(fabs(newE - oldE)<5e-6){
				// m.constTimeFPAx0(aFPAx0);
				// std::cout<<"		ItTtime: "<<itTTimes<<", ItRtime: "<<itRTimes<<", Iterations: "<<i<<endl;
				return true;
			}

			oldE = newE;
		}
		// std::cout<<"		ItTtime: "<<itTTimes<<", ItRtime: "<<itRTimes<<", Iterations: "<<1000<<endl;

		// std::cout<<"		NotConvergedARAPDiffInEnergy: "<<Energy(m)-previous5ItE<<std::endl;
		// m.constTimeFPAx0(aFPAx0);
		return false;
	}

	MatrixXd Exx(){ return aExx; }
	MatrixXd Exr(){ return aExr; }
	MatrixXd Exs(){ return aExs; }
	MatrixXd Ers(){ return aErs; }
	MatrixXd Err(){ return aErr; }
	VectorXd Er() { return aEr; }
	VectorXd Es() { return aEs; }
	VectorXd Ex() { return aEx; }
	VectorXd& FPAx0() { return aFPAx0; }

	std::vector<Eigen::Triplet<double>> to_triplets(Eigen::SparseMatrix<double> & M){
		std::vector<Eigen::Triplet<double>> v;
		for(int i = 0; i < M.outerSize(); i++){
			for(typename Eigen::SparseMatrix<double>::InnerIterator it(M,i); it; ++it){
				v.emplace_back(it.row(),it.col(),it.value());
			}
		}
		return v;
	}

	template<class T>
    void print(T a){ std::cout<<a<<std::endl; }

    MatrixXd RodriguesRotation(double wX, double wY, double wZ, double wlen){
        double c = cos(wlen);
        double s = sin(wlen);
        double c1 = 1 - c;
        Matrix3d Rot;
        Rot<< c + wX*wX*c1, -wZ*s + wX*wY*c1, wY*s + wX*wZ*c1,
            wZ*s + wX*wY*c1, c + wY*wY*c1, -wX*s + wY*wZ*c1,
            -wY*s + wX*wZ*c1, wX*s + wY*wZ*c1, c + wZ*wZ*c1;
        return Rot;
    }

    MatrixXd cross_prod_mat(double wX, double wY, double wZ){
        Matrix3d cross;
        cross<<0, -wZ, wY,
        		wZ, 0, -wX,
        		-wY, wX, 0;
        return cross;
    }

};

#endif


//
// Created by abakfja on 3/29/21.
//

#ifndef LA_MATRIX_ENGINE_TRAITS_HPP
#define LA_MATRIX_ENGINE_TRAITS_HPP

#include <boost/numeric/ublas/matrix/engine_traits.hpp>

namespace boost::numeric::ublas::experimental {

template<typename T, std::size_t R, std::size_t C>
struct static_matrix_engine;

template<typename T>
struct dynamic_matrix_engine;

} // namespace boost::numeric::ublas::experimental

namespace boost::numeric::ublas::experimental::detail {
template<typename T, std::size_t R, std::size_t C>
struct is_static<static_matrix_engine<T, R, C>> : std::true_type {
};

template<typename T>
struct is_dynamic<dynamic_matrix_engine<T>> : std::true_type {
};

}

#endif //LA_MATRIX_ENGINE_TRAITS_HPP


/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */

#include "bqvec/VectorOps.h"
#include "bqvec/Allocators.h"

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <stdexcept>
#include <vector>

using namespace breakfastquay;

BOOST_AUTO_TEST_SUITE(TestAllocators)

#define COMPARE_ARRAY(a, b)						\
    for (int cmp_i = 0; cmp_i < (int)(sizeof(a)/sizeof(a[0])); ++cmp_i) { \
        BOOST_CHECK_SMALL(a[cmp_i] - b[cmp_i], 1e-14);			\
    }

#define COMPARE_N(a, b, n)						\
    for (int cmp_i = 0; cmp_i < n; ++cmp_i) { \
        BOOST_CHECK_SMALL(a[cmp_i] - b[cmp_i], 1e-14);			\
    }

BOOST_AUTO_TEST_CASE(alloc_dealloc)
{
    double *v = allocate<double>(4);
    v[0] = 0.1;
    v[1] = 2.0;
    v[2] = -0.3;
    v[3] = 4.0;
    double *e = allocate<double>(4);
    e[0] = -0.3;
    e[1] = 4.0;
    e[2] = 0.1;
    e[3] = 2.0;
    v_fftshift(v, 4);
    COMPARE_N(v, e, 4);
    deallocate(v);
    deallocate(e);
}

BOOST_AUTO_TEST_CASE(alloc_zero)
{
    double *v = allocate_and_zero<double>(4);
    BOOST_CHECK_EQUAL(v[0], 0.f);
    BOOST_CHECK_EQUAL(v[1], 0.f);
    BOOST_CHECK_EQUAL(v[2], 0.f);
    BOOST_CHECK_EQUAL(v[3], 0.f);
    deallocate(v);
}

BOOST_AUTO_TEST_CASE(alloc_dealloc_channels)
{
    double **v = allocate_channels<double>(2, 4);
    v[0][0] = 0.1;
    v[0][1] = 2.0;
    v[0][2] = -0.3;
    v[0][3] = 4.0;
    v[1][0] = -0.3;
    v[1][1] = 4.0;
    v[1][2] = 0.1;
    v[1][3] = 2.0;
    v_fftshift(v[0], 4);
    COMPARE_N(v[0], v[1], 4);
    deallocate_channels(v, 2);
}

BOOST_AUTO_TEST_CASE(stl)
{
    std::vector<double, StlAllocator<double> > v;
    v.push_back(0.1);
    v.push_back(2.0);
    v.push_back(-0.3);
    v.push_back(4.0);
    double e[] = { -0.3, 4.0, 0.1, 2.0 };
    v_fftshift(v.data(), 4);
    COMPARE_N(v.data(), e, 4);
}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by Pedro Tacla Yamada on 2/7/20.
//

#include <array>
#include <boost/format.hpp>
#include <cmath>

#include "Utils.h"

namespace filesaver::utils
{

std::string prettyPrintBytes (off_t bytes)
{
    const std::array<std::string, 7> suffixes{
        "B",
        "KB",
        "MB",
        "GB",
        "TB",
        "PB",
        "EB",
    };
    unsigned int suffixIndex = 0;
    auto count = static_cast<double> (bytes);
    while (count >= 1024 && suffixIndex < 7)
    {
        suffixIndex++;
        count /= 1024;
    }

    if (count - floor (count) == 0.0)
    {
        return (boost::format ("%d%s") % count % suffixes[suffixIndex]).str ();
    }

    return (boost::format ("%.1f%s") % count % suffixes[suffixIndex]).str ();
}

} // namespace filesaver::utils


/* -*-C++-*-
   (c) Copyright 2008, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

#include <boost/tuple/tuple_io.hpp>
#include <Lintel/LintelLog.hpp>
#include <Lintel/StatsCube.hpp>
#include <Lintel/StringUtil.hpp>

using namespace std;
using boost::format;

typedef boost::tuple<int32_t, bool, string> IBSTuple;
typedef lintel::StatsCube<IBSTuple> IBSCube;

void checkWalk(const IBSCube::MyAny &ptuple, Stats &stat) {
    if (!ptuple.any[0]) {
	SINVARIANT(stat.count() == 1);
	SINVARIANT(stat.mean() == ptuple.data.get<0>() + 1);
    } else { // can have merges; (*, ? , ?)
	if (ptuple.any[1] && ptuple.any[2]) { // complete merge: (*, *, *)
	    SINVARIANT(stat.count() == 3 && stat.mean() == 2);
	} else if (ptuple.any[1]) { // partial merge: (*, *, v)
	    if (ptuple.data.get<0>() == 1) { // singleton
		SINVARIANT(stat.count() == 1 && stat.mean() == 2);
	    } else {
		SINVARIANT(stat.count() == 2 && stat.mean() == 2);
	    }
	} else if (ptuple.any[2]) { // partial merge: (*, v, *)
	    if (ptuple.data.get<0>() == 0) { // singleton
		SINVARIANT(stat.count() == 1 && stat.mean() == 1);
	    } else {
		SINVARIANT(stat.count() == 2 && stat.mean() == 2.5);
	    }
	} else { // partial merge: (*, v, v)
	    SINVARIANT(stat.count() == 1 && stat.mean() == ptuple.data.get<0>() + 1);
	}
    }
}

static IBSCube::MyAny cwo_last_tuple(IBSTuple(-1, true, "a"));

void checkWalkOrdered(const IBSCube::MyAny &ptuple, Stats &stat) {
    SINVARIANT(cwo_last_tuple < ptuple);
    cwo_last_tuple = ptuple;
    checkWalk(ptuple, stat);
}

struct ZeroData {
    string key;
    uint32_t count;
    uint32_t sum;
};

// Both of the below calculated using the perl program below; first with zero_count = 0,
// second with zero_count = 1
ZeroData raw_data_a[] = {
 { "*,*,*", 3, 6 }, { "*,*,a", 2, 4 }, { "*,*,b", 1, 2 }, { "*,f,*", 2, 5 }, { "*,f,a", 1, 3 },
 { "*,f,b", 1, 2 }, { "*,t,*", 1, 1 }, { "*,t,a", 1, 1 }, { "*,t,b", 0, 0 }, { "0,*,*", 1, 1 },
 { "0,*,a", 1, 1 }, { "0,*,b", 0, 0 }, { "0,f,*", 0, 0 }, { "0,f,a", 0, 0 }, { "0,f,b", 0, 0 },
 { "0,t,*", 1, 1 }, { "0,t,a", 1, 1 }, { "0,t,b", 0, 0 }, { "1,*,*", 1, 2 }, { "1,*,a", 0, 0 },
 { "1,*,b", 1, 2 }, { "1,f,*", 1, 2 }, { "1,f,a", 0, 0 }, { "1,f,b", 1, 2 }, { "1,t,*", 0, 0 },
 { "1,t,a", 0, 0 }, { "1,t,b", 0, 0 }, { "2,*,*", 1, 3 }, { "2,*,a", 1, 3 }, { "2,*,b", 0, 0 },
 { "2,f,*", 1, 3 }, { "2,f,a", 1, 3 }, { "2,f,b", 0, 0 }, { "2,t,*", 0, 0 }, { "2,t,a", 0, 0 },
 { "2,t,b", 0, 0 },
};

uint32_t nraw_data_a = sizeof(raw_data_a)/sizeof(ZeroData);

HashMap< string, std::pair<uint32_t, uint32_t> > zero_data;

ZeroData raw_data_b[] = {
 { "*,*,*", 12, 6 }, { "*,*,a", 6, 4 }, { "*,*,b", 6, 2 }, { "*,f,*", 6, 5 }, { "*,f,a", 3, 3 },
 { "*,f,b", 3, 2 }, { "*,t,*", 6, 1 }, { "*,t,a", 3, 1 }, { "*,t,b", 3, 0 }, { "0,*,*", 4, 1 },
 { "0,*,a", 2, 1 }, { "0,*,b", 2, 0 }, { "0,f,*", 2, 0 }, { "0,f,a", 1, 0 }, { "0,f,b", 1, 0 },
 { "0,t,*", 2, 1 }, { "0,t,a", 1, 1 }, { "0,t,b", 1, 0 }, { "1,*,*", 4, 2 }, { "1,*,a", 2, 0 },
 { "1,*,b", 2, 2 }, { "1,f,*", 2, 2 }, { "1,f,a", 1, 0 }, { "1,f,b", 1, 2 }, { "1,t,*", 2, 0 },
 { "1,t,a", 1, 0 }, { "1,t,b", 1, 0 }, { "2,*,*", 4, 3 }, { "2,*,a", 2, 3 }, { "2,*,b", 2, 0 },
 { "2,f,*", 2, 3 }, { "2,f,a", 1, 3 }, { "2,f,b", 1, 0 }, { "2,t,*", 2, 0 }, { "2,t,a", 1, 0 },
 { "2,t,b", 1, 0 },
};

uint32_t nraw_data_b = sizeof(raw_data_b)/sizeof(ZeroData);

void loadZeroData(ZeroData *raw_data, uint32_t nraw_data) {
    for(uint32_t i = 0; i < nraw_data; ++i) {
	ZeroData &z = raw_data[i];
	zero_data[z.key] = std::pair<uint32_t, uint32_t>(z.sum, z.count);
    }
}

void checkWalkZero(const IBSCube::MyAny &ptuple, Stats &stat) {
    vector<string> tmp;
    tmp.push_back(ptuple.any[0] ? "*" : str(format("%d") % ptuple.data.get<0>()));
    tmp.push_back(ptuple.any[1] ? "*" : (ptuple.data.get<1>() ? "t" : "f"));
    tmp.push_back(ptuple.any[2] ? "*" : ptuple.data.get<2>());

    string key = join(",", tmp);
    SINVARIANT(zero_data.exists(key));
    INVARIANT(zero_data[key].second == stat.count(), format("%s: %d != %d")
	      % key % zero_data[key].second % stat.count());
    SINVARIANT(round(stat.count() * stat.mean()) == zero_data[key].first);

    zero_data.remove(key);
}

void checkWalkZeroOrdered(const IBSCube::MyAny &ptuple, Stats &stat) {
    SINVARIANT(cwo_last_tuple < ptuple);
    cwo_last_tuple = ptuple;
    checkWalkZero(ptuple, stat);
}

bool pruneNoAny(const IBSCube::MyAny &ptuple) {
    LintelLogDebug("pna", boost::format("prune %s %d") % ptuple % ptuple.any.none());
    return ptuple.any.none();
}

int main() {
    LintelLog::parseEnv();
    lintel::StatsCube<IBSTuple> cube(boost::bind(&lintel::StatsCubeFns::cubeAll));

    cube.add(IBSTuple(0, true, "a"), 1);
    cube.add(IBSTuple(1, false, "b"), 2);
    cube.add(IBSTuple(2, false, "a"), 3);

    if (true) {
	cube.cube();

	cube.walk(boost::bind(checkWalk, _1, _2));
	cube.walkOrdered(boost::bind(checkWalkOrdered, _1, _2));

	cube.clear(false);
    }

    if (true) {
	cube.zeroCube(false);
	loadZeroData(raw_data_a, nraw_data_a);
	cube.walk(boost::bind(checkWalkZero, _1, _2));
	SINVARIANT(zero_data.size() == 0);

	cwo_last_tuple = IBSCube::MyAny(IBSTuple(-1, true, "a"));
	loadZeroData(raw_data_a, nraw_data_a);
	cube.walkOrdered(boost::bind(checkWalkZeroOrdered, _1, _2));
	SINVARIANT(zero_data.size() == 0);

	SINVARIANT(cube.size() == nraw_data_a);
	cube.prune(boost::bind(&pruneNoAny, _1));
	SINVARIANT(cube.size() == nraw_data_a - 3*2*2);
	cube.clear(false);
    }

    if (true) {
	cube.zeroCube(true);
	loadZeroData(raw_data_b, nraw_data_b);
	cube.walk(boost::bind(checkWalkZero, _1, _2));
	SINVARIANT(zero_data.size() == 0);

	cwo_last_tuple = IBSCube::MyAny(IBSTuple(-1, true, "a"));
	loadZeroData(raw_data_b, nraw_data_b);
	cube.walkOrdered(boost::bind(checkWalkZeroOrdered, _1, _2));
	SINVARIANT(zero_data.size() == 0);

	cube.clear(false);
    }

    cube.clear();
    cube.zeroCube();
    SINVARIANT(cube.size() == 0);

    return 0;
}

#if 0

#!/usr/bin/perl
my %base = ('0,t,a' => [1,1], '1,f,b' => [1,2], '2,f,a' => [1,3]);
my %zbase;
my %cube;
# stupid implementation, but different than the one in StatsCube,
# so better for regression testing.
my $default_count = 0;
foreach my $a (qw/0 1 2/) {
    foreach my $b (qw/t f/) {
	foreach my $c (qw/a b/) {
	    $zbase{"$a,$b,$c"} = $base{"$a,$b,$c"} || [$default_count,0];
	}
    }
}

foreach my $a (qw/0 1 2/) {
    foreach my $b (qw/t f/) {
	foreach my $c (qw/a b/) {
	    rollup($a,$b,$c);
	}
    }
}

my $c = 0;
foreach my $key (sort keys %cube) {
    my $nkey = $key;
    $nkey =~ s/\./\*/go;
    print qq[ { "$nkey", $cube{$key}->[0], $cube{$key}->[1] },];
    ++$c;
    if ($c > 4) {
	$c = 0;
	print "\n";
    }
}
print "\n";

sub rollup {
    my ($a,$b,$c) = @_;

    rollOne($a,$b,$c);
    rollOne($a,$b,'.');
    rollOne($a,'.',$c);
    rollOne($a,'.','.');
    rollOne('.',$b,$c);
    rollOne('.',$b,'.');
    rollOne('.','.',$c);
    rollOne('.','.','.');
}

sub rollOne {
    my $key = join(",", @_);

    return if defined $cube{$key};
    @keys = grep(/$key/, keys %zbase);
    my $count = 0;
    my $sum = 0;
    map { $count += $zbase{$_}->[0]; $sum += $zbase{$_}->[1] } @keys;
    $cube{$key} = [ $count, $sum ];
}

#endif


/* Copyright (C) 2017 IBM Corp.
 * 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.
 */
/**************************************************************
 * A program for testing the sampleG module
 **************************************************************/
//#include <mpfr.h>
#include <NTL/lzz_p.h>

NTL_CLIENT
#include "../utils/argmap.h"
#include "../TDMatrix.h"

#define NFACTORS 8
long tinyFactors[NFACTORS] = { 2, 3, 5, 7, 11, 13, 17, 19 };

int main(int argc, char *argv[])
{
#ifdef DEBUG
  NTL::SetSeed((unsigned char*)"test_sampleG",12);
#endif
  ArgMapping amap;

  long k = 2;
  amap.arg("k", k, "number of factors (~ log q)");

  long n = 2;
  amap.arg("n", n, "the small dimension"); // no default info

  long e=2;
  amap.arg("e", e, "the power of each factor");

  amap.parse(argc, argv); // parses and overrides initail values
                         // for each parameter, one per line,

  assert (k <= NFACTORS); // for this test we don't want many factors
  long m = (2+ k*e)*n;

  TDMatrixParams params(n,k,e,m);
  Vec<long>& factors = params.factors;

  cout << "n="<<n<<", m="<<m<<", k="<<k<<", e="<<e<<", q="<<params.getQ()<<endl;

  Vec<vec_zz_p> syndrome(INIT_SIZE, k);
  for (long i=0; i<k; i++) {
    params.zzp_context[i].restore();
    syndrome[i].SetLength(n);
    for (long j=0; j<n; j++) syndrome[i][j] = random_zz_p();
    cout << "syndrome mod "
	 << power_long(factors[i],e)<<"= "<<syndrome[i]<<endl <<flush;
  }

  // Sample x such that G*x = syndrome
  Vec<long> x;
  sampleG(x, syndrome, &params);

  cout << "sampled vector="<<x<<endl;
  assert(x.length()== n*e*k);
  // check that we have the right answer modulo p^e for all factors

  for (long i=0; i<k; i++) {
    params.zzp_context[i].restore();
    vec_zz_p xMod = conv<vec_zz_p>(x);
    long index = 0;
    for (long j=0; j<n; j++) { // check the next syndrome entry mod f_i
      zz_p xp = to_zz_p(0L);
      zz_p curFactor = to_zz_p(1L);
      for (long f=0; f<k; f++) for (long ei=0; ei<e; ei++) {
	  xp += (curFactor * x[index++]);
	  curFactor *= factors[f];
      }
      assert(syndrome[i][j] == xp);
    }
  }
  cout << "PASSED\n";

  return 0;
}


/**
 * Copyright (c) 2022 <Daumantas Kavolis>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once

#include "config.hpp"

MSVC_WARNING_DISABLE(4619)
#include <boost/container/small_vector.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/size.hpp>
MSVC_WARNING_POP()

#include "polynomial.hpp"
#include "traits.hpp"
#include "utils.hpp"

namespace poly {

template <class Poly>
class PolynomialProductView : public view<Poly> {
 public:
  using Base = view<Poly>;
  using Base::Base;

  using Traits = typename Poly::Traits;
  using Real = typename Traits::Real;
  using OrderType = typename Traits::OrderType;

  // NOLINTNEXTLINE(hicpp-explicit-conversions)
  operator PolynomialProductView<Poly const>() const noexcept {
    return {this->data(), this->size()};
  }

  template <bool check = true, class Range,
            typename = std::enable_if_t<detail::is_range<Range>::value>>
  [[nodiscard]] auto operator()(Range const& x,
                                bounds_check<check> /* unused */ = check_bounds) const -> Real {
    if constexpr (check) {
      if (boost::size(x) != this->size())
        throw std::out_of_range("Dimensions do not match size of polynomials");
    }

    Real y = 1.0;
    for (auto&& [index, xx] : x | boost::adaptors::indexed()) y *= (*this)[index](xx);
    return y;
  }

  template <bool check = true, class Range,
            typename = std::enable_if_t<detail::is_range<Range>::value>>
  void assign(Range const& orders, bounds_check<check> c = check_bounds) const {
    check_size(orders, c);

    for (auto&& [index, order] : orders | boost::adaptors::indexed())
      (*this)[index].order(narrow<OrderType>(order));
  }

  template <bool check = true, class Range,
            typename = std::enable_if_t<detail::is_range<Range>::value>>
  auto matches_orders(Range const& orders, bounds_check<check> c = check_bounds) const -> bool {
    return match(
        orders,
        [](Poly const& poly, auto&& order) {
          return poly.order() == narrow_cast<OrderType>(order);
        },
        c);
  }

  template <bool check = true, class Range,
            typename = std::enable_if_t<detail::is_range<Range>::value>>
  auto matches_zeros(Range const& zeros, bounds_check<check> c = check_bounds) const
      -> std::size_t {
    return match(
        zeros,
        [](Poly const& poly, auto&& is_zero) {
          return (poly.order() == 0) == static_cast<bool>(is_zero);
        },
        c);
  }

  template <bool check = true, class Range,
            typename = std::enable_if_t<detail::is_range<Range>::value>>
  auto matches_non_zeros(Range const& non_zeros, bounds_check<check> c = check_bounds) const
      -> std::size_t {
    return match(
        non_zeros,
        [](Poly const& poly, auto&& is_non_zero) {
          return (poly.order() != 0) == static_cast<bool>(is_non_zero);
        },
        c);
  }

 private:
  template <class Range, bool check, class F>
  auto match(Range const& range, F const& function, bounds_check<check> c = check_bounds) const
      -> bool {
    check_size(range, c);
    for (auto&& [index, value] : range | boost::adaptors::indexed()) {
      if (!function((*this)[index], value)) return false;
    }
    return true;
  }

  template <class Range, bool check>
  void check_size(Range const& range, bounds_check<check> /* unused */ = check_bounds) const {
    if constexpr (check) {
      if (boost::size(range) != this->size())
        throw std::out_of_range("Size of orders does not match size of polynomials");
    }
  }
};

template <typename Poly, std::size_t N = SmallStorageSize>
class PolynomialProduct {
 public:
  using Traits = typename Poly::Traits;
  using Real = typename Traits::Real;
  using OrderType = typename Traits::OrderType;
  using PolynomialType = Poly;
  using View = PolynomialProductView<PolynomialType>;
  using ConstView = PolynomialProductView<PolynomialType const>;

  template <typename T>
  using small_vector = boost::container::small_vector<T, N>;
  using polynomials_vector = small_vector<PolynomialType>;
  using iterator = typename polynomials_vector::iterator;
  using const_iterator = typename polynomials_vector::const_iterator;

  explicit PolynomialProduct(OrderType dimensions) : polynomials_{dimensions} {}

  template <class Range, typename = std::enable_if_t<detail::is_range<Range>::value>>
  explicit PolynomialProduct(Range const& orders) {
    auto count = boost::size(orders);
    polynomials_.resize(count);
    static_cast<View>(*this).assign(orders, no_bounds_check);
  }

  [[nodiscard]] auto polynomials() noexcept -> view<PolynomialType> {
    return {polynomials_.data(), polynomials_.size()};
  }
  [[nodiscard]] auto polynomials() const noexcept -> view<PolynomialType const> {
    return {polynomials_.data(), polynomials_.size()};
  }

  template <class... Args>
  void emplace_back(Args... args) {
    polynomials_.emplace_back(std::forward<Args>(args)...);
  }

  auto operator[](std::size_t index) noexcept -> PolynomialType& { return polynomials_[index]; }
  [[nodiscard]] auto operator[](std::size_t index) const noexcept -> PolynomialType const& {
    return polynomials_[index];
  }

  auto at(std::size_t index) noexcept -> PolynomialType& { return polynomials_.at(index); }
  [[nodiscard]] auto at(std::size_t index) const noexcept -> PolynomialType const& {
    return polynomials_.at(index);
  }

  template <bool check = true, class Range,
            typename = std::enable_if_t<detail::is_range<Range>::value>>
  [[nodiscard]] auto operator()(Range const& x, bounds_check<check> c = check_bounds) const
      -> Real {
    return static_cast<ConstView>(*this).operator()(x, c);
  }

  [[nodiscard]] auto dimensions() const noexcept -> std::size_t { return polynomials_.size(); }
  void dimensions(OrderType new_size) { polynomials_.resize(new_size); }
  [[nodiscard]] auto size() const noexcept -> std::size_t { return dimensions(); }
  void resize(std::size_t new_size) { polynomials_.resize(new_size); }
  void clear() { polynomials_.clear(); }

  auto begin() noexcept -> iterator { return polynomials_.begin(); }
  auto begin() const noexcept -> const_iterator { return polynomials_.begin(); }
  auto end() noexcept -> iterator { return polynomials_.end(); }
  auto end() const noexcept -> const_iterator { return polynomials_.end(); }

  // NOLINTNEXTLINE(hicpp-explicit-conversions)
  operator View() noexcept { return {polynomials_.data(), polynomials_.size()}; }

  // NOLINTNEXTLINE(hicpp-explicit-conversions)
  operator ConstView() const noexcept { return {polynomials_.data(), polynomials_.size()}; }

 private:
  polynomials_vector polynomials_;
};
}  // namespace poly

POLY_TEMPLATE_RANGE(poly::PolynomialProduct)


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include "paal/data_structures/splay_tree.hpp"

#include <boost/test/unit_test.hpp>

#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/algorithm/equal.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/range/adaptor/sliced.hpp>
#include <boost/range/adaptor/indexed.hpp>

#include <algorithm>
#include <array>
#include <climits>
#include <utility>
#include <vector>
#include <random>

using paal::data_structures::splay_tree;

template <typename T, typename I> bool check_content(T &t, I const & input) {
    if (!boost::equal(input, boost::make_iterator_range(t.begin(), t.end()))) {
        return false;
    }

    if (!boost::equal(input | boost::adaptors::reversed,
                      boost::make_iterator_range(t.rbegin(), t.rend()))) {
        return false;
    }

    //checking operator[]
    for (auto v : input | boost::adaptors::indexed()) {
        if (v.value() != t[v.index()]) {
            return false;
        }
    }
    return true;
}

template <typename T, typename I> bool check_content(T &t, I begin, I end) {
    return check_content(t, boost::make_iterator_range(begin, end));
}

template <typename T, typename I>
bool check_random_splay(T &t, I const & input, const std::size_t k_m) {
    const std::size_t k_n = boost::size(input);
    for (std::size_t i = 0; i < k_m; i++) {
        int el = rand() % k_n; // NOLINT
        if (input[el] != *(t.splay(el))) {
            return false;
        }
    }
    if (!check_content(t, input)) {
        return false;
    }
    return true;
}

template <typename T, typename I> bool check_random_splay(T &t, I begin, I end, const std::size_t k_m) {
    return check_random_splay(t, boost::make_iterator_range(begin, end), k_m);
}

BOOST_AUTO_TEST_SUITE(splay_treeTest)

BOOST_AUTO_TEST_CASE(splay_treeCreateEmpty) {
    splay_tree<int> t;
    BOOST_CHECK_EQUAL(std::size_t(0), t.size());
    BOOST_CHECK(t.empty());
    std::vector<int> input(0);
    BOOST_CHECK(check_content(t, input));
}

BOOST_AUTO_TEST_CASE(splay_tree_CreateFromRange) {
    const int k_n = 101;
    std::array<int, k_n> input;
    boost::iota(input, 0);
    splay_tree<int> t(input.begin(), input.end());
    splay_tree<int> t1(input);
    BOOST_CHECK(check_content(t, input));
    BOOST_CHECK(check_content(t1, input));
}

BOOST_AUTO_TEST_CASE(splay_tree_Find) {
    const int k_n = 1001;
    std::array<int, k_n> input;
    boost::iota(input, 0);
    splay_tree<int> t(input);
    BOOST_CHECK(check_content(t, input));
    BOOST_CHECK_EQUAL_COLLECTIONS(input.begin(), input.end(),
        t.begin(), t.end());
}

BOOST_AUTO_TEST_CASE(splay_tree_splay) {
    srand(123433);
    const int k_n = 131, k_m = 1701;
    std::array<int, k_n> input;
    boost::iota(input, 0);
    splay_tree<int> t(input);
    BOOST_CHECK(check_content(t, input));
    BOOST_CHECK(check_random_splay(t, input, k_m));
}

BOOST_AUTO_TEST_CASE(splay_tree_split_higher) {
    srand(123433);
    const int k_n = 10435, kL = 47;
    std::array<int, k_n> input;
    boost::iota(input, 0);
    splay_tree<int> t(input);
    BOOST_CHECK(check_content(t, input));
    const int k1 = k_n / 2, k2 = k_n / 4, k3 = k_n / 8;
    splay_tree<int> t1 = t.split_higher(k1);
    auto b = input.begin();
    BOOST_CHECK(check_random_splay(t, b, b + k1 + 1, kL));
    BOOST_CHECK(check_random_splay(t1, b + k1 + 1, b + k_n, kL));
    splay_tree<int> t2 = t.split_higher(k2);
    BOOST_CHECK(check_random_splay(t, b, b + k2 + 1, kL));
    BOOST_CHECK(check_random_splay(t2, b + k2 + 1, b + k1 + 1, kL));
    splay_tree<int> t3 = t.split_higher(k3);
    BOOST_CHECK(check_random_splay(t, b, b + k3 + 1, kL));
    BOOST_CHECK(check_random_splay(t3, b + k3 + 1, b + k2 + 1, kL));
}

BOOST_AUTO_TEST_CASE(splay_tree_split_lower) {
    srand(123433);
    const int k_n = 10435, kL = 47;
    std::array<int, k_n> input;
    boost::iota(input, 0);
    splay_tree<int> t(input);
    BOOST_CHECK(check_content(t, input));
    const int k1 = k_n / 2, k2 = k_n / 4, k3 = k_n / 8;
    splay_tree<int> t1 = t.split_lower(k1);
    auto b = input.begin();
    BOOST_CHECK(check_random_splay(t, b + k1, b + k_n, kL));
    BOOST_CHECK(check_random_splay(t1, b, b + k1, kL));
    splay_tree<int> t2 = t.split_lower(k2);
    BOOST_CHECK(check_random_splay(t, b + k1 + k2, b + k_n, kL));
    BOOST_CHECK(check_random_splay(t2, b + k1, b + k1 + k2, kL));
    splay_tree<int> t3 = t.split_lower(k3);
    BOOST_CHECK(check_random_splay(t, b + k1 + k2 + k3, b + k_n, kL));
    BOOST_CHECK(
    check_random_splay(t3, b + k1 + k2, b + k1 + k2 + k3, kL));
}

BOOST_AUTO_TEST_CASE(splay_tree_Merge) {
    srand(123433);
    const int k_n = 10382, k_m = 3875, kL = 51;
    BOOST_CHECK(k_n > k_m);
    std::array<int, k_n> input;
    boost::iota(input, 0);
    auto front = input | boost::adaptors::sliced(0, k_m);
    splay_tree<int> t1(front);
    BOOST_CHECK(check_content(t1, front));
    auto b = input.begin();
    splay_tree<int> t2(b + k_m, b + k_n);
    BOOST_CHECK(check_content(t2, b + k_m, b + k_n));
    t1.merge_right(t2);
    BOOST_CHECK(check_random_splay(t1, input, kL));
}

BOOST_AUTO_TEST_CASE(splay_tree_Reverse) {
    srand(123433);
    const int k_n = 231, k_m = 2000, kL = 47;
    std::array<int, k_n> input;
    boost::iota(input, 0);
    splay_tree<int> t(input);
    BOOST_CHECK(check_content(t, input));
    for (int i = 0; i < k_m; i++) {
        int a = rand() % k_n;                       // NOLINT
        int b = (a < k_n) ? rand() % (k_n - a) : 0; // NOLINT
        auto begin = input.begin();
        std::reverse(begin + a, begin + a + b + 1);
        t.reverse(a, a + b);
        BOOST_CHECK(check_random_splay(t, input, kL));
    }
}

class splay_tree_performance {
  public:
    static const int k_n = 104729, k_m = 26669;
    static const uint32_t seed = 331u;

    std::mt19937 gen;
    std::uniform_int_distribution<int> dist;
    std::array<int, k_n> input;
    splay_tree<int> tree;

    splay_tree_performance()
        : dist(std::uniform_int_distribution<int>(0, k_n - 1)) {}

    void test() {
        set_up();
        for (int i = 0; i < k_m; i++) {
            auto p = rand_reverse();
            tree.reverse(p.first, p.second);
        }
    }

  protected:
    std::pair<int, int> rand_reverse() {
        int a = dist(gen);
        int b = (a < k_n) ? dist(gen) % (k_n - a) : 0;
        return std::make_pair(a, a + b);
    }

    virtual void set_up() {
        gen.seed(seed);
        boost::iota(input, 0);
        tree = splay_tree<int>(input);
    }
};

const uint32_t splay_tree_performance::seed;

/*BOOST_AUTO_TEST_CASE(splay_tree_performance_ReverseVector) {
  for (int i = 0; i < k_m; i++) {
    auto p = rand_reverse();
    std::reverse(input + p.first, input + p.second + 1);
  }
}*/

BOOST_AUTO_TEST_CASE(splay_tree_performance_reverse_splay) {
    splay_tree_performance().test();
}

BOOST_AUTO_TEST_SUITE_END()


/*!
  \file     MatrixIO.cpp
  \author       Tody
  MatrixIO definition.
  date      2014/07/09
*/

#include "MatrixIO.h"

#include <Eigen/SparseCore>
#include <unsupported/Eigen/SparseExtra>

#include <fstream>

void MatrixIO::save ( const Eigen::MatrixXd& mat, const std::string& filePath )
{
    std::ofstream  ofs ( filePath );
    if ( !ofs )
    {
        std::string openError ( "Could not open: " );
        openError += filePath;
        std::cout << openError << std::endl;
        return;
    }
    ofs << mat;
}

void MatrixIO::saveSparse ( const Eigen::SparseMatrix<double>& M, const std::string& filePath )
{
    Eigen::saveMarket ( M, filePath );
}

void MatrixIO::load ( Eigen::MatrixXd& mat, const std::string& filePath )
{
    std::ifstream ifs ( filePath );

    if ( !ifs )
    {
        std::string openError ( "Could not open: " );
        openError += filePath;
        std::cout << openError << std::endl;
        return;
    }

    std::string str;

    std::vector <double> dataVec;

    int rowSize = 0;
    int colSize = 0;

    while ( ifs && getline ( ifs, str ) )
    {
        std::istringstream iss ( str );

        int tColSize = 0;
        while ( !iss.eof() )
        {
            double data;
            iss >> data;
            dataVec.push_back ( data );
            tColSize++;
        }

        if ( colSize == 0 )
        {
            colSize = tColSize;
        }

        if ( colSize != tColSize )
        {
            std::cout << "Matrix data is broken."  << std::endl;
        }
        rowSize++;
    }

    if ( dataVec.size() != rowSize * colSize )
    {
        std::cout << "Matrix data is broken."  << std::endl;
    }

    mat = Eigen::MatrixXd ( rowSize, colSize );

    for ( unsigned int i = 0; i < rowSize; ++i )
    {
        for ( unsigned int j = 0; j < colSize; ++j )
        {
            mat ( i, j ) = dataVec[i * colSize + j];
        }
    }
}

void MatrixIO::loadSparse ( Eigen::SparseMatrix<double>& M, const std::string& filePath )
{
    Eigen::loadMarket ( M, filePath );
}


#include <iostream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/timer.hpp>

#include <vector>
#include <math.h>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
#include "OneChQS.hpp"

void OneChQS_UpdateQm1fQ(CNRGmatrix &Qm1fNQ,CNRGarray Aeig,
			 CNRGbasisarray Abasis, CNRGbasisarray SingleSite){

  boost::numeric::ublas::matrix<double> Mtemp;
  boost::numeric::ublas::matrix<double> fnw;

  int icount=0;

  Qm1fNQ.ClearAll();

  Qm1fNQ.SyncNRGarray(Aeig);

  // Find blocks such that Q'=Q+1, S'=S+-1/2

  for (int ii=0;ii<Aeig.NumBlocks();ii++)
    {
      double Qi=Aeig.GetQNumber(ii,0);
      double Si=Aeig.GetQNumber(ii,1);

      int Nstibl=Aeig.GetBlockSize(ii);

      for (int jj=ii+1;jj<Aeig.NumBlocks();jj++)
	{

	  double Qj=Aeig.GetQNumber(jj,0);
	  double Sj=Aeig.GetQNumber(jj,1);

	  int Nstjbl=Aeig.GetBlockSize(jj);

	  int kkp=0;
	  //if (Sj==Si+0.5) kkp=23;
	  //if (Sj==Si-0.5) kkp=32;
	  //if (fabs(Sj-(Si+0.5))<1.0E-10) kkp=23;
	  //if (fabs(Sj-(Si-0.5))<1.0E-10) kkp=32;

	  if (dEqual(Sj,(Si+0.5))) kkp=23;
	  if (dEqual(Sj,(Si-0.5))) kkp=32;

	  //	  if ( (Qj==Qi+1.0)&&( kkp!=0 ) )
	  //if ( ( fabs(Qj-(Qi+1.0))<1.0E-10 )&&( kkp!=0 ) )
	  if ( ( dEqual(Qj,(Qi+1.0)) )&&( kkp!=0 ) )
	    {

	      cout << "  Setting up Block i : " << ii
	         << " (size " <<  Nstibl
	      	   << ") x Block j " << jj
	         << " (size " << Nstjbl << ")" << endl;

 	      Qm1fNQ.MatBlockMap.push_back(ii);
 	      Qm1fNQ.MatBlockMap.push_back(jj);
	      Qm1fNQ.MatBlockBegEnd.push_back(icount);
	      icount+=Nstibl*Nstjbl;
	      Qm1fNQ.MatBlockBegEnd.push_back(icount-1);

	      // Sets the uBLAS matrix for Z_QS(iblock)
	      // Sets the uBLAS matrix for Z_QS(jblock)

	      cout << "    Setting up BLAS matrices..." << endl;
// 	      boost::numeric::ublas::matrix<double> Zibl=Aeig.EigVec2BLAS(ii);
// 	      boost::numeric::ublas::matrix<double> Zjbl=Aeig.EigVec2BLAS(jj);

	      boost::numeric::ublas::matrix<double> Zibl(Nstibl,Nstibl);
	      boost::numeric::ublas::matrix<double> Zjbl(Nstjbl,Nstjbl);
	      Zibl=Aeig.EigVec2BLAS(ii);
	      Zjbl=Aeig.EigVec2BLAS(jj);

	      cout << "    ...Zs done ..." << endl;
	      // Sets the uBLAS matrix <type|fn|type^'> with a bunch of zeroes

	      boost::numeric::ublas::matrix<double> fnbasis (Nstibl,Nstjbl);

	      // Check time
	      boost::timer t;
	      double time_elapsed;

	      // Loop on each block
	      int istbl=0;
	      for (int ist=Abasis.GetBlockLimit(ii,0);
		   ist<=Abasis.GetBlockLimit(ii,1);ist++)
		{
		  int typei=Abasis.iType[ist];
		  int stcfi=Abasis.StCameFrom[ist];
		  //cout << "Type i : " << typei << endl;
		  int jstbl=0;
		  for (int jst=Abasis.GetBlockLimit(jj,0);
		       jst<=Abasis.GetBlockLimit(jj,1);jst++)
		    {
		      int typej=Abasis.iType[jst];
		      int stcfj=Abasis.StCameFrom[jst];
		      //cout << "Type j : " << typej << endl;
		      // setting MatBlockMap
  		      ////Qm1fNQ.MatBlockMap.push_back(ii);
  		      ////Qm1fNQ.MatBlockMap.push_back(jj);

		      fnbasis(istbl,jstbl)=0.0;

		      // Both basis sates have to originate from the same
		      // previous state! Otherwise, zero.

		      if (stcfi==stcfj)
			{
// 			  if (
// 			      ( (typei==1) && (typej==2) &&(kkp==23) )||
// 			      ( (typei==1) && (typej==3) &&(kkp==32) )
// 			      )
// 			    fnbasis(istbl,jstbl)=1.0;
// 			  if ( (typej==4)&&(typei==2)&&(kkp==32) )
// 			    fnbasis(istbl,jstbl)=-sqrt((2.0*Si+1.0)/(2.0*Si));
// 			  if ( (typej==4)&&(typei==3)&&(kkp==23) )
// 			    fnbasis(istbl,jstbl)=sqrt((2.0*Si+1.0)/(2.0*Si+2.0));
			  // New approach
			  double Qtildei=SingleSite.GetQNumber(typei,0);
			  double Stildei=SingleSite.GetQNumber(typei,1);
			  double Sztildei=SingleSite.GetQNumber(typei,2);

			  double Qtildej=SingleSite.GetQNumber(typej,0);
			  double Stildej=SingleSite.GetQNumber(typej,1);
			  double Sztildej=SingleSite.GetQNumber(typej,2);

			  double siteqnumsi[]={Qtildei,Stildei,Sztildei};
			  double siteqnumsj[]={Qtildej,Stildej,Sztildej};

			  double Scfi=Si-Sztildei;
			  double Scfj=Sj-Sztildej;

			  double auxBasis[2]={0.0,0.0};
			  double auxCG[2]={0.0,0.0};

			  //cout << " Si = " << Si << " Scfi = " << Scfi << " S~i = "<< Stildei << endl;
			  //cout << " Sj = " << Sj << " Scfj = " << Scfj << " S~j = "<< Stildej << endl;

			  // Loop in Sztildej,sigma: sztildei=Sztildej-sigma

			  for (Sztildej=-Stildej;
			       Sztildej<=Stildej;Sztildej+=1.0)
			    {
			      siteqnumsj[2]=Sztildej;
			      int sitestatej=SingleSite.GetBlockFromQNumbers(siteqnumsj);
			      //cout << " Sz~j = "<<Sztildej << " ssj =  "<< sitestatej << endl;
			      int i1=0;
			      for (double sigma=-0.5;sigma<=0.5;sigma+=1.0)
				{
			      siteqnumsi[2]=Sztildej-sigma;
			      int sitestatei=SingleSite.GetBlockFromQNumbers(siteqnumsi);
			      //cout << " Sz~i = "<<Sztildej-sigma << " ssi =  "<< sitestatei << " sigma = " << sigma << endl;

			      auxCG[0]=CGordan(Scfi,Si-Sztildej+sigma,Stildei,Sztildej-sigma,Si,Si);
			      auxCG[1]=CGordan(Scfj,Sj-Sztildej,Stildej,Sztildej,Sj,Sj);
			      auxBasis[i1]+=fd_table((int)(sigma/0.5),sitestatej,sitestatei)*auxCG[0]*auxCG[1];

			      //cout << " CG 1 = " << auxCG[0] << " CG 2 = " << auxCG[1] << " mat_el = " <<  fd_table((int)(sigma/0.5),sitestatej,sitestatei) << endl;
			      i1++;
				}
			    }

			  // Check to see which of them is non-zero
			  auxCG[0]=CGordan(Si,Si,0.5,-0.5,Sj,Sj);
			  auxCG[1]=CGordan(Si,Si,0.5,0.5,Sj,Sj);
			  if ( !(dEqual(auxCG[0],0.0)) )
			  fnbasis(istbl,jstbl)=auxBasis[0]/auxCG[0];
			  else
			    {
			    if ( !(dEqual(auxCG[1],0.0)) )
			      fnbasis(istbl,jstbl)=auxBasis[1]/auxCG[1];
			    else fnbasis(istbl,jstbl)=0.0;
			    }

			}
		      // END if sci=scf

		      //cout << "fn(" << istbl << "," << jstbl << ") = ";
		      //cout << fnbasis(istbl,jstbl) << endl;

		      jstbl++;
		    }
		  istbl++;

		}

	      cout << "    ...fnbasis done." << endl;

	      // Multiply the 3 of them. Zi has eigvecs in ROWS!

	      cout << "    Multiplying BLAS matrices... " << endl;

//  	      boost::numeric::ublas::matrix<double> Mtemp=prod(fnbasis,trans(Zjbl));
//  	      boost::numeric::ublas::matrix<double> fnw=prod (Zibl,Mtemp);
// 	      fnw=prod (Zibl,Mtemp);

	      //Mtemp.resize(Nstibl,Nstjbl);
	      fnw.resize(Nstibl,Nstjbl);
 	      //noalias(Mtemp)=prod(fnbasis,trans(Zjbl));
	      //cout << "    ...one done..." << endl;
 	      //noalias(fnw)=prod (Zibl, Mtemp );

	      t.restart();
 	      noalias(fnw)=prod (Zibl,
			boost::numeric::ublas::matrix<double>(prod(fnbasis,trans(Zjbl))) );
	      time_elapsed=t.elapsed();
	      cout << "    ...done. Elapsed time:" << time_elapsed << endl;

   	      //cout << "Z(i="<<ii<<")  : " <<  Zibl << endl;
   	      //cout << "Z(j="<<jj<<")  : " <<  Zjbl << endl;
    	      //cout << "fbasis   :" <<  fnbasis << endl;
    	      //cout << "Zi.fbasis.ZjT : " <<  fnw << endl;

	      // Add to Qm1fNQ
	      for (int ii=0;ii<fnw.size1();ii++)
		{
		  for (int jj=0;jj<fnw.size2();jj++)
		    {
		      Qm1fNQ.MatEl.push_back(fnw(ii,jj));
		    }
		}

	    }
	}
      //Loop in jblock
    }
  // Loop in iblock

}


#include <fast_linalg/cblas.h>
#include <fast_linalg/lapacke.h>
#include <boost/dll/import.hpp>
#include <boost/dll/shared_library.hpp>

namespace fast_linalg {

  typedef lapack_int(LAPACKE_dpftrf_t)(int, char, char,
    lapack_int, double *);
  typedef lapack_int(LAPACKE_spftrf_t)(int, char, char,
    lapack_int, float *);
  typedef lapack_int(LAPACKE_dsfrk_t) (int, char, char, char,
    lapack_int, lapack_int, double, const double *,
    lapack_int, double, double *);
  typedef lapack_int(LAPACKE_ssfrk_t) (int, char, char, char,
    lapack_int, lapack_int, double, const float *,
    lapack_int, double, float *);
  typedef lapack_int(LAPACKE_dtfttp_t) (int, char, char,
    lapack_int, const double *, double *);
  typedef lapack_int(LAPACKE_stfttp_t) (int, char, char,
    lapack_int, const float *, float *);
  typedef lapack_int(LAPACKE_dtpttf_t) (int, char, char,
    lapack_int, const double *, double *);
  typedef lapack_int(LAPACKE_stpttf_t) (int, char, char,
    lapack_int, const float *, float *);
  typedef lapack_int(LAPACKE_dpftri_t)(int, char, char,
    lapack_int, double *);
  typedef lapack_int(LAPACKE_spftri_t)(int, char, char,
    lapack_int, float *);

  typedef void (cblas_ssyr_t)(int, int, int, float, const float *,
    int, float *, int);
  typedef void (cblas_dsyr_t)(int, int, int, double, const double *,
    int, double *, int);

  typedef void (cblas_sspr_t)(int, int, int, float, const float*,
    int, float*);
  typedef void (cblas_dspr_t)(int, int, int, double, const double*,
    int, double*);

  typedef void (cblas_ssyrk_t)(int, int, int,
    int, int, float, const float *, int, float, float *, int);
  typedef void (cblas_dsyrk_t)(int, int, int,
    int, int, double, const double *, int, double, double *, int);

  typedef int (openblas_get_num_threads_t)();
  typedef int (openblas_get_num_procs_t)();
  typedef void (openblas_set_num_threads_t)(int);
  typedef char* (openblas_get_corename_t)();
  typedef char* (openblas_get_config_t)();

  namespace dll = boost::dll;

  class Wrapper {
    dll::shared_library lib;
    void init_(std::string lib_name) {
      lib = dll::shared_library(lib_name, dll::load_mode::search_system_folders);
      LAPACKE_dpftrf = &lib.get<LAPACKE_dpftrf_t>("LAPACKE_dpftrf");
      LAPACKE_spftrf = &lib.get<LAPACKE_spftrf_t>("LAPACKE_spftrf");
      LAPACKE_dsfrk = &lib.get<LAPACKE_dsfrk_t>("LAPACKE_dsfrk");
      LAPACKE_ssfrk = &lib.get<LAPACKE_ssfrk_t>("LAPACKE_ssfrk");
      LAPACKE_dtfttp = &lib.get<LAPACKE_dtfttp_t>("LAPACKE_dtfttp");
      LAPACKE_stfttp = &lib.get<LAPACKE_stfttp_t>("LAPACKE_stfttp");
      LAPACKE_dtpttf = &lib.get<LAPACKE_dtpttf_t>("LAPACKE_dtpttf");
      LAPACKE_stpttf = &lib.get<LAPACKE_stpttf_t>("LAPACKE_stpttf");
      LAPACKE_dpftri = &lib.get<LAPACKE_dpftri_t>("LAPACKE_dpftri");
      LAPACKE_spftri = &lib.get<LAPACKE_spftri_t>("LAPACKE_spftri");

      openblas_get_num_threads = &lib.get<openblas_get_num_threads_t>(
        "openblas_get_num_threads");
      openblas_get_num_procs = &lib.get<openblas_get_num_procs_t>(
        "openblas_get_num_procs");
      openblas_set_num_threads = &lib.get<openblas_set_num_threads_t>(
        "openblas_set_num_threads");
      openblas_get_corename = &lib.get<openblas_get_corename_t>(
        "openblas_get_corename");
      openblas_get_config = &lib.get<openblas_get_config_t>(
        "openblas_get_config");

      cblas_ssyr = &lib.get<cblas_ssyr_t>("cblas_ssyr");
      cblas_dsyr = &lib.get<cblas_dsyr_t>("cblas_dsyr");

      cblas_sspr = &lib.get<cblas_sspr_t>("cblas_sspr");
      cblas_dspr = &lib.get<cblas_dspr_t>("cblas_dspr");

      cblas_ssyrk = &lib.get<cblas_ssyrk_t>("cblas_ssyrk");
      cblas_dsyrk = &lib.get<cblas_dsyrk_t>("cblas_dsyrk");
    }
    void init_() {
      LAPACKE_dpftrf = 0;
      LAPACKE_spftrf = 0;
      LAPACKE_dsfrk = 0;
      LAPACKE_ssfrk = 0;
      LAPACKE_dtfttp = 0;
      LAPACKE_stfttp = 0;
      LAPACKE_dtpttf = 0;
      LAPACKE_stpttf = 0;
      LAPACKE_dpftri = 0;
      LAPACKE_spftri = 0;

      openblas_get_num_threads = 0;
      openblas_get_num_procs = 0;
      openblas_set_num_threads = 0;
      openblas_get_corename = 0;
      openblas_get_config = 0;

      cblas_ssyr = 0;
      cblas_dsyr = 0;

      cblas_sspr = 0;
      cblas_dspr = 0;

      cblas_ssyrk = 0;
      cblas_dsyrk = 0;
    }
  public:
    LAPACKE_dpftrf_t *LAPACKE_dpftrf;
    LAPACKE_spftrf_t *LAPACKE_spftrf;
    LAPACKE_dsfrk_t *LAPACKE_dsfrk;
    LAPACKE_ssfrk_t *LAPACKE_ssfrk;
    LAPACKE_dtfttp_t *LAPACKE_dtfttp;
    LAPACKE_stfttp_t *LAPACKE_stfttp;
    LAPACKE_dtpttf_t *LAPACKE_dtpttf;
    LAPACKE_stpttf_t *LAPACKE_stpttf;
    LAPACKE_dpftri_t *LAPACKE_dpftri;
    LAPACKE_spftri_t *LAPACKE_spftri;
    openblas_get_num_threads_t *openblas_get_num_threads;
    openblas_get_num_procs_t *openblas_get_num_procs;
    openblas_set_num_threads_t *openblas_set_num_threads;
    openblas_get_corename_t *openblas_get_corename;
    openblas_get_config_t *openblas_get_config;

    cblas_ssyr_t *cblas_ssyr;
    cblas_dsyr_t *cblas_dsyr;

    cblas_sspr_t* cblas_sspr;
    cblas_dspr_t* cblas_dspr;

    cblas_ssyrk_t *cblas_ssyrk;
    cblas_dsyrk_t *cblas_dsyrk;

    static Wrapper &instance() {
      SCITBX_ASSERT(initialised());
      return *instance_();
    }

    Wrapper() {
      SCITBX_ASSERT(instance_() == 0);
      init_();
      instance_() = this;
    }

    void initialise(const std::string &lib_name) {
      SCITBX_ASSERT(instance_() != 0);
      init_(lib_name);
    }

    static bool initialised() {
      Wrapper *w = instance_();
      return w != 0 && w->LAPACKE_dpftrf != 0;
    }
    // for internal use only!
    static Wrapper *&instance_() {
      static Wrapper *inst = 0;
      return inst;
    }
  };
}

using namespace fast_linalg;

bool is_fast_linalg_initialised() {
  return Wrapper::initialised();
}
//............................................................................
void initialise_fast_linalg(const std::string &lib_name) {
  if (Wrapper::instance_() == 0) {
    new Wrapper();
  }
  Wrapper::instance_()->initialise(lib_name);
}
//............................................................................
void finalise_fast_linalg() {
  if (Wrapper::instance_() != 0) {
    delete Wrapper::instance_();
    Wrapper::instance_() = 0;
  }
}
//............................................................................
//............................................................................
lapack_int lapack_pftrf(int matrix_order, char transr, char uplo,
    lapack_int n, double* a)
  {
  return (*Wrapper::instance().LAPACKE_dpftrf)(
    matrix_order, transr, uplo, n, a);
}
//............................................................................
lapack_int lapack_spftrf(int matrix_order, char transr, char uplo,
    lapack_int n, float* a)
{
  return (*Wrapper::instance().LAPACKE_spftrf)(
    matrix_order, transr, uplo, n, a);
}
//............................................................................
//............................................................................
lapack_int lapack_sfrk(int matrix_order, char transr, char uplo, char trans,
    lapack_int n, lapack_int k, double alpha,
    const double* a, lapack_int lda, double beta,
    double* c)
{
  return (*Wrapper::instance().LAPACKE_dsfrk)(
    matrix_order, transr, uplo, trans,
    n, k, alpha,
    a, lda, beta,
    c);
}
//............................................................................
lapack_int lapack_ssfrk(int matrix_order, char transr, char uplo, char trans,
    lapack_int n, lapack_int k, float alpha,
    const float* a, lapack_int lda, float beta,
    float* c)
{
  return (*Wrapper::instance().LAPACKE_ssfrk)(
    matrix_order, transr, uplo, trans,
    n, k, alpha,
    a, lda, beta,
    c);
}
//............................................................................
//............................................................................
lapack_int lapack_tfttp(int matrix_order, char transr, char uplo,
    lapack_int n, const double* arf, double* ap)
{
  return (*Wrapper::instance().LAPACKE_dtfttp)(
    matrix_order, transr, uplo, n, arf, ap);
}
//............................................................................
lapack_int lapack_stfttp(int matrix_order, char transr, char uplo,
    lapack_int n, const float* arf, float* ap)
{
  return (*Wrapper::instance().LAPACKE_stfttp)(
    matrix_order, transr, uplo, n, arf, ap);
}
//............................................................................
//............................................................................
lapack_int lapack_tpttf(int matrix_order, char transr, char uplo,
    lapack_int n, const double* ap, double* arf)
{
  return (*Wrapper::instance().LAPACKE_dtpttf)(
    matrix_order, transr, uplo, n, ap, arf);
}
//............................................................................
lapack_int lapack_stpttf(int matrix_order, char transr, char uplo,
    lapack_int n, const float* ap, float* arf)
{
  return (*Wrapper::instance().LAPACKE_stpttf)(
    matrix_order, transr, uplo, n, ap, arf);
}
//............................................................................
//............................................................................
lapack_int lapack_pftri(int matrix_order, char transr, char uplo,
    lapack_int n, double* a)
{
  return (*Wrapper::instance().LAPACKE_dpftri)(
    matrix_order, transr, uplo, n, a);
}
//............................................................................
lapack_int lapack_spftri(int matrix_order, char transr, char uplo,
    lapack_int n, float* a)
{
  return (*Wrapper::instance().LAPACKE_spftri)(
    matrix_order, transr, uplo, n, a);
}
//............................................................................
//............................................................................
void cblas_sspr(int Order, int Uplo, int N, float alpha,
  const float *X, int incX, float *A)
{
  return (*Wrapper::instance().cblas_sspr)(Order, Uplo, N, alpha,
    X, incX, A);
}
//............................................................................
void cblas_dspr(int Order, int Uplo, int N, double alpha,
  const double *X, int incX, double *A)
{
  return (*Wrapper::instance().cblas_dspr)(Order, Uplo, N, alpha,
    X, incX, A);
}
//............................................................................
//............................................................................
void cblas_ssyr(int Order, int Uplo, int N, float alpha,
  const float* X, int incX, float* A, int lda)
{
  return (*Wrapper::instance().cblas_ssyr)(Order, Uplo, N, alpha,
    X, incX, A, lda);
}
//............................................................................
void cblas_dsyr(int Order, int Uplo, int N, double alpha,
  const double* X, int incX, double* A, int lda)
{
  return (*Wrapper::instance().cblas_dsyr)(Order, Uplo, N, alpha,
    X, incX, A, lda);
}
//............................................................................
//............................................................................
void cblas_ssyrk(int Order, int Uplo, int Trans, int N, int K,
  float alpha, const float *A, int lda, float beta, float *C, int ldc)
{
  return (*Wrapper::instance().cblas_ssyrk)(Order, Uplo, Trans, N, K, alpha,
    A, lda, beta, C, ldc);
}
//............................................................................
void cblas_dsyrk(int Order, int Uplo, int Trans, int N, int K,
  double alpha, const double *A, int lda, double beta, double *C, int ldc)
{
  return (*Wrapper::instance().cblas_dsyrk)(Order, Uplo, Trans, N, K, alpha,
    A, lda, beta, C, ldc);
}
//............................................................................
//............................................................................
int openblas_get_num_threads() {
  return (*Wrapper::instance().openblas_get_num_threads)();
}
//............................................................................
int openblas_get_num_procs() {
  return (*Wrapper::instance().openblas_get_num_procs)();
}
//............................................................................
void openblas_set_num_threads(int n) {
  (*Wrapper::instance().openblas_set_num_threads)(n);
}
//............................................................................
char* openblas_get_corename() {
  return (*Wrapper::instance().openblas_get_corename)();
}
//............................................................................
char* openblas_get_config() {
  return (*Wrapper::instance().openblas_get_config)();
}
//............................................................................


/*
 * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: areas.cpp 21675 2009-08-12 19:40:12Z jfaustwg $
 *
 */

/** \author Radu Bogdan Rusu */

#include <point_cloud_mapping/geometry/point.h>
#include <point_cloud_mapping/geometry/areas.h>
#include <point_cloud_mapping/geometry/distances.h>
#include <point_cloud_mapping/geometry/transforms.h>
#include <point_cloud_mapping/geometry/intersections.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <cfloat>
#include <algorithm>

namespace cloud_geometry
{

  namespace areas
  {
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the area of a 2D planar polygon patch - using a given normal
      * \param points the point cloud (planar)
      * \param normal the plane normal
      */
    double
      compute2DPolygonalArea (const sensor_msgs::PointCloud &points, const std::vector<double> &normal)
    {
      int k0, k1, k2;

      // Find axis with largest normal component and project onto perpendicular plane
      k0 = (fabs (normal.at (0) ) > fabs (normal.at (1))) ? 0  : 1;
      k0 = (fabs (normal.at (k0)) > fabs (normal.at (2))) ? k0 : 2;
      k1 = (k0 + 1) % 3;
      k2 = (k0 + 2) % 3;

      // cos(theta), where theta is the angle between the polygon and the projected plane
      double ct = fabs ( normal.at (k0) );

      double area = 0;
      float p_i[3], p_j[3];

      for (unsigned int i = 0; i < points.points.size (); i++)
      {
        p_i[0] = points.points[i].x; p_i[1] = points.points[i].y; p_i[2] = points.points[i].z;
        int j = (i + 1) % points.points.size ();
        p_j[0] = points.points[j].x; p_j[1] = points.points[j].y; p_j[2] = points.points[j].z;

        area += p_i[k1] * p_j[k2] - p_i[k2] * p_j[k1];
      }
      area = fabs (area) / (2 * ct);

      return (area);
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the area of a 2D planar polygon patch - using a given normal
      * \param polygon the planar polygon
      * \param normal the plane normal
      */
    double
      compute2DPolygonalArea (const geometry_msgs::Polygon &polygon, const std::vector<double> &normal)
    {
      int k0, k1, k2;

      // Find axis with largest normal component and project onto perpendicular plane
      k0 = (fabs (normal.at (0) ) > fabs (normal.at (1))) ? 0  : 1;
      k0 = (fabs (normal.at (k0)) > fabs (normal.at (2))) ? k0 : 2;
      k1 = (k0 + 1) % 3;
      k2 = (k0 + 2) % 3;

      // cos(theta), where theta is the angle between the polygon and the projected plane
      double ct = fabs ( normal.at (k0) );

      double area = 0;
      float p_i[3], p_j[3];

      for (unsigned int i = 0; i < polygon.points.size (); i++)
      {
        p_i[0] = polygon.points[i].x; p_i[1] = polygon.points[i].y; p_i[2] = polygon.points[i].z;
        int j = (i + 1) % polygon.points.size ();
        p_j[0] = polygon.points[j].x; p_j[1] = polygon.points[j].y; p_j[2] = polygon.points[j].z;

        area += p_i[k1] * p_j[k2] - p_i[k2] * p_j[k1];
      }
      area = fabs (area) / (2 * ct);

      return (area);
    }

    bool compute2DPolygonNormal(const geometry_msgs::Polygon &poly, std::vector<double> &normal)
    {
      int k0,k1,k2;
      int sz=poly.points.size();
      if(sz<3)
	return false;

      k0=0;
      k1=sz/3;
      if(k1<=k0) k1=k0+1;
      k2=(2*sz)/3;
      if(k2<=k1) k2=k1+1;

      if(k2>=sz) return false;

      Eigen::Vector3d p1, p2, p3;

      p1 (0) = poly.points[k0].x-poly.points[k2].x;
      p1 (1) = poly.points[k0].y-poly.points[k2].y;
      p1 (2) = poly.points[k0].z-poly.points[k2].z;

      p2 (0) = poly.points[k1].x-poly.points[k2].x;
      p2 (1) = poly.points[k1].y-poly.points[k2].y;
      p2 (2) = poly.points[k1].z-poly.points[k2].z;

      p3 = p1.cross (p2);
      p3.normalize();

      normal.resize(3);
      normal[0]=p3(0);
      normal[1]=p3(1);
      normal[2]=p3(2);
      return true;
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute the area of a 2D planar polygon patch. The normal is computed automatically.
      * \param polygon the planar polygon
      * \param normal the plane normal
      */
    double
      compute2DPolygonalArea (const geometry_msgs::Polygon &polygon)
    {
      std::vector<double> normal;
      if(!compute2DPolygonNormal(polygon,normal))
	return 0; //the polygon is degenerate, so its area is 0;

      return compute2DPolygonalArea (polygon,normal);

    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute a 2D convex hull in 3D space using Andrew's monotone chain algorithm
      * \param points the point cloud
      * \param indices the point indices to use from the cloud (they must form a planar model)
      * \param coeff the *normalized* planar model coefficients
      * \param hull the resultant convex hull model as a \a Polygon3D
      */
    void
      convexHull2D (const sensor_msgs::PointCloud &points, const std::vector<int> &indices, const std::vector<double> &coeff,
                    geometry_msgs::Polygon &hull)
    {
      // Copy the point data to a local Eigen::Matrix. This is slow and should be replaced by extending geometry_msgs::Point32
      // to allow []/() accessors.
      std::vector<Eigen::Vector3f> epoints (indices.size ());
      for (unsigned int cp = 0; cp < indices.size (); cp++)
      {
        epoints[cp](0) = points.points[indices.at (cp)].x;
        epoints[cp](1) = points.points[indices.at (cp)].y;
        epoints[cp](2) = points.points[indices.at (cp)].z;
      }

      // Determine the best plane to project points onto
      int k0, k1, k2;
      k0 = (fabs (coeff.at (0) ) > fabs (coeff.at (1))) ? 0  : 1;
      k0 = (fabs (coeff.at (k0)) > fabs (coeff.at (2))) ? k0 : 2;
      k1 = (k0 + 1) % 3;
      k2 = (k0 + 2) % 3;

      // Compute a 2D centroid for two dimensions
      Eigen::Vector2d centroid (0, 0);
      for (unsigned int cp = 0; cp < epoints.size (); cp++)
      {
        centroid (0) += epoints[cp](k1);
        centroid (1) += epoints[cp](k2);
      }
      centroid (0) /= epoints.size ();
      centroid (1) /= epoints.size ();

      // Push projected centered 2d points
      std::vector<geometry_msgs::Point32> epoints_demean (epoints.size ());
      for (unsigned int cp = 0; cp < indices.size (); cp++)
      {
        epoints_demean[cp].x = epoints[cp](k1) - centroid (0);
        epoints_demean[cp].y = epoints[cp](k2) - centroid (1);
      }

      std::sort (epoints_demean.begin (), epoints_demean.end (), comparePoint2D);

      geometry_msgs::Polygon hull_2d;
      convexHull2D (epoints_demean, hull_2d);

      int nr_points_hull = hull_2d.points.size ();
      if (nr_points_hull >= 3)
      {
        // Determine the convex hull direction
        Eigen::Vector3d p1, p2, p3;

        p1 (k0) = 0;
        p1 (k1) = -hull_2d.points[0].x + hull_2d.points[1].x;
        p1 (k2) = -hull_2d.points[0].y + hull_2d.points[1].y;

        p2 (k0) = 0;
        p2 (k1) = -hull_2d.points[0].x + hull_2d.points[2].x;
        p2 (k2) = -hull_2d.points[0].y + hull_2d.points[2].y;

        p3 = p1.cross (p2);

        bool direction = (p3 (k0) * coeff.at (k0) > 0);

        // Create the Polygon3D object
        hull.points.resize (nr_points_hull);

        // Copy hull points in clockwise or anti-clockwise format
        for (int cp = 0; cp < nr_points_hull; cp++)
        {
          int d = direction ? cp : (nr_points_hull - cp - 1);
          Eigen::Vector3f pt;
          pt (k1) = hull_2d.points[cp].x + centroid (0);
          pt (k2) = hull_2d.points[cp].y + centroid (1);
          pt (k0) = -(coeff.at (3) + pt (k1) * coeff.at (k1) + pt (k2) * coeff.at (k2)) / coeff.at (k0);

          // Copy the point data to Polygon3D format
          hull.points[d].x = pt (0);
          hull.points[d].y = pt (1);
          hull.points[d].z = pt (2);
        }
      }
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Compute a 2D convex hull using Andrew's monotone chain algorithm
      * \note (code snippet inspired from http://www.softsurfer.com/Archive/algorithm_0109/algorithm_0109.htm)
      *        Copyright 2001, softSurfer (www.softsurfer.com)
      * \param points the 2D projected point cloud representing a planar model
      * \param hull the resultant 2D convex hull model as a \a Polygon
      */
    void
      convexHull2D (const std::vector<geometry_msgs::Point32> &points, geometry_msgs::Polygon &hull)
    {
      int nr_points = points.size ();
      hull.points.resize (nr_points + 1);

      // Indices for bottom and top of the stack
      int bot = 0, top = -1;
      int i;

      for (i = 1; i < nr_points; i++)
        // points[0].x represents the smallest X coordinate
        if (points[i].x != points[0].x)
          break;

      // Get the indices of points with min|max y-coord
      int minmax = i - 1;

      // Degenerate case: all x-coords == xmin
      if ( minmax == (nr_points - 1) )
      {
        ++top;
        hull.points[top].x = points[0].x;
        hull.points[top].y = points[0].y;
        hull.points[top].z = points[0].z;
        // A nontrivial segment
        if (points[minmax].y != points[0].y)
        {
          ++top;
          hull.points[top].x = points[minmax].x;
          hull.points[top].y = points[minmax].y;
          hull.points[top].z = points[minmax].z;
        }
        ++top;
        // Add the polygon's endpoint
        hull.points[top].x = points[0].x;
        hull.points[top].y = points[0].y;
        hull.points[top].z = points[0].z;
        hull.points.resize (top + 1);
        return;
      }

      int maxmin;
      for (i = nr_points - 2; i >= 0; i--)
        if (points[i].x != points[nr_points - 1].x)
          break;
      maxmin = i + 1;

      // Compute the lower hull
      ++top;
      // Add the polygon's endpoint
      hull.points[top].x = points[0].x;
      hull.points[top].y = points[0].y;
      hull.points[top].z = points[0].z;

      i = minmax;
      while (++i <= maxmin)
      {
        // The lower line joins P[minmin] with P[maxmin]
        if ((i < maxmin) && (
            (points[maxmin].x - points[0].x) * (points[i].y      - points[0].y) -
            (points[i].x      - points[0].x) * (points[maxmin].y - points[0].y) >= 0))
          continue;          // ignore P[i] above or on the lower line

        // If there are at least 2 points on the stack
        while (top > 0)
        {
          // Test if P[i] is left of the line at the stack top
          if ((hull.points[top].x - hull.points[top-1].x) * (points[i].y        - hull.points[top-1].y) -
              (points[i].x        - hull.points[top-1].x) * (hull.points[top].y - hull.points[top-1].y) > 0)
            break;         // P[i] is a new hull vertex
          else
            top--;         // pop top point off stack
        }
        ++top;
        hull.points[top].x = points[i].x;
        hull.points[top].y = points[i].y;
        hull.points[top].z = points[i].z;
      }

      // Next, compute the upper hull above the bottom hull
      if ((nr_points - 1) != maxmin)      // if distinct xmax points
      {
        ++top;
        // Add the point with max X and max Y coordinates to the hull
        hull.points[top].x = points[nr_points - 1].x;
        hull.points[top].y = points[nr_points - 1].y;
        hull.points[top].z = points[nr_points - 1].z;
      }
      // The bottom point of the upper hull stack
      bot = top;

      i = maxmin;
      while (--i >= minmax)
      {
        // The upper line joins P[nr_points - 1] with P[minmax]
        if ((i > minmax) && (
            (points[minmax].x - points[nr_points - 1].x) * (points[i].y      - points[nr_points - 1].y) -
            (points[i].x      - points[nr_points - 1].x) * (points[minmax].y - points[nr_points - 1].y) >= 0))
          continue;          // ignore P[i] below or on the upper line

        // If there are at least 2 points on the stack
        while (top > bot)
        {
          // Test if P[i] is left of the line at the stack top
          if ((hull.points[top].x - hull.points[top-1].x) * (points[i].y        - hull.points[top-1].y) -
              (points[i].x        - hull.points[top-1].x) * (hull.points[top].y - hull.points[top-1].y) > 0)
            break;         // P[i] is a new hull vertex
          else
            top--;         // pop top point off stack
        }
        ++top;

        hull.points[top].x = points[i].x;
        hull.points[top].y = points[i].y;
        hull.points[top].z = points[i].z;
      }

      if (minmax != 0)
      {
        ++top;
        // Add the polygon's endpoint
        hull.points[top].x = points[0].x;
        hull.points[top].y = points[0].y;
        hull.points[top].z = points[0].z;
      }
      hull.points.resize (top + 1);
      return;
    }

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    /** \brief Check if a 2d point (X and Y coordinates considered only!) is inside or outisde a given polygon
      * \note (This is highly optimized code taken from http://www.visibone.com/inpoly/)
      *       Copyright (c) 1995-1996 Galacticomm, Inc.  Freeware source code.
      * \param point a 3D point projected onto the same plane as the polygon
      * \param polygon a polygon
      */
    bool
      isPointIn2DPolygon (const geometry_msgs::Point32 &point, const geometry_msgs::Polygon &polygon)
    {
      bool in_poly = false;
      double x1, x2, y1, y2;

      int nr_poly_points = polygon.points.size ();
      double xold = polygon.points[nr_poly_points - 1].x;
      double yold = polygon.points[nr_poly_points - 1].y;
      for (int i = 0; i < nr_poly_points; i++)
      {
        double xnew = polygon.points[i].x;
        double ynew = polygon.points[i].y;
        if (xnew > xold)
        {
          x1 = xold;
          x2 = xnew;
          y1 = yold;
          y2 = ynew;
        }
        else
        {
          x1 = xnew;
          x2 = xold;
          y1 = ynew;
          y2 = yold;
        }

        if ( (xnew < point.x) == (point.x <= xold) && (point.y - y1) * (x2 - x1) < (y2 - y1) * (point.x - x1) )
        {
          in_poly = !in_poly;
        }
        xold = xnew;
        yold = ynew;
      }
      return (in_poly);
    }

  }
}


// Boost.Polygon library skeleton_predicates_test.cpp file

//          Copyright Andrii Sydorchuk 2012.
//          Copyright Lucanus Simonson 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#define BOOST_TEST_MODULE skeleton_predicates_test

#include <boost/test/test_case_template.hpp>
#include <boost/polygon/detail/skeleton_predicates.hpp>
using boost::polygon::detail::int32;
using boost::polygon::detail::fpt64;
using boost::polygon::detail::intersection_event;
using boost::polygon::detail::skeleton_ctype_traits;
using boost::polygon::detail::skeleton_predicates;

#include <boost/polygon/point_data.hpp>
#include <boost/polygon/segment_data.hpp>
using boost::polygon::point_data;
using boost::polygon::segment_data;

typedef point_data<int32> point_type;
typedef segment_data<int32> segment_type;
typedef skeleton_ctype_traits<int32> ctraits_type;
typedef intersection_event<fpt64> event_type;
skeleton_predicates<ctraits_type>::event_formation_predicate<
    segment_data<int32>, intersection_event<fpt64> > create_event;

BOOST_AUTO_TEST_CASE(event_predicate_test1) {
  point_type point1(0, 0);
  point_type point2(0, 4);
  point_type point3(4, 4);
  point_type point4(4, 0);
  segment_type segment1(point1, point2);
  segment_type segment2(point2, point3);
  segment_type segment3(point3, point4);
  event_type e;
  BOOST_CHECK(create_event(segment1, segment2, segment3, &e));
  BOOST_CHECK_EQUAL(e.x(), 2);
  BOOST_CHECK_EQUAL(e.y(), 2);
  BOOST_CHECK_EQUAL(e.r(), 2);
}

BOOST_AUTO_TEST_CASE(event_predicate_test2) {
  point_type point1(0, 0);
  point_type point2(0, 4);
  point_type point3(4, 4);
  point_type point4(4, 0);
  segment_type segment1(point4, point3);
  segment_type segment2(point3, point2);
  segment_type segment3(point2, point1);
  event_type e;
  BOOST_CHECK(create_event(segment1, segment2, segment3, &e));
  BOOST_CHECK_EQUAL(e.x(), 2);
  BOOST_CHECK_EQUAL(e.y(), 2);
  BOOST_CHECK_EQUAL(e.r(), 2);
}