/*

Program that compares lines from ancestor and endpoint common lines file and estimates mut rate.
Note that these input files are preprocessed and derived from pileups. (ATCG data)
We have two files with the same number of lines and the two files correspond line by line.

This program takes 3 argument: extant ancestor_common_processed_pileup endpoint_common_processed_pileup

	extant: for defining minimum mut_freq to consider endpoint as muted

	extant must be in {8, 12, 16, 20, 24, 28, 32, 48, 75, 100}

This program, for all couples of lines in the two files:

1) Reads one line from ancestor file and one from endpoint;
2) Parses them to get muted frequency and coverage;
3) Applies thresholds and in case skips lines;
4) Checks if lines that are fine with thresholds are muted or not.

Tresholds:

	- For both files discards lines with reference base different from A, T, C, G

	- For ancestor discard lines with
		 coverage < 100
		 mut_freq > 0.0

	- For endpoint discard lines with:
		mut_freq > 0.2
		unbalanced forward and reverse muted reads.

Counts base as muted if endpoint frequency

	f >= Cutoff(Coverage, f_min) e Coverage = TotCoverage/3

	cutoff (Coverage, f_min) = f_min + alpha/Sqrt[Coverage]

based on extant f_min and corresponding alphas are

	f_min = {1/8, 1/12, 1/16, 1/20, 1/24, 1/28, 1/32, 1/48, 1/75, 1/100}

	alpha = {0.52, 0.45, 0.36, 0.32, 0.3, 0.28, 0.25, 0.2, 0.18, 0.15}
*/

#include <iostream>
#include <string>
#include <sstream>
#include <fstream>
#include <cassert>
#include <cmath>
#include <algorithm> // std::max(a,b)

//Boost
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/gzip.hpp>

using namespace std;

double compute_mut_rate(double P_hat, double death_prob, int extant);

bool endpoint_muted(float mut_freq, int total_coverage, int extant);

int main(int argc, char** argv){

	//*******************************************
	//          COMMAND LINE ARGS
	//*******************************************

	if (argc != 4){
		cerr << "Usage: " << argv[0] << " extant ancestor_common_lines_file endpoint_common_lines_file" << endl;
		return -1;
	}

	int extant = atoi(argv[1]);

	string ancestor_file = argv[2];
	string endpoint_file = argv[3];

	if (extant != 8 && extant != 12 &&extant != 16 && extant != 20 && extant != 24 && extant != 28 && extant != 32 && extant != 48 && extant != 75 && extant != 100){
		cerr << "Extant must be of following: {8, 12, 16, 20, 24, 28, 32, 48, 75, 100}"
		<< endl;
		return -1;
	}

	//*******************************************
	//          FIXED THRESHOLDS
	//*******************************************
	//ancestor
	int coverage_min_ancestor = 100;
	float freq_max_ancestor_confirm_reference = 0.;
	//endpoint
	float endpoint_freq_max = 0.2;

	//*******************************************
	//          COMPRESSED INPUT
	//*******************************************

	//open and unzip ancestor file
	ifstream file_1(ancestor_file, ios_base::in | ios_base::binary);

	if (file_1.is_open() == false) {
		cerr << "Ancestor not opened: exit!" <<endl;
		return -1;
	}

	//uncompress
    	boost::iostreams::filtering_streambuf<boost::iostreams::input> file_1_inbuf;
    	file_1_inbuf.push(boost::iostreams::gzip_decompressor());
    	file_1_inbuf.push(file_1);
    	//Convert streambuf to istream
    	istream file_1_instream(&file_1_inbuf);

	cout << endl << "Ancestor file opened." << endl;

	//open and unzip endpoint file
	ifstream file_2(endpoint_file, ios_base::in | ios_base::binary);

	if (file_2.is_open() == false) {
		cerr << "Endopint file not opened: exit!" <<endl;
		return -1;
	}

	//uncompress
    	boost::iostreams::filtering_streambuf<boost::iostreams::input> file_2_inbuf;
    	file_2_inbuf.push(boost::iostreams::gzip_decompressor());
    	file_2_inbuf.push(file_2);
    	//Convert streambuf to istream
    	istream file_2_instream(&file_2_inbuf);

	cout << "Endpoint file opened." << endl << endl;

	//*******************************************
	//          PARSING VARIABLES
	//*******************************************
	//Ancestor parsing variables-------------
	string ancestor_line; //line of the file to parse
	string ancestor_chromosome; //pos
	int ancestor_chromosome_number = 0;
	long int ancestor_base_number = 0; //1-based
	string ancestor_ATCG_data;
	char ancestor_reference;
	int ancestor_A_counter = 0;
	int ancestor_T_counter = 0;
	int ancestor_C_counter = 0;
	int ancestor_G_counter = 0;
	int ancestor_a_counter = 0;
	int ancestor_t_counter = 0;
	int ancestor_c_counter = 0;
	int ancestor_g_counter = 0;
	int ancestor_coverage = 0;
	double ancestor_mut_freq = 0;

	//Endpoint parsing variables-------------
	string endpoint_line; //line of the file to parse
	string endpoint_chromosome;
	int endpoint_chromosome_number = 0;
	long int endpoint_base_number = 0;
	string endpoint_ATCG_data;
	char endpoint_reference;
	int endpoint_A_counter = 0;
	int endpoint_T_counter = 0;
	int endpoint_C_counter = 0;
	int endpoint_G_counter = 0;
	int endpoint_a_counter = 0;
	int endpoint_t_counter = 0;
	int endpoint_c_counter = 0;
	int endpoint_g_counter = 0;
	int endpoint_coverage = 0;
	double endpoint_mut_freq = 0;
	char endpoint_muted_in;

	//*******************************************
	//          OTHERS
	//*******************************************
	long int count_line = 0; //all lines in file
	long int discarded_lines = 0; //not matching thresholds
	long int muted_bases = 0;
	long int not_muted_bases = 0;
	double P_hat;

	//******************************************
	//--------------------------------------
	// 		    START
	//
	// read line by line both ancestor and endpoint
	// check thresholds and count muted lines
	//--------------------------------------
	//******************************************

	cout << "Start reading files." << endl << endl;

	//Loop for all line in file 2 (endpoint)
	while (getline(file_2_instream, endpoint_line)) { //until file_2 EOF

		//reset counters
		endpoint_coverage = 0;
		ancestor_coverage = 0;

		//*******************************************
		// READ AND PARSE A LINE FROM ENDPOINT FILE
		//*******************************************

		//Parse file 2 input line
		stringstream file_2_linestream(endpoint_line);
		//get file_2 line pos finding the separator ('\t')
		getline(file_2_linestream, endpoint_chromosome, '\t');
		//get file_2 loc
		file_2_linestream >> endpoint_base_number;
		//we need chromosome numeber and loc
		//extract chromosome number from chromosome string
		// "chrN", N integer
		//     ^
		endpoint_chromosome_number = atoi(&endpoint_chromosome[3]);
		//get file_2 ATCG data finding the separator ('\n')
		getline(file_2_linestream, endpoint_ATCG_data, '\n');

		//parse endpoint ATCG data
		stringstream endpoint_ATCG_linestream(endpoint_ATCG_data);
		//endpoint reference
		endpoint_ATCG_linestream >> endpoint_reference;
		//counters
		endpoint_ATCG_linestream >> endpoint_A_counter; endpoint_coverage += endpoint_A_counter;
		endpoint_ATCG_linestream >> endpoint_T_counter; endpoint_coverage += endpoint_T_counter;
		endpoint_ATCG_linestream >> endpoint_C_counter; endpoint_coverage += endpoint_C_counter;
		endpoint_ATCG_linestream >> endpoint_G_counter; endpoint_coverage += endpoint_G_counter;

		endpoint_ATCG_linestream >> endpoint_a_counter; endpoint_coverage += endpoint_a_counter;
		endpoint_ATCG_linestream >> endpoint_t_counter; endpoint_coverage += endpoint_t_counter;
		endpoint_ATCG_linestream >> endpoint_c_counter; endpoint_coverage += endpoint_c_counter;
		endpoint_ATCG_linestream >> endpoint_g_counter; endpoint_coverage += endpoint_g_counter;

		//**********************************************
		// COMPUTE MUTED READS AND MUT FREQ FOR ENDPOINT
		//**********************************************
		int A = endpoint_A_counter + endpoint_a_counter;
		int T = endpoint_T_counter + endpoint_t_counter;
		int C = endpoint_C_counter + endpoint_c_counter;
		int G = endpoint_G_counter + endpoint_g_counter;

		if (endpoint_reference == 'A'){
			int muted_reads = max(T,C);
			muted_reads = max(muted_reads,G);
			endpoint_mut_freq = double(muted_reads)/double(endpoint_coverage);
			if (muted_reads != 0) {
				if (muted_reads == T) endpoint_muted_in = 'T';
				if (muted_reads == C) endpoint_muted_in = 'C';
				if (muted_reads == G) endpoint_muted_in = 'G';
			} else endpoint_muted_in = ' ';
		}
		if (endpoint_reference == 'T'){
			int muted_reads = max(A,C);
			muted_reads = max(muted_reads,G);
			endpoint_mut_freq = double(muted_reads)/double(endpoint_coverage);
			if (muted_reads != 0) {
				if (muted_reads == A) endpoint_muted_in = 'A';
				if (muted_reads == C) endpoint_muted_in = 'C';
				if (muted_reads == G) endpoint_muted_in = 'G';
			} else endpoint_muted_in = ' ';
		}
		if (endpoint_reference == 'C'){
			int muted_reads = max(A,T);
			muted_reads = max(muted_reads,G);
			endpoint_mut_freq = double(muted_reads)/double(endpoint_coverage);
			if (muted_reads != 0) {
				if (muted_reads == A) endpoint_muted_in = 'A';
				if (muted_reads == T) endpoint_muted_in = 'T';
				if (muted_reads == G) endpoint_muted_in = 'G';
			} else endpoint_muted_in = ' ';
		}
		if (endpoint_reference == 'G'){
			int muted_reads = max(A,T);
			muted_reads = max(muted_reads,C);
			endpoint_mut_freq = double(muted_reads)/double(endpoint_coverage);
			if (muted_reads != 0) {
			if (muted_reads == A) endpoint_muted_in = 'A';
			if (muted_reads == T) endpoint_muted_in = 'T';
			if (muted_reads == C) endpoint_muted_in = 'C';
			} else endpoint_muted_in = ' ';
		}

		//*******************************************
		// READ AND PARSE A LINE FROM ENDPOINT FILE
		//*******************************************
		//take line from file 1
		getline(file_1_instream, ancestor_line);
		stringstream file_1_linestream(ancestor_line);
		getline(file_1_linestream, ancestor_chromosome, '\t');
		// "chrN", N integer
		//     ^
		ancestor_chromosome_number = atoi(&ancestor_chromosome[3]);
		file_1_linestream >> ancestor_base_number;
		getline(file_1_linestream, ancestor_ATCG_data, '\n');

		//parse ancestor ATCG data
		stringstream ancestor_ATCG_linestream(ancestor_ATCG_data);
		//ancestor reference
		ancestor_ATCG_linestream >> ancestor_reference;
		//counters
		ancestor_ATCG_linestream >> ancestor_A_counter; ancestor_coverage += ancestor_A_counter;
		ancestor_ATCG_linestream >> ancestor_T_counter; ancestor_coverage += ancestor_T_counter;
		ancestor_ATCG_linestream >> ancestor_C_counter; ancestor_coverage += ancestor_C_counter;
		ancestor_ATCG_linestream >> ancestor_G_counter; ancestor_coverage += ancestor_G_counter;

		ancestor_ATCG_linestream >> ancestor_a_counter; ancestor_coverage += ancestor_a_counter;
		ancestor_ATCG_linestream >> ancestor_t_counter; ancestor_coverage += ancestor_t_counter;
		ancestor_ATCG_linestream >> ancestor_c_counter; ancestor_coverage += ancestor_c_counter;
		ancestor_ATCG_linestream >> ancestor_g_counter; ancestor_coverage += ancestor_g_counter;

		//**********************************************
		// COMPUTE MUTED READS AND MUT FREQ FOR ANCESTOR
		//**********************************************
		A = ancestor_A_counter + ancestor_a_counter;
		T = ancestor_T_counter + ancestor_t_counter;
		C = ancestor_C_counter + ancestor_c_counter;
		G = ancestor_G_counter + ancestor_g_counter;

		if (ancestor_reference == 'A'){
			int muted_reads = max(T,C);
			muted_reads = max(muted_reads,G);
			ancestor_mut_freq = double(muted_reads)/double(ancestor_coverage);
		}
		if (ancestor_reference == 'T'){
			int muted_reads = max(A,C);
			muted_reads = max(muted_reads,G);
			ancestor_mut_freq = double(muted_reads)/double(ancestor_coverage);
		}
		if (ancestor_reference == 'C'){
			int muted_reads = max(A,T);
			muted_reads = max(muted_reads,G);
			ancestor_mut_freq = double(muted_reads)/double(ancestor_coverage);
		}
		if (ancestor_reference == 'G'){
			int muted_reads = max(A,T);
			muted_reads = max(muted_reads,C);
			ancestor_mut_freq = double(muted_reads)/double(ancestor_coverage);
		}

		count_line++; //all lines in files

		//**********************************************
		// APPLY THRESHOLDS (and do checks)
		//**********************************************

		//rarely there are "M","N", or "R" in reference
		//we need to discard that line
		if(ancestor_reference != 'A' && ancestor_reference != 'T'
		&& ancestor_reference != 'C' && ancestor_reference != 'G') {
			//reference is not A, T, C, G
			discarded_lines++;
			continue; //skip this line
		}

		if(endpoint_reference != 'A' && endpoint_reference != 'T'
		&& endpoint_reference != 'C' && endpoint_reference != 'G') {
			//reference is not A, T, C, G
			discarded_lines++;
			continue; //skip this line
		}

		// check that we loaded corresponding lines
		// (common lines file are ok!)
		assert(endpoint_chromosome_number == ancestor_chromosome_number);
		assert(endpoint_base_number == ancestor_base_number);
		assert(endpoint_reference == ancestor_reference);

		// apply fixed threshold on ancestor's coverage min
		if (ancestor_coverage < coverage_min_ancestor) {
			discarded_lines++;
			continue; //skip this line
		}

		// apply theshold on ancestor mut_freq
		//same as check if == zero
		if(ancestor_mut_freq > freq_max_ancestor_confirm_reference){
			discarded_lines++;
			continue; //skip this line
		}

		// apply theshold on endpoint mut_freq max
		if(endpoint_mut_freq > endpoint_freq_max) {
			discarded_lines++;
			continue; //skip this line
		}

		//are forward and reverse reads balanced?
		// example: endpoint muted in A
		// accept line if
		// (1/2 - 1/sqrt(#(A + a))) < #(A) / #(A + a) < (1/2 + 1/sqrt(#(A + a)))

		if(endpoint_muted_in == 'A') {
			float forward_reads = float(endpoint_A_counter);
			float total = forward_reads + float(endpoint_a_counter);
			float x = forward_reads / total ;

			if ( (x < 0.5 - 1./sqrt(total)) || (x > 0.5 + 1./sqrt(total)) ){
				discarded_lines++;
				continue; //skip this line
			}
		}

		if(endpoint_muted_in == 'T') {
			float forward_reads = float(endpoint_T_counter);
			float total = forward_reads + float(endpoint_t_counter);
			float x = forward_reads / total ;

			if ( (x < 0.5 - 1./sqrt(total)) || (x > 0.5 + 1./sqrt(total)) ){
				discarded_lines++;
				continue; //skip this line
			}
		}
		if(endpoint_muted_in == 'C') {
			float forward_reads = float(endpoint_C_counter);
			float total = forward_reads + float(endpoint_c_counter);
			float x = forward_reads / total ;

			if ( (x < 0.5 - 1./sqrt(total)) || (x > 0.5 + 1./sqrt(total)) ){
				discarded_lines++;
				continue; //skip this line
			}
		}

		if(endpoint_muted_in == 'G') {
			float forward_reads = float(endpoint_G_counter);
			float total = forward_reads + float(endpoint_g_counter);
			float x = forward_reads / total ;

			if ( (x < 0.5 - 1./sqrt(total)) || (x > 0.5 + 1./sqrt(total)) ){
				discarded_lines++;
				continue; //skip this line
			}
		}

		//**********************************************
		// DECIDE IF MUTED OR NOT
		//**********************************************
		// finally if here theese lines are fine with all thresholds
		//check if to be considered muted or not

		if( endpoint_muted(endpoint_mut_freq, endpoint_coverage, extant)){
			//muted -> Numerator & denominator
			muted_bases++;
		} else {
			//not muted -> Denominator
			not_muted_bases++;
		}
	} //EOF

	//Print result summary

	cout << "File readings ended: total lines " << count_line << endl;
	cout << "Discarded: " << discarded_lines << endl;
	cout << "Parameter used extant = " << extant << endl;

	assert(count_line - discarded_lines == muted_bases + not_muted_bases);

	// compute muted/(muted + not_muted)
	cout << "Muted bases:" << muted_bases << endl;
	cout << "Not muted bases: " << not_muted_bases << endl;

	P_hat = double (muted_bases) / double (muted_bases + not_muted_bases);

	cout << "muted/(muted + not_muted) = " << P_hat <<endl;

	cout << "Mutatior rate min [d/(b+d) = 0.45] = " << compute_mut_rate(P_hat, 0.45, extant) << endl;

	cout << "Mutation rate max [d/(b+d) = 0.1] = " << compute_mut_rate(P_hat, 0.1, extant) << endl;

	//Cleanup
	file_1.close();
	file_2.close();

	return 0;
} // main

double compute_mut_rate(double P_hat, double death_prob, int extant){

	// gen = log_{2*(1-death_prob)} extant
	float generations = log(extant)/log(2.*(1.-death_prob));

	//integral estimate with continuous time
	float attempts = (1. - death_prob*death_prob) * (
				( pow((2*(1-death_prob)), generations - 1.) - 1.) /
			  	log(2.*(1.- death_prob))
			  	) + extant;

	double mut_rate = -1. *( log(1. - P_hat)/attempts);

	return mut_rate;
}

bool endpoint_muted(float mut_freq, int total_coverage, int extant){
/*
Count base as muted if endpoint frequency

	f >= Cutoff(Coverage, f_min) e Coverage = TotCoverage/3

	cutoff (Coverage, f_min) = f_min + alpha/Sqrt[Coverage]

based on extant f_min and corresponding alphas are

	f_min = {1/8, 1/12, 1/16, 1/20, 1/24, 1/28, 1/32, 1/48, 1/75, 1/100}

	alpha = {0.52, 0.45, 0.36, 0.32, 0.3, 0.28, 0.25, 0.2, 0.18, 0.15}
*/
	float f_min= 0.;
	float coverage = float(total_coverage)/3.;
	double alpha = 0.;

	if (extant == 8){
		f_min = 1./extant;
		alpha = 0.52;
	}
	if (extant == 12){
		f_min = 1./extant;
		alpha = 0.45;
	}

	if (extant == 16){
		f_min = 1./extant;
		alpha = 0.36;
	}
	if (extant == 20){
		f_min = 1./extant;
		alpha = 0.32;
	}
	if (extant == 24){
		f_min = 1./extant;
		alpha = 0.3;
	}
	if (extant == 28){
		f_min = 1./extant;
		alpha = 0.28;
	}
	if (extant == 32){
		f_min = 1./extant;
		alpha = 0.25;
	}
	if (extant == 48){
		f_min = 1./extant;
		alpha = 0.2;
	}

	if (extant == 75){
		f_min = 1./extant;
		alpha = 0.18;
	}
	if (extant == 100){
		f_min = 1./extant;
		alpha = 0.15;
	}

	if ( mut_freq >= f_min + alpha/sqrt(coverage) ) {
		//muted
		return true;
	} else {
		//not muted
		return false;
	}
}


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>

#ifndef _DAVIDSON_SOLVER_
#define _DAVIDSON_SOLVER_

class DavidsonSolver
{

	public:

		DavidsonSolver();

		void set_iter_max(int N) { this->iter_max = N; }
		void set_tolerance(double eps) { this->tol = eps; }
		void set_max_search_space(int N) { this->max_search_space = N;}
		void set_initial_guess_size(int N) {this->size_initial_guess=N;}
		void set_linsolve_tol(double tol){this->linsolve_tol=tol;}
		void set_guess_vectors(std::string method){this->guess_vectors=method;}

		void set_correction(std::string method);
		void set_jacobi_linsolve(std::string method);

		Eigen::VectorXd eigenvalues() const {return this->_eigenvalues;}
		Eigen::MatrixXd eigenvectors() const {return this->_eigenvectors;}

		template <typename MatrixReplacement>
		void solve(MatrixReplacement &A, int neigen, int size_initial_guess = 0)
		{

		    std::cout << std::endl;
		    std::cout << "===========================" << std::endl;
		    if(this->correction == CORR::JACOBI)  std::cout << "= Jacobi-Davidson  : " << this->jacobi_linsolve <<  std::endl;

		    else if (this->correction == CORR::OLSEN)  std::cout << "= Olsen-Davidson  : " <<  std::endl;

		    else  std::cout << "= Davidson (DPR)" <<  std::endl;

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

		    //double res_norm;
		    Eigen::ArrayXd res_norm = Eigen::ArrayXd::Zero(neigen);
		    Eigen::ArrayXd root_converged = Eigen::ArrayXd::Zero(neigen);
		    Eigen::ArrayXd lambda_conv = Eigen::ArrayXd::Zero(neigen);
		    int size = A.rows();
		    bool has_converged = false;

		    // initial guess size
		    if (size_initial_guess == 0) {
		    	size_initial_guess = 2 * neigen;
		    	if (size_initial_guess < 10)
		    		size_initial_guess = 10;
		    }
		    int search_space = size_initial_guess;
		    max_search_space = 2*size_initial_guess;

		    // initialize the guess eigenvector
		    Eigen::VectorXd Adiag = A.diagonal();
		    Eigen::MatrixXd V = DavidsonSolver::_get_initial_eigenvectors(Adiag,size_initial_guess);

		    Eigen::VectorXd lambda; // eigenvalues hodlers
		    Eigen::VectorXd old_val = Eigen::VectorXd::Zero(neigen);

		    // temp varialbes
		    Eigen::MatrixXd T, U, q;
		    Eigen::VectorXd w, tmp;

		    // project the matrix on the trial subspace
		    T = A * V;
		    T = V.transpose()*T;

		    printf("iter\tSearch Space\tNorm/%.0e\n",tol);
		    std::cout << "-----------------------------------" << std::endl;
		    for (int iiter = 0; iiter < iter_max; iiter ++ )
		    {

		        // std::cout << "\nT:\n" << T << std::endl;
		        // diagonalize the small subspace
		        Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(T);
		        lambda = es.eigenvalues();
		        U = es.eigenvectors();

		        // Ritz eigenvectors
		        q = V*U.block(0,0,U.rows(),neigen);

		        // residue and correction vectors
		        for (int j=0; j<neigen; j++) {

		        	// (not root_converged[j]) {

			            // residue vector
			            w = A*q.col(j) - lambda(j)*q.col(j);
			            res_norm[j] = w.norm();

			            // jacobi-davidson correction
			            if (this->correction == CORR::JACOBI) {
			                tmp = q.col(j);
			                w = DavidsonSolver::_jacobi_correction<MatrixReplacement>(A,w,tmp,lambda(j));
			            }

			            else if (this->correction == CORR::OLSEN) {
			            	tmp = q.col(j);
			                w = DavidsonSolver::_olsen_correction(w,tmp,Adiag,lambda(j));
			            }

			            // Davidson DPR
			            else  {
			                w = DavidsonSolver::_dpr_correction(w,Adiag,lambda(j));
			            }

			            // append the correction vector to the search space
			            V.conservativeResize(Eigen::NoChange,V.cols()+1);
			            V.col(V.cols()-1) = w.normalized();

			            // check the root
			            root_converged[j] = res_norm[j] < tol;
			        //}

		        }

		        // eigenvalue norm
		        lambda_conv = (lambda.head(neigen)-old_val).array().abs();
		        printf("%4d\t%12d\t%4.2e\t%4.2e\t%4.1f%% converged\n", iiter,search_space,res_norm.maxCoeff(),lambda_conv.maxCoeff(),100*root_converged.sum()/neigen);

		        // update
		        search_space = V.cols();
		        old_val = lambda.head(neigen);

		        // break if converged, update otherwise
		        if((res_norm<tol).all()) {
		        //if((lambda_conv<tol).all()) {
		            has_converged = true;
		            break;
		        }

		        // check if we need to restart
		        if (search_space > max_search_space or search_space > size )
		        {

		            V = q.block(0,0,V.rows(),neigen);
		            for (int j=0; j<neigen; j++) {
		                V.col(j) = V.col(j).normalized();
		            }
		            search_space = neigen;

		            // recompute the projected matrix
		            T = V.transpose()*(A * V);
		        }

		        // continue otherwise
		        else
		        {
		            // orthogonalize the V vectors
		            //V = DavidsonSolver::_QR(V);
		            V = DavidsonSolver::_gramschmidt(V,V.cols()-neigen);

		            // update the T matrix : avoid recomputing V.T A V
		            // just recompute the element relative to the new eigenvectors
		            DavidsonSolver::_update_projected_matrix<MatrixReplacement>(T,A,V);

		        }

		    }

		    // store the eigenvalues/eigenvectors
		    this->_eigenvalues = lambda.head(neigen);
		    this->_eigenvectors = q.block(0,0,q.rows(),neigen);

		    // normalize the eigenvectors
		    for (int i=0; i<neigen; i++){
		        this->_eigenvectors.col(i).normalize();
		    }

		    std::cout << "-----------------------------------" << std::endl;
		    if (!has_converged) {
		        std::cout << "- Warning : Davidson didn't converge ! " <<  std::endl;
		        this->_eigenvalues = Eigen::VectorXd::Zero(neigen);
		        this->_eigenvectors = Eigen::MatrixXd::Zero(size,neigen);
		    }
		    else   {
		        std::cout << "- Davidson converged " <<  std::endl;
		        printf("- final residue norm %4.2e\n",res_norm.maxCoeff());
		        printf("- final eigenvalue norm %4.2e\n",lambda_conv.maxCoeff());
		    }
		    std::cout << "-----------------------------------" << std::endl;

		}

	private :

		int iter_max = 1000;
		double tol = 1E-6;
		int max_search_space = 100;
		int size_initial_guess = 0;
		double linsolve_tol = 1E-3;

		std::string guess_vectors = "target";
		enum CORR {DPR,JACOBI,OLSEN};
		enum LSOLVE {CG,GMRES,LLT};

		CORR correction = CORR::DPR;
		LSOLVE jacobi_linsolve = LSOLVE::CG;

		Eigen::VectorXd _eigenvalues;
		Eigen::MatrixXd _eigenvectors;

		Eigen::ArrayXd _sort_index(Eigen::VectorXd &V) const;
		Eigen::MatrixXd _get_initial_eigenvectors(Eigen::VectorXd &D, int size ) const;
		Eigen::MatrixXd _solve_linear_system(Eigen::MatrixXd &A, Eigen::VectorXd &b) const;
		Eigen::MatrixXd _QR(Eigen::MatrixXd &A) const;
		Eigen::MatrixXd _gramschmidt( Eigen::MatrixXd &A, int nstart ) const;

		template <typename MatrixReplacement>
		Eigen::MatrixXd _jacobi_correction(MatrixReplacement &A, Eigen::VectorXd &r, Eigen::VectorXd &u, double lambda) const
		{

			std::chrono::time_point<std::chrono::system_clock> start, end;
    		std::chrono::duration<double> elapsed_time;

    		start = std::chrono::system_clock::now();
		    // form the projector  P = I -u * u.T
		    Eigen::MatrixXd P = -u*u.transpose();
		    P.diagonal().array() += 1.0;

		    // project the matrix P * (A - lambda*I) * P^T
		    Eigen::MatrixXd projA = A*P.transpose();
		    projA -= lambda*P.transpose();
		    projA = P * projA;
		    end = std::chrono::system_clock::now();
		    elapsed_time = end-start;
		    std::cout << "_ form linear system " << this->jacobi_linsolve << " in " << elapsed_time.count() << " secs" <<  std::endl;
		    return DavidsonSolver::_solve_linear_system(projA,r);
		}

		Eigen::VectorXd _dpr_correction(Eigen::VectorXd &w, Eigen::VectorXd &A0, double lambda) const;
		Eigen::VectorXd _olsen_correction(Eigen::VectorXd &r, Eigen::VectorXd &x, Eigen::VectorXd &D, double lambda) const;

		template<class MatrixReplacement>
		void _update_projected_matrix(Eigen::MatrixXd &T, MatrixReplacement &A, Eigen::MatrixXd &V) const
		{
		    int nvec_old = T.cols();
		    int nvec = V.cols();
		    int nnew_vec = nvec-nvec_old;

		    Eigen::MatrixXd _tmp = A * V.block(0,nvec_old,nvec,nnew_vec);
		    T.conservativeResize(nvec,nvec);
		    T.block(0,nvec_old,nvec,nnew_vec) = V.transpose() * _tmp;
		    T.block(nvec_old,0,nnew_vec,nvec_old) = T.block(0,nvec_old,nvec_old,nnew_vec).transpose();

		    return;
		}
};

#endif


/*
MIT License

Copyright (c) 2019 Xiaohong Chen

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.
*/

#ifndef BOOST_UBLAS_CONJUGATE_GRADIENT_HPP
#define BOOST_UBLAS_CONJUGATE_GRADIENT_HPP

#include "krylov_solvers_config.hpp"

#include <boost/numeric/ublas/vector.hpp>

namespace boost { namespace numeric { namespace ublas {

namespace detail {

template<class F1, class F2, class E, class V, typename Int, typename Floating>
std::tuple<Int, Floating>
conjugate_gradient_impl(const F1& A, const F2& PInv, const vector_expression<E>& b, V& x,
           Int max_iter_, Floating tol_, std::false_type) {
    typedef typename V::value_type value_type;
    typedef typename V::size_type size_type;
    typedef vector<value_type> vector_type;
    typedef std::tuple<Int, Floating> return_type;

    BOOST_UBLAS_CHECK(b().size() == x.size(), bad_size());
    BOOST_UBLAS_CHECK(max_iter_ > Int/*zero*/(), bad_argument());
    BOOST_UBLAS_CHECK(tol_ > Floating/*zero*/(), bad_argument());

    size_type max_iter = static_cast<size_type>(max_iter_);
    value_type tol = static_cast<value_type>(tol_);

    size_type n = x.size();
    // adjust max_iter
    max_iter = std::min(n, max_iter);

    value_type b_norm = norm_2(b);
    if (b_norm == value_type/*zero*/()) {
        x *= value_type/*zero*/();
        return return_type(Int/*zero*/(), Floating/*zero*/());
    }

    vector_type r = b - A(x);
    value_type error = norm_2(r) / b_norm;
    if (error < tol) {
        return return_type(Int/*zero*/(), error);
    }

    vector_type z = PInv(r);
    vector_type p(z);

    value_type r_sq_old = inner_prod(r, z);

    size_type num_iter = 1;
    for (; num_iter <= max_iter; ++num_iter) {
        vector_type Ap = A(p);
        value_type alpha = r_sq_old / inner_prod(p, Ap);
        x.plus_assign(alpha*p);
        r.minus_assign(alpha*Ap);
        z.assign(PInv(r));
        value_type r_sq_new = inner_prod(r, z);
        error = norm_2(r) / b_norm;
        if (error < tol) {
            return return_type(num_iter, error);
        }

        p = z + (r_sq_new / r_sq_old)*p;
        r_sq_old = r_sq_new;
    }

    return return_type(n, norm_2(r) / b_norm);
}

template<class M, class F, class E, class V, typename Int, typename Floating>
std::tuple<Int, Floating>
conjugate_gradient_impl(const M& A, const F& PInv, const vector_expression<E>& b, V& x,
           Int max_iter_, Floating tol_, std::true_type) {
    return conjugate_gradient_impl([&A](const auto& v){return ublas::prod(A, v);}, PInv,
                      b, x, max_iter_, tol_, std::false_type());
}

} // end namespace detail

template <class M, class F = identity_precond<M> >
class conjugate_gradient {
public:
    // param
    struct param {
        int max_iter = 10;
        int restart_iter = 10;
        double tol = 1e-5;
    };

    typedef std::tuple<int, double> return_type;

    conjugate_gradient(const M& A)
        : A_(A), PInv_(A) {}

    conjugate_gradient(const M& A, const param& p)
        : A_(A), PInv_(A), param_(p) {}

    conjugate_gradient(const M& A, const F& PInv)
        : A_(A), PInv_(PInv) {}

    conjugate_gradient(const M& A, F&& PInv)
        : A_(A), PInv_(std::move(PInv)) {}

    conjugate_gradient(const M& A, const F& PInv, const param& p)
        : A_(A), PInv_(PInv), param_(p) {}

    conjugate_gradient(const M& A, F&& PInv, const param& p)
        : A_(A), PInv_(PInv), param_(p) {}

    conjugate_gradient(const conjugate_gradient&) = delete;
    conjugate_gradient(conjugate_gradient&&) = delete;
    conjugate_gradient& operator=(const conjugate_gradient&) = delete;
    conjugate_gradient& operator=(conjugate_gradient&&) = delete;

    param get_param() const {
        return param_;
    }

    void set_param(const param& p) {
        param_ = p;
    }

    template<class E, class V>
    return_type operator()(const vector_expression<E>& b, V& x) const {
        return detail::conjugate_gradient_impl(A_, PInv_, b, x,
                                       param_.max_iter,
                                       param_.tol,
                                       detail::is_matrix_expression_t<M>());
    }

private:
    const M& A_;
    F PInv_;
    param param_;
};

} // end namespace linear
} // end namespace solver
} // end namespace math

#endif


//! [distance-hypot-all]
#include <chrono>
#include <iostream>
#include <algorithm>
#include <vector>

#include <boost/simd/function/aligned_load.hpp>
#include <boost/simd/function/aligned_store.hpp>
#include <boost/simd/function/fast.hpp>
#include <boost/simd/function/hypot.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/memory/allocator.hpp>

int main(int argc, char** argv)
{
  using namespace std::chrono;
  namespace bs = boost::simd;
  typedef float T;
  using pack_t = bs::pack<T>;

  std::size_t num_points = 1600000;
  std::vector<T, bs::allocator<T>> X(num_points);
  std::vector<T, bs::allocator<T>> Y(num_points);
  std::vector<T, bs::allocator<T>> distance0(num_points);
  std::vector<T, bs::allocator<T>> distance1(num_points);
  std::vector<T, bs::allocator<T>> distance2(num_points);
  std::vector<T, bs::allocator<T>> distance3(num_points);

  std::generate(X.begin(), X.end(), [](){return T(std::rand()) / std::numeric_limits<int>::max();});
  std::generate(Y.begin(), Y.end(), [](){return T(std::rand()) / std::numeric_limits<int>::max();});

  T refX = 0, refY = 0;

  auto t0 = high_resolution_clock::now();
  //! [distance-hypot-scalar]
  for (int i = 0; i < num_points; ++i) {
    auto x = refX - X[i];
    auto y = refY - Y[i];
    distance0[i] = std::hypot(refX - X[i], refY - Y[i]);
  }
  //! [distance-hypot-scalar]
  auto t1 = high_resolution_clock::now();
  std::cout << " time scalar " << duration_cast<microseconds>(t1 - t0).count() << std::endl;

  //! [distance-hypot-time]
  t0 = high_resolution_clock::now();
  //! [distance-hypot-calc]
  pack_t vrefX = pack_t(refX);
  pack_t vrefY = pack_t(refY);

  for (int i = 0; i < num_points; i += pack_t::static_size) {
    pack_t vX  = bs::aligned_load<pack_t>(&X[i]);
    pack_t vY  = bs::aligned_load<pack_t>(&Y[i]);
    pack_t res = bs::hypot(vrefX - vX, vrefY - vY);
    bs::aligned_store(res, &distance1[i]);
  }
  //! [distance-hypot-calc]

  t1 = high_resolution_clock::now();
  std::cout << " time SIMD hypot " << duration_cast<microseconds>(t1 - t0).count() << std::endl;

  t0 = high_resolution_clock::now();
  //! [distance-hypot-fast-hypot]
  for (int i = 0; i < num_points; i += pack_t::static_size) {
    pack_t vX  = bs::aligned_load<pack_t>(&X[i]);
    pack_t vY  = bs::aligned_load<pack_t>(&Y[i]);
    pack_t res = bs::fast_(bs::hypot)(vrefX - vX, vrefY - vY);
    bs::aligned_store(res, &distance2[i]);
  }
  ////! [distance-hypot-fast-hypot]
  t1 = high_resolution_clock::now();
  std::cout << " time SIMD fast hypot " << duration_cast<microseconds>(t1 - t0).count() << std::endl;
}
//! [distance-hypot-all]


#include <iostream>
#include <climits>
// BGL include
#include <boost/graph/adjacency_list.hpp>

// BGL flow include *NEW*
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>

// Graph Type with nested interior edge properties for flow algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;

using namespace std;

int c_to_i (char c) {
  return c - 'A';
}
// Custom edge adder class, highly recommended
class edge_adder {
  graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

void make_it_flow() {
  int h, w;
  cin >> h; cin >> w;
  string note; cin >> note;
  int n = note.length();
  graph G(26 * 26 + 26);
  edge_adder adder(G);
  // Add special vertices source and sink
  const vertex_desc v_source = boost::add_vertex(G);
  const vertex_desc v_sink = boost::add_vertex(G);

  vector<int> char_count(26, 0);
  for(int i = 0; i < n; i++) {
    char c = note[i];
    char_count[c_to_i(c)]++;
  }

  vector<int> pair_count(26*26, 0);
  vector<string> front(h);
  for(int i = 0; i < h; i++) {
    string line; cin >> line;
    front[i] = line;
    // for(int j = 0; j < w; j++) {
    //   char c = line[j];
    //   adder.add_edge(i * w + j, h * w + c_to_i(c), 1);
    // }
  }
  vector<string> back(h);
  for(int i = 0; i < h; i++) {
    string line; cin >> line;
    back[i] = line;
    // for(int j = w - 1; j >= 0; j--) {
    //   char c = line[w - j - 1];
    //   adder.add_edge(i * w + j, h * w + c_to_i(c), 1);
    // }
  }

  for(int i = 0; i < h; i++) {
    string line_front = front[i];
    string line_back = back[i];
    for(int j = 0; j < w; j++) {
      char c_front = line_front[j];
      char c_back = line_back[w - j - 1];
      int i_front = c_to_i(c_front);
      int i_back = c_to_i(c_back);
      pair_count[i_front * 26 + i_back]++;
    }
  }

  for(int i = 0; i < 26; i++) {
    for(int j = 0; j < 26; j++) {
      adder.add_edge(i * 26 + j, 26 * 26 + i, INT_MAX);
      adder.add_edge(i * 26 + j, 26 * 26 + j, INT_MAX);
    }
  }

  for(int i = 0; i < 26 * 26; i++) {
    adder.add_edge(v_source, i, pair_count[i]);
  }

  for(int i = 0; i < 26; i++) {
    adder.add_edge(26 * 26 + i, v_sink, char_count[i]);
  }

  // Calculate flow from source to sink
  // The flow algorithm uses the interior properties (managed in the edge adder)
  // - edge_capacity, edge_reverse (read access),
  // - edge_residual_capacity (read and write access).
  long flow = boost::push_relabel_max_flow(G, v_source, v_sink);
  if(flow == n) {
    std::cout << "Yes" << "\n";
  } else {
    std::cout << "No" << "\n";
  }

  // Retrieve the capacity map and reverse capacity map
  // const auto c_map = boost::get(boost::edge_capacity, G);
  // const auto rc_map = boost::get(boost::edge_residual_capacity, G);

  // Iterate over all the edges to print the flow along them
  // auto edge_iters = boost::edges(G);
  // for (auto edge_it = edge_iters.first; edge_it != edge_iters.second; ++edge_it) {
  //   const edge_desc edge = *edge_it;
  //   const long flow_through_edge = c_map[edge] - rc_map[edge];
  //   std::cerr << "edge from " << boost::source(edge, G) << " to " << boost::target(edge, G)
  //             << " runs " << flow_through_edge
  //             << " units of flow (negative for reverse direction). \n";
  // }
}

int main() {
  std::ios_base::sync_with_stdio(false); // Always!
  int t; cin >> t;
  while(t--) {
    // cerr << "-----------------testcase done" << endl;
    make_it_flow();
  }
  return 0;
}


#ifndef PLANE3D_HPP
#define PLANE3D_HPP
#include <Eigen/Dense>

//#define DEBUG_PLANE

/** Representation of the plane equation
* ax + by + cz + d = dist
*/
class Plane3D
{
public:

  /**
   * Coefficient constructor - simply copy the given coefficients
   * @param coeffs Coefficients {a,b,c,d}
   */
  Plane3D(const double coeffs[4])
    {
      for(int i = 0; i < 4; i++)
      {
	m_coeffs[i] = coeffs[i];
      }
    }

  /**
   * Matrix constructor - treat the z axis of the rotation part of the matrix as a normal vector
   * @param coeffs Matrix to construct from
   */
  Plane3D(const Eigen::Matrix4f coeffs)
    {
      for(int i = 0; i < 3; i++)
      {
	m_coeffs[i] = coeffs(i,2);
      }
      m_coeffs[3] = 0.0;
      for(int i = 0; i < 3; i++)
      {

	m_coeffs[3] -= m_coeffs[i]*coeffs(i,3);
      }
    }

  /**
   * Matrix constructor - treat the z axis of the rotation part of the matrix as a normal vector
   * @param coeffs Matrix to construct from
   */
  Plane3D(const Eigen::Matrix4d coeffs)
    {
      for(int i = 0; i < 3; i++)
      {
	m_coeffs[i] = coeffs(i,2);
      }
      m_coeffs[3] = 0.0;
      for(int i = 0; i < 3; i++)
      {

	m_coeffs[3] -= m_coeffs[i]*coeffs(i,3);
      }
    }

  /**
   * Zero constructor. Initialize coefficients to zero
   */
  Plane3D()
    {
      for(int i = 0; i < 4; i++)
      {
	m_coeffs[i] = 0.0;
      }
    };
  ~Plane3D() { }

  /**
   * Set a coefficient
   * @param i coefficient to set
   * @param c value to set it to
   */
  inline void setCoefficient(const int i, const double c)
    {
      m_coeffs[i] = c;
    }
  /**
   * Get a coefficient
   * @param i coefficient to get
   * @return coefficient
   */
  inline double getCoefficient(const int i)
    {
      return m_coeffs[i];
    }
  /**
   * Get distance from the plane to a point
   * @param pt point to find distance to
   * @return distance to point
   */
  inline double getDistance(double const pt[3]) const
    {
      double ret = 0.0;
      for(int i = 0; i < 3; i++)
      {
	ret += pt[i]*m_coeffs[i];
      }
      ret += m_coeffs[3];
#ifdef DEBUG_PLANE
      cerr << "Distance: " << ret << endl;
#endif
      return ret;
    }

  /**
   * Project a point onto the plane
   * @param projected Projected point will be returned here
   * @param pt Point to project
   */
 // void projectOnto(double projected[3], double const pt[3] ) const; DELETED

protected:
  /**
   * The plane equation coefficients a,b,c,d
   */
  double m_coeffs[4];

};

#endif // PLANE3D_HPP


// Filename: matrix_free_1.cpp (part of MTL4)

#include <iostream>
#include <cassert>
#include <boost/numeric/mtl/mtl.hpp>

struct poisson2D_dirichlet
{
    poisson2D_dirichlet(int m, int n) : m(m), n(n) {}

    template <typename Vector>
    Vector operator*(const Vector& v) const
    {
	assert(int(size(v)) == m * n);
	Vector w(m * n);

	for (int i= 0; i < m; i++)
	    for (int j= 0; j < n; j++) {
		int k= i * n + j; // offset
		w[k]= 4 * v[k];
		if (i > 0) w[k]-= v[k-n];   // upper neighbor
		if (i < m-1) w[k]-= v[k+n]; // lower neighbor
		if (j > 0) w[k]-= v[k-1];   // left neighbor
		if (j < n-1) w[k]-= v[k+1]; // right neighbor
	    }
	return w;
    }
    int m, n;
};

namespace mtl { namespace ashape {
    template <> struct ashape_aux<poisson2D_dirichlet>
    {	typedef nonscal type;    };
}}

int main(int, char**)
{
    using namespace std;

    mtl::dense_vector<double> v(20);
    iota(v);

    poisson2D_dirichlet A(4, 5);
    cout << "A * v is " << A * v << endl;

    return 0;
}


#include <igl/boundary_facets.h>
#include <igl/colon.h>
#include <igl/cotmatrix.h>
#include <igl/jet.h>
#include <igl/min_quad_with_fixed.h>
#include <igl/readOFF.h>
#include <igl/setdiff.h>
#include <igl/slice.h>
#include <igl/slice_into.h>
#include <igl/unique.h>
#include <igl/opengl/glfw/Viewer.h>
#include <Eigen/Sparse>
#include <iostream>
#include "tutorial_shared_path.h"

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;
  MatrixXd V;
  MatrixXi F;
  igl::readOFF(TUTORIAL_SHARED_PATH "/camelhead.off",V,F);
  // Find boundary edges
  MatrixXi E;
  igl::boundary_facets(F,E);
  // Find boundary vertices
  VectorXi b,IA,IC;
  igl::unique(E,b,IA,IC);
  // List of all vertex indices
  VectorXi all,in;
  igl::colon<int>(0,V.rows()-1,all);
  // List of interior indices
  igl::setdiff(all,b,in,IA);

  // Construct and slice up Laplacian
  SparseMatrix<double> L,L_in_in,L_in_b;
  igl::cotmatrix(V,F,L);
  igl::slice(L,in,in,L_in_in);
  igl::slice(L,in,b,L_in_b);

  // Dirichlet boundary conditions from z-coordinate
  VectorXd bc;
  VectorXd Z = V.col(2);
  igl::slice(Z,b,bc);

  // Solve PDE
  SimplicialLLT<SparseMatrix<double > > solver(-L_in_in);
  VectorXd Z_in = solver.solve(L_in_b*bc);
  // slice into solution
  igl::slice_into(Z_in,in,Z);

  // Alternative, short hand
  igl::min_quad_with_fixed_data<double> mqwf;
  // Linear term is 0
  VectorXd B = VectorXd::Zero(V.rows(),1);
  // Empty constraints
  VectorXd Beq;
  SparseMatrix<double> Aeq;
  // Our cotmatrix is _negative_ definite, so flip sign
  igl::min_quad_with_fixed_precompute((-L).eval(),b,Aeq,true,mqwf);
  igl::min_quad_with_fixed_solve(mqwf,B,bc,Beq,Z);

  // Pseudo-color based on solution
  MatrixXd C;
  igl::jet(Z,true,C);

  // Plot the mesh with pseudocolors
  igl::opengl::glfw::Viewer viewer;
  viewer.data().set_mesh(V, F);
  viewer.data().show_lines = false;
  viewer.data().set_colors(C);
  viewer.launch();
}


/*
license in here
This is free and unencumbered software released into the public domain.

Anyone is free to copy, modify, publish, use, compile, sell, or
distribute this software, either in source code form or as a compiled
binary, for any purpose, commercial or non-commercial, and by any
means.

In jurisdictions that recognize copyright laws, the author or authors
of this software dedicate any and all copyright interest in the
software to the public domain. We make this dedication for the benefit
of the public at large and to the detriment of our heirs and
successors. We intend this dedication to be an overt act of
relinquishment in perpetuity of all present and future rights to this
software under copyright law.

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 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.

For more information, please refer to <https://unlicense.org>
*/

#include <boost/random.hpp>
#include <iostream>

int main(int argc,char *argv[])
{
  boost::mt19937 gen; //伪随机数发生器（generator）
  boost::uniform_int<>dist(1,6);
  boost::variate_generator<boost::mt19937&, boost::uniform_int<>>die(gen,dist);

  for(auto i = 0; i < 10; ++i)
  {
	std::cout << die() << "  ";
  }
  std::cout << "\n";
}


#include <boost/mpi.hpp>
#include "mpi_impl/scatter_mesh.hpp"
#include "mpi_impl/poisson.hpp"

namespace mpi = boost::mpi;
using namespace schro_mpi;

double f(double x, double y)
{
    return x*x - y*y;
}

bool test_poisson(mpi::communicator& comm)
{
    int order = 6;

    Mesh<double> mesh = scatter_mesh<double>(order, "../meshes/small_mesh", comm, 0);
    mesh.compute_metrics();

    int dof = mesh.dof(); // processor-local degrees of freedom
    dof = mpi::all_reduce(comm, dof, std::plus<int>{}); // total degrees of freedom

    const auto& z = mesh.quadrature.x;

    SparseData<matrix<double>> F, u;
    for (auto& [e, element] : mesh.elements)
    {
        F[e] = arma::zeros(mesh.N, mesh.N);
        u[e] = arma::zeros(mesh.N, mesh.N);

        for (int i=0; i < mesh.N; ++i)
            for (int j=0; j < mesh.N; ++j)
            {
                auto [x, y] = mesh.elements[e].from_local_coo(z[i], z[j]);
                F[e](i, j) = f(x, y);
            }
    }

    auto rslts = poisson<double>(u, F, mesh, dof, 1e-10);

    if (comm.rank() == 0)
        std::cout << "poisson returned after " << rslts.n_iter << " iteration with residual " << rslts.residual << std::endl;

    return rslts.success;
}


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

#include <boost/simd/bitwise/functions/popcnt.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/bitwise_notand.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/rem.hpp>
#include <boost/simd/include/functions/simd/shri.hpp>
#include <boost/simd/include/constants/digits.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::popcnt_, tag::cpu_, (A0)(X)
                            , ((simd_<int8_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      const result_type m1  = boost::simd::integral_constant<result_type,0x55>(); //binary: 0101...
      const result_type m2  = boost::simd::integral_constant<result_type,0x33>(); //binary: 00110011..
      const result_type m4  = boost::simd::integral_constant<result_type,0x0f>(); //binary:  4 zeros,  4 ones ...
      result_type x = simd::bitwise_cast<result_type>(a0);
      x -= (shri(x, 1)) & m1;             //put count of each 2 bits into those 2 bits
      x = (x & m2) + (shri(x, 2) & m2); //put count of each 4 bits into those 4 bits
      x = (x + shri(x, 4)) & m4;        //put count of each 8 bits into those 8 bits
      return x & boost::simd::integral_constant<result_type,0x7f > ();
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::popcnt_, tag::cpu_, (A0)(X)
                            , ((simd_<int64_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      const result_type m1  = boost::simd::integral_constant<result_type,0x5555555555555555ULL>(); //binary: 0101...
      const result_type m2  = boost::simd::integral_constant<result_type,0x3333333333333333ULL>(); //binary: 00110011..
      const result_type m4  = boost::simd::integral_constant<result_type,0x0f0f0f0f0f0f0f0fULL>(); //binary:  4 zeros,  4 ones ...
      result_type x = simd::bitwise_cast<result_type>(a0);
      x -= (shri(x, 1)) & m1;             //put count of each 2 bits into those 2 bits
      x = (x & m2) + (shri(x, 2) & m2); //put count of each 4 bits into those 4 bits
      x = (x + shri(x, 4)) & m4;        //put count of each 8 bits into those 8 bits
      x += shri(x, 8);  //put count of each 16 bits into their lowest 8 bits
      x += shri(x, 16);  //put count of each 32 bits into their lowest 8 bits
      x += shri(x, 32);  //put count of each 64 bits into their lowest 8 bits
      return x & boost::simd::integral_constant<result_type,0x7f > ();
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::popcnt_, tag::cpu_, (A0)(X)
                            , ((simd_<int16_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      const result_type m1  = boost::simd::integral_constant<result_type,0x5555>(); //binary: 0101...
      const result_type m2  = boost::simd::integral_constant<result_type,0x3333>(); //binary: 00110011..
      const result_type m4  = boost::simd::integral_constant<result_type,0x0f0f>(); //binary:  4 zeros,  4 ones ...
      result_type x = simd::bitwise_cast<result_type>(a0);
      x -= (shri(x, 1)) & m1;             //put count of each 2 bits into those 2 bits
      x = (x & m2) + (shri(x, 2) & m2); //put count of each 4 bits into those 4 bits
      x = (x + shri(x, 4)) & m4;        //put count of each 8 bits into those 8 bits
      x += shri(x, 8);  //put count of each 16 bits into their lowest 8 bits
      return x & boost::simd::integral_constant<result_type,0x7f > ();
      }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::popcnt_, tag::cpu_, (A0)(X)
                            , ((simd_<int32_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      const result_type m1  = boost::simd::integral_constant<result_type,0x55555555>(); //binary: 0101...
      const result_type m2  = boost::simd::integral_constant<result_type,0x33333333>(); //binary: 00110011..
      const result_type m4  = boost::simd::integral_constant<result_type,0x0f0f0f0f>(); //binary:  4 zeros,  4 ones ...
      result_type x = simd::bitwise_cast<result_type>(a0);
      x -= (shri(x, 1)) & m1;             //put count of each 2 bits into those 2 bits
      x = (x & m2) + (shri(x, 2) & m2); //put count of each 4 bits into those 4 bits
      x = (x + shri(x, 4)) & m4;        //put count of each 8 bits into those 8 bits
      x += shri(x, 8);  //put count of each 16 bits into their lowest 8 bits
      x += shri(x, 16);  //put count of each 32 bits into their lowest 8 bits
      return x & boost::simd::integral_constant<result_type,0x7f > ();
      }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::popcnt_, tag::cpu_, (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return popcnt(simd::bitwise_cast<result_type>(a0));
    }
  };
} } }

#endif


//
// Created by Сергей Кривонос on 10.08.17.
//
#define BOOST_TEST_MODULE Extrapolator test
#include <boost/test/unit_test.hpp>
#include "Extrapolator.h"
#include "math/Sum.h"

using namespace omnn::math;
using namespace boost::unit_test;

std::string l(const omnn::math::Valuable& v)
{
    std::stringstream ss;
    ss << v;
    return ss.str();
}

BOOST_AUTO_TEST_CASE(ExtrapolatorSolve_test)
{

}

BOOST_AUTO_TEST_CASE(Extrapolator_test, *disabled())
{
    // lets define math vars:
    // TODO : link to youtube video with description of the sample for deep learning
    // for example we'll take bool square 2x2
    // lets say the goal is to determine type of squere
    // verticals:   {1,0},{1,0} or {0,1},{0,1}
    // horizontals: {1,1},{0,0} or {0,0},{1,1}
    // diagonals:   {1,0},{0,1} or {0,1},{1,0}
    // this way the vars are as follows:
    // box four vars a,b,c,d;  quantors: is_vericals, is_horizontals, is_diagonals

    //    Extrapolator<> e_verticals {{ {1,0,
    //                                   1,0,      1},
    //
    //                                  {0,1,
    //                                   0,1,      1},
    //
    //                                        {0,0,0,0,0},
    //                                        {1,1,1,1,0},
    //                                        {0,1,1,0,0}
    //                      }};

    // Test vericals:
    Extrapolator e_verticals {{
        {1,-1,
            1,-1},

        {-1,1,
            -1,1},

        //        {-1,-1,-1,-1},
        //        {1,1,1,1}
    }};

    ublas::vector<double> augment(e_verticals.size1());
    augment[0] = 2;
    augment[1] = 1;
    //    augment[2] = -1;
    //    augment[3] = -2;
    //BOOST_TEST(e_verticals.Consistent(augment));

    auto r = e_verticals.Solve(augment);
    std::cout << r[0] << ' ' << r[1] << ' ' << r[2] << ' ' << r[3] << std::endl;
    auto val = r[0] * 1 + r[1] * -1 + r[2] * 1 + r[3] * -1;
    std::cout << "(r[0] * 1 + r[1] * -1 + r[2] * 1 + r[3] * -1) is " << val << std::endl;
    BOOST_TEST(r[0] * 1 + r[1] * -1 + r[2] * 1 + r[3] * -1 == 1.);
    val = r[0] * -1 + r[1] * 1 + r[2] * -1 + r[3] * 1;
    std::cout << "(r[0] * -1 + r[1] * 1 + r[2] * -1 + r[3] * 1) is " << val << std::endl;
    BOOST_TEST(r[0] * -1 + r[1] * 1 + r[2] * -1 + r[3] * 1 == 2);
    val = r[0] * -1 + r[1] * -1 + r[2] * -1 + r[3] * -1;
    std::cout << "(r[0] * 1 + r[1] * 1 + r[2] * 1 + r[3] * 1) is " << val << std::endl;
    BOOST_TEST(r[0] * -1 + r[1] * -1 + r[2] * -1 + r[3] * -1 == -1);
    val = r[0] * 1 + r[1] * 1 + r[2] * 1 + r[3] * 1;
    std::cout << "(r[0] * 1 + r[1] * 1 + r[2] * 1 + r[3] * 1) is " << val << std::endl;
    BOOST_TEST(r[0] * 1 + r[1] * 1 + r[2] * 1 + r[3] * 1 == -2);

    Extrapolator e {{
        {0,0,0,0, 0,0,0},
        // verticals
        {1,0,1,0, 1,0,0},
        {0,1,0,1, 1,0,0},
        // horizontals
        {1,1,0,0, 0,1,0},
        {0,0,1,1, 0,1,0},
        // diagonals
        {0,1,1,0, 0,0,1},
        {1,0,0,1, 0,0,1},
    }};
    BOOST_CHECK_NO_THROW(e.Determinant());

    //BOOST_TEST(e.Consistent());
}

BOOST_AUTO_TEST_CASE(ViewMatrix_test)
{
    Extrapolator e {{ {1, 2},
        {3, 4} }};
    {
        Valuable v = e;
        std::cout << v << std::endl;
    }

    // view matrix
    //    0 0 1
    //    0 1 2
    //    1 0 3
    //    1 1 4
    auto vm = e.ViewMatrix();
    BOOST_TEST(vm.size1() == 4);
    BOOST_TEST(vm.size2() == 3);
    BOOST_TEST(vm(0,0) == 0); BOOST_TEST(vm(0,1) == 0); BOOST_TEST(vm(0,2) == 1);
    BOOST_TEST(vm(1,0) == 0); BOOST_TEST(vm(1,1) == 1); BOOST_TEST(vm(1,2) == 2);
    BOOST_TEST(vm(2,0) == 1); BOOST_TEST(vm(2,1) == 0); BOOST_TEST(vm(2,2) == 3);
    BOOST_TEST(vm(3,0) == 1); BOOST_TEST(vm(3,1) == 1); BOOST_TEST(vm(3,2) == 4);
    {
        // ax+by+cz=0
        // a=0:N, b=0:M, c=-(ax+by)/z     // if x=0 and y=0 then c becames 0 but it wasnt;
        // lets make c a va
        Valuable eq = 1_v;
        Variable x,y,z;
        for (auto i=vm.size1(); i--;) {
            auto e1 = x - vm(i,0);
            auto e2 = y - vm(i,1);
            auto e3 = z - vm(i,2);
            auto subsyst = e1*e1 + e2*e2 + e3*e3; // squares sum equivalent to conjunction
            std::cout << subsyst << std::endl;

            eq *= subsyst;

            BOOST_TEST(subsyst.IsSum());
            auto formula = FormulaOfVaWithSingleIntegerRoot(z, subsyst.as<Sum>());
            std::cout << "formula of value: " << formula << std::endl;
            auto evaluated = formula(vm(i, 0), vm(i, 1));
            std::cout << evaluated << std::endl;
            evaluated.optimize();
            std::cout << evaluated << std::endl;
            BOOST_TEST(evaluated == vm(i, 2)); // test row formula

            subsyst.Eval(x, vm(i, 0));
            subsyst.Eval(y, vm(i, 1));
            subsyst.Eval(z, vm(i, 2));
            subsyst.optimize();
            BOOST_TEST(subsyst == 0); // test row equation

            auto e = eq;
            e.Eval(x, vm(i, 0));
            e.Eval(y, vm(i, 1));
            e.Eval(z, vm(i, 2));
            e.optimize();
//            std::cout << e.str() << std::endl;
            BOOST_TEST(e == 0); //test current eq
        }
        std::cout << "Total equation:" << eq << std::endl;
        BOOST_TEST(eq.IsSum());
        auto f = FormulaOfVaWithSingleIntegerRoot(z, eq.as<Sum>());
        std::cout << "Formula : " << f << std::endl;
        // checking
        for (auto i=vm.size1(); i--;) {
            Valuable v = eq;
            v.Eval(x, vm(i,0));
            v.Eval(y, vm(i,1));
            v.Eval(z, vm(i,2));
            v.optimize();
            BOOST_TEST(v == 0);  //test whole equation
//            std::cout << std::endl << vm(i,2) << " : " << v << std::endl;
            BOOST_TEST(f(vm(i,0),vm(i,1))==vm(i,2)); // test formula
        }
    }
    Extrapolator m {{
        {0,0},
        {0,1},
        {1,0},
        {1,1}
    }};
    ublas::vector<Valuable> au(4);
    for(int i=1; i<4; ++i)
    {
        au[i] = i;
    }
}

BOOST_AUTO_TEST_CASE(Codec_test)
{
    Extrapolator ex {{
        //  input   output
        {0,0,0,0, 0,0,0},
        // verticals
        {1,0,1,0, 1,0,0},
        {0,1,0,1, 1,0,0},
        // horizontals
        {1,1,0,0, 0,1,0},
        {0,0,1,1, 0,1,0},
        // diagonals
        {0,1,1,0, 0,0,1},
        {1,0,0,1, 0,0,1},
    }};

//    Extrapolator e {{       // input    output
//        {0,0},              //{0,0,0,0, 0,0,0},
//        // verticals
//        {10,4},             //{1,0,1,0, 1,0,0},
//        {5,4},              //{0,1,0,1, 1,0,0},
//        // horizontals
//        {12,2},             //{1,1,0,0, 0,1,0},
//        {3,2},              //{0,0,1,1, 0,1,0},
//        // diagonals
//        {6,1},              //{0,1,1,0, 0,0,1},
//        {9,1},              //{1,0,0,1, 0,0,1},
//    }};

    Variable x,y,z;
    auto f = ex.Factors(y,x,z);
    std::list<Variable> formulaParamSequence = {y,x};
    FormulaOfVaWithSingleIntegerRoot fo(z, f, &formulaParamSequence);

    // TODO : extrapolation
//    std::cout << fo(ex.size1(), ex.size2()) << std::endl;

    // inbound data deduce
    for (auto i=ex.size1(); i--;) { // raw
        for (auto j=ex.size2(); j--;) { // column
            auto c = f;
            c.Eval(x, j);
            c.Eval(y, i);
            c.Eval(z, ex(i,j));
            c.optimize();
            BOOST_TEST(c==0_v);//

            c = fo(i,j);
            std::cout << c.str() << std::endl;
            BOOST_TEST(c == ex(i,j));
        }
    }
}


#ifndef USE_EIGEN_HPP
#define USE_EIGEN_HPP

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/StdVector>

#define EIGEN_DONT_PARALLELIZE

#ifdef HAS_HPX
#include "serialization/eigen_matrix.hpp"
#endif

namespace SO {
constexpr int ColumnMajor = Eigen::StorageOptions::ColMajor;
constexpr int RowMajor    = Eigen::StorageOptions::RowMajor;
}

template <typename Matrix>
using Column = typename Eigen::DenseBase<Matrix>::ColXpr;

template <typename T, uint m>
using StatVector = Eigen::Matrix<T, m, 1>;
template <typename T, uint m, uint n>
using StatMatrix = Eigen::Matrix<T, m, n>;

template <typename T>
using DynVector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
template <typename T>
using DynRowVector = Eigen::Matrix<T, 1, Eigen::Dynamic>;
template <typename T, int SO = SO::ColumnMajor>
using DynMatrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
template <typename T, uint m>
using HybMatrix = Eigen::Matrix<T, m, Eigen::Dynamic>;

template <typename T>
using SparseVector = Eigen::SparseVector<T>;
template <typename T>
using SparseMatrix = Eigen::SparseMatrix<T>;

template <typename EigenType>
using AlignedAllocator = Eigen::aligned_allocator<EigenType>;

template <typename T>
DynMatrix<T> IdentityMatrix(const uint size) {
    return Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Identity(size, size);
}

template <typename T>
DynVector<T> IdentityVector(const uint size) {
    DynVector<T> I_vector = DynVector<T>::Zero(size * size);
    for (uint i = 0; i < size; ++i) {
        I_vector[i * size + i] = 1.0;
    }
    return I_vector;
}

template <typename T>
struct SparseMatrixMeta {
    std::vector<Eigen::Triplet<T>> data;

    void add_triplet(const uint row, const uint col, const T value) {
        this->data.emplace_back(Eigen::Triplet<T>(row, col, value));
    }

    void get_sparse_matrix(SparseMatrix<T>& sparse_matrix) { sparse_matrix.setFromTriplets(data.begin(), data.end()); }
};

/* Vector/Matrix (aka Tensor) Operations */
template <typename ArrayType>
void set_constant(ArrayType&& array, const double value) {
    array = std::remove_reference<ArrayType>::type::Constant(
        std::forward<ArrayType>(array).rows(), std::forward<ArrayType>(array).cols(), value);
}

template <typename ArrayType>
decltype(auto) transpose(const ArrayType& array) {
    return array.transpose();
}

template <typename ArrayType>
double norm(const ArrayType& array) {
    return array.norm();
}

template <typename ArrayType>
double sq_norm(const ArrayType& array) {
    return array.squaredNorm();
}

template <typename ArrayType>
decltype(auto) power(const ArrayType& array, const double exp) {
    return array.array().pow(exp);
}

/* Vector Operations */
template <typename VectorType>
uint size(const VectorType& vector) {
    return vector.size();
}

template <typename LeftVectorType, typename RightVectorType>
decltype(auto) vec_cw_mult(const LeftVectorType& vector_left, const RightVectorType& vector_right) {
    return vector_left.cwiseProduct(vector_right);
}

template <typename LeftVectorType, typename RightVectorType>
decltype(auto) vec_cw_div(const LeftVectorType& vector_left, const RightVectorType& vector_right) {
    return vector_left.cwiseQuotient(vector_right);
}

template <typename T>
Eigen::Map<DynVector<T>> vector_from_array(T* array, const uint m) {
    return Eigen::Map<DynVector<T>>(array, m);
}

template <typename VectorType>
decltype(auto) subvector(VectorType&& vector, const uint start_row, const uint size_row) {
    return vector.segment(start_row, size_row);
}

template <typename T, int m, int n = m, int SO = Eigen::StorageOptions::RowMajor>
Eigen::Map<Eigen::Matrix<T, m, n, SO>> reshape(const StatVector<T, m * n>& vector) {
    return Eigen::Map<Eigen::Matrix<T, m, n, SO>>(const_cast<T*>(vector.data()), m, n);
}

template <typename T, int m, int SO = Eigen::StorageOptions::RowMajor>
Eigen::Map<Eigen::Matrix<T, m, Eigen::Dynamic, SO>> reshape(const DynVector<T>& vector, const int n) {
    return Eigen::Map<Eigen::Matrix<T, m, Eigen::Dynamic, SO>>(const_cast<T*>(vector.data()), m, n);
}

template <typename T, int SO = Eigen::StorageOptions::RowMajor>
Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, SO>> reshape(const DynVector<T>& vector,
                                                                         const int m,
                                                                         const int n) {
    return Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, SO>>(const_cast<T*>(vector.data()), m, n);
}

/* Matrix Operations */
template <typename MatrixType>
uint rows(const MatrixType& matrix) {
    return matrix.rows();
}

template <typename MatrixType>
uint columns(const MatrixType& matrix) {
    return matrix.cols();
}

template <typename MatrixType>
decltype(auto) submatrix(MatrixType&& matrix,
                         const uint start_row,
                         const uint start_col,
                         const uint size_row,
                         const uint size_col) {
    return matrix.block(start_row, start_col, size_row, size_col);
}

template <typename MatrixType>
decltype(auto) row(MatrixType&& matrix, const uint row) {
    return matrix.row(row);
}

template <typename MatrixType>
decltype(auto) column(MatrixType&& matrix, const uint col) {
    return matrix.col(col);
}

template <typename MatrixType>
decltype(auto) determinant(MatrixType& matrix) {
    return matrix.determinant();
}

template <typename MatrixType>
decltype(auto) inverse(MatrixType& matrix) {
    return matrix.inverse();
}

template <typename T, int m, int n = m, int SO = Eigen::StorageOptions::RowMajor>
StatVector<T, m * n> flatten(const StatMatrix<T, m, n>& matrix) {
    StatVector<T, m * n> ret;
    Eigen::Map<Eigen::Matrix<double, m, n, SO>>(ret.data(), m, n) = matrix;
    return ret;
}

template <typename T, int m, int SO = Eigen::StorageOptions::RowMajor>
DynVector<T> flatten(const HybMatrix<T, m>& matrix) {
    uint n = matrix.cols();

    DynVector<T> ret(m * n);
    Eigen::Map<Eigen::Matrix<double, m, Eigen::Dynamic, SO>>(ret.data(), m, n) = matrix;
    return ret;
}

template <typename T, int SO = Eigen::StorageOptions::RowMajor>
DynVector<T> flatten(const DynMatrix<T>& matrix) {
    uint m = matrix.rows();
    uint n = matrix.cols();

    DynVector<T> ret(m * n);
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, SO>>(ret.data(), m, n) = matrix;
    return ret;
}

template <typename T, int SO = Eigen::StorageOptions::RowMajor>
void flatten(const DynMatrix<T>& matrix, T* mem) {
    uint m                                                                           = matrix.rows();
    uint n                                                                           = matrix.cols();
    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, SO>>(mem, m, n) = matrix;
}

/* Solving Linear System */
template <typename MatrixType, typename ArrayType>
void solve_sle(MatrixType& A, ArrayType& B) {
    B = A.fullPivLu().solve(B);
}

template <typename ArrayType, typename T>
void solve_sle(SparseMatrix<T>& A_sparse, ArrayType& B) {
    Eigen::SparseLU<SparseMatrix<T>> solver;
    solver.analyzePattern(A_sparse);
    solver.factorize(A_sparse);
    B = solver.solve(B);
}

#endif


#include <boost/random/mersenne_twister.hpp>
#include <boost/random.hpp>
#include <boost/random/binomial_distribution.hpp>
#include <boost/random/variate_generator.hpp>

#include "GenRandomNetworkMapFunction.h"
#include "core/Hash.h"

GenRandomNetworkMapFunction::GenRandomNetworkMapFunction(
  uint64_t _numVertices, double _edgeProbability, uint64_t _maxNeighbors,
  uint64_t _mypeerid, uint64_t _numPeers)
  : numVertices(_numVertices), edgeProbability(_edgeProbability),
    maxNeighbors(_maxNeighbors), mypeerid(_mypeerid), numPeers(_numPeers) {
}

/**
   Each mapper should run exactly one instance of this map function; we assume
   that the function is kick-started by a single <mapper ID / # mappers> pair.

   Each map function is responsible for generating the adjacency lists for
   a disjoint set of approximately (numVertices / numPeers) vertices.

   The vertex structures output by the map function are an array of the
   following form:

   <numNeighbors (uint64_t), firstNeighborID (uint64_t), ..., lastNeighborID
   (uint64_t), vertexPageRankValue (double)>

 */
void GenRandomNetworkMapFunction::map(KeyValuePair& kvPair,
                                      KVPairWriterInterface& writer) {
  uint64_t *pMapperID = (uint64_t*) kvPair.getKey();
  uint64_t *pNumMappers = (uint64_t*) kvPair.getValue();

  uint64_t maxAdjListSize = (numVertices < maxNeighbors) ?
                                              numVertices : maxNeighbors;

  // Need two additional slots to store the number of neighbors and the node's
  // initial PageRank
  uint64_t *outputValue = new uint64_t[maxAdjListSize + 2];

  srand48(Timer::posixTimeInMicros() * getpid());

  // partition at node level Determine the portion of the vertex space for
  // which this node is responsible
  uint64_t numVerticesPerPeer = numVertices / numPeers;
  if (numVertices % numPeers != 0) {
    numVerticesPerPeer++;
  }

  uint64_t nodeBeginningVertex = mypeerid * numVerticesPerPeer;
  uint64_t nodeEndingVertex = nodeBeginningVertex + numVerticesPerPeer;

  if (nodeEndingVertex > numVertices) {
    nodeEndingVertex = numVertices;
  }

  // Determine the range of vertex IDs that this map function is responsible
  // for generating
  uint64_t numVerticesAtThisNode = nodeEndingVertex - nodeBeginningVertex;
  uint64_t numVerticesPerMapper = numVerticesAtThisNode / (*pNumMappers);
  if (numVerticesAtThisNode % (*pNumMappers) != 0) {
    numVerticesPerMapper++;
  }
  uint64_t beginningVertex = nodeBeginningVertex + (*pMapperID) *
    numVerticesPerMapper;

  uint64_t endingVertex = beginningVertex + numVerticesPerMapper;

  if (endingVertex > nodeEndingVertex) {
    endingVertex = nodeEndingVertex;
  }

  unsigned short nrandBuf[3];
  nrandBuf[0] = (unsigned short) Timer::posixTimeInMicros();
  nrandBuf[1] = (unsigned short) getpid();
  nrandBuf[2] = (unsigned short) *pMapperID;

  // We will draw the number of edges from a binomial distribution so that the
  // probability that any two vertices are connected by an edge is roughly
  // equal to edgeProbability

  boost::mt19937 mersenneTwister;
  boost::binomial_distribution<> binomialDistribution(
    numVertices, edgeProbability);
  boost::variate_generator<boost::mt19937&, boost::binomial_distribution<> >
    nextBinomialValue(mersenneTwister, binomialDistribution);

  for (uint64_t i = beginningVertex; i < endingVertex; i++) {
    uint64_t numEdges = 0;

    if ((int) edgeProbability == 1) {
      // If edge probability is 1, generate maxNeighbors random neighbors
      numEdges = maxAdjListSize;
    } else {
      // Draw the number of edges from the binomial distribution
      numEdges = std::min<uint64_t>(nextBinomialValue(), maxAdjListSize);
    }

    for (uint64_t j = 1; j <= numEdges; j++) {
      uint32_t neighbor = nrand48(nrandBuf) % numVertices;
      outputValue[j] = Hash::hash(neighbor);
    }

    double initialPageRank = 1.0 / numVertices;
    outputValue[0] = numEdges;
    memcpy(&outputValue[numEdges + 1], &initialPageRank, sizeof(double));

    KeyValuePair outputKVPair;
    uint64_t key = Hash::hash(i);

    outputKVPair.setKey(reinterpret_cast<const uint8_t *>(&key),
                                                sizeof(uint64_t));
    outputKVPair.setValue(reinterpret_cast<const uint8_t *>(outputValue),
                                 (numEdges + 2) * sizeof(uint64_t));

    writer.write(outputKVPair);
  }

  delete[] outputValue;
}


// Boost.Geometry
// Unit Test

// Copyright (c) 2016-2021 Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Copyright (c) 2018 Adeel Ahmad, Islamabad, Pakistan.

// Contributed and/or modified by Adeel Ahmad, as part of Google Summer of Code 2018 program

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <sstream>

#include "test_formula.hpp"
#include "inverse_cases.hpp"
#include "inverse_cases_antipodal.hpp"
#include "inverse_cases_small_angles.hpp"

#include <boost/geometry/formulas/karney_inverse.hpp>

#include <boost/geometry/srs/spheroid.hpp>

#include <boost/geometry/util/math.hpp>

void test_all(expected_results const& results)
{
    double lon1d = results.p1.lon * bg::math::d2r<double>();
    double lat1d = results.p1.lat * bg::math::d2r<double>();
    double lon2d = results.p2.lon * bg::math::d2r<double>();
    double lat2d = results.p2.lat * bg::math::d2r<double>();

    // WGS84
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);

    bg::formula::result_inverse<double> result_k;

    typedef bg::formula::karney_inverse<double, true, true, true, true, true> ka_t;
    result_k = ka_t::apply(lon1d, lat1d, lon2d, lat2d, spheroid);
    result_k.azimuth *= bg::math::r2d<double>();
    result_k.reverse_azimuth *= bg::math::r2d<double>();
    check_inverse("karney", results, result_k, results.vincenty, results.reference, 0.0000001);
}

template <typename ExpectedResults>
void test_karney(ExpectedResults const& results)
{
    double lon1d = results.p1.lon * bg::math::d2r<double>();
    double lat1d = results.p1.lat * bg::math::d2r<double>();
    double lon2d = results.p2.lon * bg::math::d2r<double>();
    double lat2d = results.p2.lat * bg::math::d2r<double>();

    // WGS84
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);

    bg::formula::result_inverse<double> result;

    typedef bg::formula::karney_inverse<double, true, true, true, true, true> ka_t;
    result = ka_t::apply(lon1d, lat1d, lon2d, lat2d, spheroid);
    result.azimuth *= bg::math::r2d<double>();
    result.reverse_azimuth *= bg::math::r2d<double>();
    check_inverse("karney", results, result, results.karney, results.karney, 0.0000001);
}

int test_main(int, char*[])
{
    for (size_t i = 0; i < expected_size; ++i)
    {
        test_all(expected[i]);
    }

    for (size_t i = 0; i < expected_size_antipodal; ++i)
    {
        test_karney(expected_antipodal[i]);
    }

    for (size_t i = 0; i < expected_size_small_angles; ++i)
    {
        test_karney(expected_small_angles[i]);
    }

    return 0;
}


//
// Created by kerail on 10.07.16.
//
#include "geom/SO3.hpp"

#define BOOST_TEST_MODULE boost_test_so3
#define BOOST_TEST_LOG_LEVEL all
#include <boost/test/unit_test.hpp>

using Eigen::Quaterniond;
using Eigen::Vector3d;
using Eigen::AngleAxisd;

BOOST_AUTO_TEST_CASE(test_so3) {
  const double tol = 1e-5;
  boost::unit_test::unit_test_log.set_threshold_level(boost::unit_test::log_messages);

  for (double i = -1; i <= 1; i += 1) {
    for (double j = -1; j <= 1; j += 1) {
      for (double k = -1; k <= 1; k += 1) {
        for (double a = -2; a <= 2; a += 0.5) {
          Vector3d axisAngle(i, j, k);
          if (fabs(i) + fabs(j) + fabs(k) > 0) {
            axisAngle.normalize();
          }
          double angle = M_PI * a;

          BOOST_TEST_MESSAGE("Axis:\n" << axisAngle << "\n");
          BOOST_TEST_MESSAGE("Angle:\n" << angle << "\n");

          BOOST_TEST_MESSAGE(
              "Rotation SO3:\n" << axisAngle * angle << " \nNorm: " << (axisAngle * angle).norm() << "\n");

          SO3d so3(axisAngle * angle);

          Quaterniond q(AngleAxisd(angle, axisAngle));
          q.normalize();

          SO3d so3FromQ(q);

          Quaterniond rotationQ = so3.getQuaternion();

          Quaterniond rotationQinv = so3.inverted().getQuaternion();
          Quaterniond qInv = q.inverse();

          BOOST_TEST_MESSAGE("Rotation from so3:\n" << rotationQ.coeffs() << "\n");
          BOOST_TEST_MESSAGE("Rotation from q:\n" << q.coeffs() << "\n");

          bool t1 = (rotationQ.coeffs() - q.coeffs()).norm() < tol;
          bool t2 = (rotationQ.coeffs() + q.coeffs()).norm() < tol;

          BOOST_REQUIRE(t1 || t2);

          bool t3 = (rotationQinv.coeffs() - qInv.coeffs()).norm() < tol;
          bool t4 = (rotationQinv.coeffs() + qInv.coeffs()).norm() < tol;

          BOOST_REQUIRE(t3 || t4);

          BOOST_TEST_MESSAGE("SO3:\n" << so3.coeffs() << "\n");
          BOOST_TEST_MESSAGE("SO3 from q:\n" << so3FromQ.coeffs() << "\n");

//          BOOST_REQUIRE((so3FromQ.coeffs() - so3.coeffs()).norm() < tol);
          bool t5 = (so3FromQ.getQuaternion().coeffs() - so3.getQuaternion().coeffs()).norm() < tol;
          bool t6 = (so3FromQ.getQuaternion().coeffs() + so3.getQuaternion().coeffs()).norm() < tol;
          BOOST_REQUIRE(t5 || t6);

          for (double i2 = -1; i2 <= 1; i2 += 1) {
            for (double j2 = -1; j2 <= 1; j2 += 1) {
              for (double k2 = -1; k2 <= 1; k2 += 1) {
                Vector3d v(i2, j2, k2);

                Vector3d checkVector = q.toRotationMatrix() * v;

                Vector3d rotatedVecByFunc = so3.rotateVector(v);
                Vector3d rotatedVecByMatrix = so3.getMatrix() * v;

                BOOST_TEST_MESSAGE("Check vector:\n " << checkVector << "\n\n");
                BOOST_TEST_MESSAGE("Check rotatedVecByFunc:\n " << rotatedVecByFunc << "\n");
                BOOST_TEST_MESSAGE("Check rotationMatrix so3:\n " << so3.getMatrix() << "\n");
                BOOST_TEST_MESSAGE("Check rotationMatrix q:\n " << q.toRotationMatrix() << "\n");
                BOOST_TEST_MESSAGE("Check rotatedVecByMatrix:\n " << rotatedVecByMatrix << "\n");

                BOOST_REQUIRE((rotatedVecByFunc - checkVector).norm() < tol);
                BOOST_REQUIRE((rotatedVecByMatrix - checkVector).norm() < tol);

              }
            }
          }
        }
      }
    }
  }

//  cout << "Original vector: " << v << endl << endl;
//  cout << "Rotate vector by func: " << endl << so3.rotateVector(v) << endl << endl;
//  cout << "Rotation matrix: " << endl << so3.getMatrix() << endl << endl;
//  cout << "Rotate vector by matrix: " << endl << (so3.getMatrix()*v) << endl << endl;
//  cout << "Rotate vector by quaternion: " << endl << (so3.getQuaternion().toRotationMatrix()*v) << endl << endl;
//  cout << "Rotate vector by test quaternion: " << endl << (q.toRotationMatrix()*v) << endl << endl;
}


#include <NTL/FacVec.h>
#include <NTL/ZZ.h>

#include <NTL/new.h>

NTL_START_IMPL

NTL_vector_impl(IntFactor,vec_IntFactor)

static
void swap(IntFactor& x, IntFactor& y)
{
   IntFactor t;

   t = x;  x = y;  y = t;
}

static
void FindMin(FacVec& v, long lo, long hi)
{
   long minv = 0;
   long minp = -1;
   long i;

   for (i = lo; i <= hi; i++) {
      if (minv == 0 || v[i].val < minv) {
         minv = v[i].val;
         minp = i;
      }
   }

   swap(v[lo], v[minp]);
}

void FactorInt(FacVec& fvec, long n)
{
   if (n <= 1) Error("internal error: FactorInt(FacVec,long n) with n<=1");

   if (NTL_OVERFLOW(n, 1, 0))
      Error("internal error: FactorInt(FacVec,long n) with n too large");

   long NumFactors;
   long q;

   fvec.SetLength(2*NextPowerOfTwo(n));

   NumFactors = 0;
   q = 2;

   while (n != 1) {
      if (n%q == 0) {
         fvec[NumFactors].q = q;
         n = n/q;
         fvec[NumFactors].a = 1;
         fvec[NumFactors].val = q;
         while (n%q == 0) {
            n = n/q;
            (fvec[NumFactors].a)++;
            fvec[NumFactors].val *= q;
         }
         fvec[NumFactors].link = -1;
         NumFactors++;
      }

      q++;
   }

   fvec.SetLength(2*NumFactors-1);

   long lo = 0;
   long hi = NumFactors - 1;

   while (lo < hi) {
      FindMin(fvec, lo, hi);
      FindMin(fvec, lo+1, hi);
      hi++;
      fvec[hi].link = lo;
      fvec[hi].val = fvec[lo].val * fvec[lo+1].val;
      lo += 2;
   }
}

NTL_END_IMPL


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>

#include <Eigen/Dense>
#include <sophus/se3.hpp>

#include <kontiki/trajectories/uniform_se3_spline_trajectory.h>
#include "trajectory_helper.h"
#include "spline_helpers.h"

namespace py = pybind11;

namespace TT = kontiki::trajectories;

struct PySE3SplineHelper {
  using PyControlPointType = Eigen::Matrix4d;

  static Eigen::Matrix4d ConvertCppToPy(const Sophus::SE3d & cp) {
    return cp.matrix();
  }

  static Sophus::SE3d ConvertPyToCpp(const Eigen::Matrix4d& cp) {
    ValidateSE3Element(cp); // Throws domain_error on error
    return Sophus::SE3d(cp);
  }

  static void ValidateSE3Element(const Eigen::Matrix4d &cp) {
    double eps = Sophus::Constants<double>::epsilon();
    Eigen::Matrix3d R = cp.topLeftCorner(3, 3);
    if (std::abs<double>(R.determinant() - 1) >= eps) {
      throw std::domain_error("Rotation matrix determinant is not 1!");
    }
    else if ((cp.row(3) - Eigen::Matrix<double, 1, 4>(0, 0, 0, 1)).squaredNorm() >= eps) {
      throw std::domain_error("Final row must be [0, 0, 0, 1]");
    }
  }
};

PYBIND11_MODULE(_uniform_se3_spline_trajectory, m) {
  m.doc() = "Uniform splined trajectory in SE3";

  using Class = TT::UniformSE3SplineTrajectory;
  using Helper = PySE3SplineHelper;
  auto cls = py::class_<Class, std::shared_ptr<Class>>(m, "UniformSE3SplineTrajectory");
  cls.doc() = R"pbdoc( A spline with control points in SE(3)

  Control points are 4x4 matrices T = [R, p; 0, 1].
  )pbdoc";

  cls.def("evaluate", [](Class &self, double t){
    Sophus::SE3d P_SE3;
    Eigen::Matrix4d P, P_prim, P_bis;
    self.EvaluateSpline(t, 0xff, P_SE3, P_prim, P_bis);
    P = P_SE3.matrix();

    return std::make_tuple(P, P_prim, P_bis);
  });

  // Common attributes
  declare_spline_common<Class, Helper>(cls);
  declare_trajectory_common<Class>(cls);

} // PYBIND11_MODULE


#pragma once
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <future>
#include <random>
#include <stdexcept>
#include <thread>
#include <tuple>
#include <unordered_map>

#include "argcheck.hpp"

namespace irspack {
namespace sparse_util {

template <typename Real>
using CSRMatrix = Eigen::SparseMatrix<Real, Eigen::RowMajor>;

template <typename Real>
using DenseVector = Eigen::Matrix<Real, Eigen::Dynamic, 1>;

template <typename Real>
using CSCMatrix = Eigen::SparseMatrix<Real, Eigen::ColMajor>;

template <typename Real>
inline CSRMatrix<Real> parallel_sparse_product(const CSRMatrix<Real> &left,
                                               const CSCMatrix<Real> &right,
                                               const size_t n_thread) {
  CSRMatrix<Real> result(left.rows(), right.cols());
  check_arg(n_thread > 0, "n_thraed must be > 0");
  const int n_row = left.rows();
  const int rows_per_block = n_row / n_thread;
  const int remnant = n_row % n_thread;
  int start = 0;
  std::vector<std::future<CSRMatrix<Real>>> workers;
  for (int i = 0; i < static_cast<int>(n_thread); i++) {
    int block_size = rows_per_block;
    if (i < remnant) {
      ++block_size;
    }
    workers.emplace_back(std::async(std::launch::async, [&left, &right, start,
                                                         block_size]() {
      CSRMatrix<Real> local_result = left.middleRows(start, block_size) * right;
      return local_result;
    }));
    start += block_size;
  }
  start = 0;
  for (int i = 0; i < static_cast<int>(n_thread); i++) {
    int block_size = rows_per_block;
    if (i < remnant) {
      ++block_size;
    }
    result.middleRows(start, block_size) = workers[i].get();
    start += block_size;
  }
  return result;
}

template <typename Real, typename Integer = int64_t>
std::pair<CSCMatrix<Real>, CSRMatrix<Real>>
train_test_split_rowwise(const CSRMatrix<Real> &X, const double test_ratio,
                         std::int64_t random_seed) {
  using Triplet = Eigen::Triplet<Integer>;
  std::mt19937 random_state(random_seed);
  check_arg(((test_ratio <= 1.0 && (test_ratio >= 0.0))),
            "test_ratio must be within [0, 1]");
  std::vector<Integer> col_buffer;
  std::vector<Real> data_buffer;
  std::vector<uint64_t> index_;
  std::vector<Triplet> train_data, test_data;
  for (int row = 0; row < X.outerSize(); ++row) {
    col_buffer.clear(); // does not change capacity
    data_buffer.clear();
    index_.clear();
    Integer cnt = 0;
    for (typename CSRMatrix<Real>::InnerIterator it(X, row); it; ++it) {
      index_.push_back(cnt);
      col_buffer.push_back(it.col());
      data_buffer.push_back(it.value());
      cnt += 1;
    }
    std::shuffle(index_.begin(), index_.end(), random_state);
    size_t n_test = static_cast<Integer>(std::floor(cnt * test_ratio));
    for (size_t i = 0; i < n_test; i++) {
      test_data.emplace_back(row, col_buffer[index_[i]],
                             data_buffer[index_[i]]);
    }
    for (size_t i = n_test; i < col_buffer.size(); i++) {
      train_data.emplace_back(row, col_buffer[index_[i]],
                              data_buffer[index_[i]]);
    }
  }
  CSRMatrix<Real> X_train(X.rows(), X.cols()), X_test(X.rows(), X.cols());
  auto dupfunction = [](const Integer &a, const Integer &b) { return a + b; };
  X_train.setFromTriplets(train_data.begin(), train_data.end(), dupfunction);
  X_test.setFromTriplets(test_data.begin(), test_data.end(), dupfunction);
  X_train.makeCompressed();
  X_test.makeCompressed();
  return {X_train, X_test};
}

template <typename Real>
CSRMatrix<Real> okapi_BM_25_weight(const CSRMatrix<Real> &X, Real k1, Real b) {
  CSRMatrix<Real> result(X);
  using itertype = typename CSRMatrix<Real>::InnerIterator;
  const int N = X.rows();
  result.makeCompressed();
  DenseVector<Real> idf(X.cols());
  DenseVector<Real> doc_length(N);
  idf.array() = 0;
  doc_length.array() = 0;

  for (int i = 0; i < N; i++) {
    for (itertype iter(X, i); iter; ++iter) {
      idf(iter.col()) += 1;
      doc_length(i) += iter.value();
    }
  }
  Real avgdl = doc_length.sum() / N;
  idf.array() =
      (N / (idf.array() + static_cast<Real>(1)) + static_cast<Real>(1)).log();
  for (int i = 0; i < N; i++) {
    Real regularizer = k1 * (1 - b + b * doc_length(i) / avgdl);
    for (itertype iter(result, i); iter; ++iter) {
      iter.valueRef() = idf(iter.col()) * (iter.valueRef() * (k1 + 1)) /
                        (iter.valueRef() + regularizer);
    }
  }
  return result;
}

template <typename Real>
CSRMatrix<Real> tf_idf_weight(const CSRMatrix<Real> &X, bool smooth) {
  CSRMatrix<Real> result(X);
  using itertype = typename CSRMatrix<Real>::InnerIterator;
  const int N = X.rows();
  result.makeCompressed();
  DenseVector<Real> idf(X.cols());
  idf.array() = 0;

  for (int i = 0; i < N; i++) {
    for (itertype iter(X, i); iter; ++iter) {
      idf(iter.col()) += 1;
    }
  }
  idf.array() = (N / (idf.array() + static_cast<Real>(smooth))).log();
  for (int i = 0; i < N; i++) {
    for (itertype iter(result, i); iter; ++iter) {
      iter.valueRef() *= idf(iter.col());
    }
  }
  return result;
}

template <typename Real>
CSRMatrix<Real> remove_diagonal(const CSRMatrix<Real> &X) {
  check_arg(X.rows() == X.cols(), "X must be square");
  CSRMatrix<Real> result(X);
  using itertype = typename CSRMatrix<Real>::InnerIterator;
  const int N = X.rows();
  result.makeCompressed();
  for (int i = 0; i < N; i++) {
    for (itertype iter(result, i); iter; ++iter) {
      if (i == iter.col()) {
        iter.valueRef() = static_cast<Real>(0);
      }
    }
  }
  return result;
}

} // namespace sparse_util
} // namespace irspack


#pragma once
#ifndef DOWNHILL_SIMPLEX_HPP_HPP
#define DOWNHILL_SIMPLEX_HPP_HPP

#include <cmath>
#include <chrono>
#include <vector>
#include <iostream>
#include <algorithm>
#include <functional>
#include <boost/config.hpp>

template<typename _ObjectiveFunction, typename _ContainerType,
         typename _DurationValueType = std::chrono::hours::rep,
         typename _DurationRatio = std::chrono::hours::period>
_ContainerType downhill_simplex(const _ObjectiveFunction& eval, std::vector<_ContainerType> guess,
                         const typename _ContainerType::value_type tolerance = 0.0,
                         const size_t maxIteration = std::numeric_limits<size_t>::max(),
                         const std::chrono::duration<_DurationValueType, _DurationRatio>
                                maxDuration = std::chrono::hours(24 * 365))
{
    using namespace std::chrono;
    using vt = typename _ContainerType::value_type;
    if (guess.empty())
        return {};

    const system_clock::time_point start = system_clock::now();
    const size_t numDimensions = guess.front().size();
    if (guess.size() < numDimensions + 1){
        // initialize simplex points
        _ContainerType avg(numDimensions, 0.0);
        for (const auto& e : guess)
            std::transform(e.cbegin(), e.cend(), avg.cbegin(), avg.begin(), std::plus<vt>());

        std::transform(avg.cbegin(), avg.cend(), avg.begin(), [&guess](const vt& v) -> vt{ return v / guess.size(); });

        guess.reserve(numDimensions - guess.size() + 1);
        for (size_t i = 0; guess.size() < numDimensions + 1; ++i){
            // add adjusted value to guess
            // adjustment: 1 parameter will get +10%
            guess.push_back(avg);
            auto iter = guess.back().begin();
            std::advance(iter, i);
            (*iter) *= 1.10;
        }
    }
    // Paramter
    constexpr const double para_reflect = 1.0, para_expand = 1.0, para_contract = 0.5;

    std::vector<vt> y_currentSimplex;
    y_currentSimplex.reserve(guess.size());
    for (const auto& e : guess)
        y_currentSimplex.push_back(eval(e));

    _ContainerType x_centroid(numDimensions);
    _ContainerType x_expanded(numDimensions);
    _ContainerType x_reflected(numDimensions);
    _ContainerType x_contracted(numDimensions);
    size_t idx_min = std::numeric_limits<size_t>::max();
    size_t idx_max = std::numeric_limits<size_t>::max();

    size_t varianceCounter = 0;
    auto optimizationFinish = [&guess, &varianceCounter, &y_currentSimplex, &tolerance, &maxDuration, &start]() -> bool{
        // time constraint
        if (BOOST_UNLIKELY(system_clock::now() - start > maxDuration))
            return true;

        // tolerance constraint, based on y- or x-value varinace
#ifndef DOWNHILL_SIMPLEX_Y_VLAUE_VARIATION
        double coefVariance = 0.0;
        for (const auto& data : guess){
#else
            const auto& data = y_currentSimplex;
#endif
            double mean = 0.0;
            for (const auto& e : data)
                mean += e;
            mean /= double(data.size());

            double variance = 0.0;
            for (const auto& e : data){
                const double tmp = e - mean;
                variance += tmp * tmp;
            }
#ifndef DOWNHILL_SIMPLEX_Y_VLAUE_VARIATION
            coefVariance += std::sqrt(variance) / std::abs(mean);
        }
        coefVariance /= double(guess.size());
#else
        const double coefVariance = std::sqrt(variance) / mean;
#endif

        if (BOOST_UNLIKELY(coefVariance <= tolerance)){
            ++varianceCounter;
            if (varianceCounter > 3)
                return true;
        }
        else
            varianceCounter = 0;

        return false;
    };

    auto accept = [&idx_max, &guess, &y_currentSimplex](const _ContainerType& x_value, const vt& y_value){
        y_currentSimplex[idx_max] = y_value;
        guess[idx_max] = x_value;
    };
    size_t iterationCounter = 0;
    for (; iterationCounter < maxIteration; ++iterationCounter){
        size_t idx_2ndMax;
        idx_min = 0;
        idx_max = 0;

        // find min, max and 2ndMax
        for (size_t i = 1; i < y_currentSimplex.size(); ++i){
            if (y_currentSimplex[idx_min] > y_currentSimplex[i])
                idx_min = i;
            else if (y_currentSimplex[idx_max] < y_currentSimplex[i])
                idx_max = i;
        }

        idx_2ndMax = idx_min;
        for (size_t i = 1; i < y_currentSimplex.size(); ++i){
            if (y_currentSimplex[idx_2ndMax] < y_currentSimplex[i] && y_currentSimplex[i] < y_currentSimplex[idx_max])
                idx_2ndMax = i;
        }

        // calculate centroid
        std::fill(x_centroid.begin(), x_centroid.end(), vt(0.0));
        for (const auto& e : guess)
            std::transform(e.cbegin(), e.cend(), x_centroid.cbegin(), x_centroid.begin(), std::plus<vt>());

        std::transform(x_centroid.cbegin(), x_centroid.cend(), guess[idx_max].cbegin(), x_centroid.begin(), std::minus<vt>());
        std::transform(x_centroid.cbegin(), x_centroid.cend(), x_centroid.begin(), [&numDimensions](const vt& v) -> vt{
            return v / vt(numDimensions);
        });

        // reflection
        std::transform(x_centroid.cbegin(), x_centroid.cend(), guess[idx_max].cbegin(), x_reflected.begin(),
                       [](const vt& v_centroid, const vt& v_guess) -> vt{
            return (1.0 + para_reflect) * v_centroid - para_reflect * v_guess;
        });

        /// TODO
        /// change to c++17 if with initializer statements
        const vt y_reflected = eval(x_reflected);
        if (y_reflected < y_currentSimplex[idx_min]){ // expansion
            std::transform(x_centroid.cbegin(), x_centroid.cend(), x_reflected.cbegin(), x_expanded.begin(),
                           [](const vt& v_centroid, const vt& v_reflected) -> vt{
                return (1.0 + para_expand) * v_reflected - para_expand * v_centroid;
            });

            /// TODO
            /// change to c++17 if with initializer statements
            const vt y_expanded = eval(x_expanded);
            if (y_expanded < y_currentSimplex[idx_min])
            //if (y_expanded < y_reflected) // IGD version uses this if
                accept(x_expanded, y_expanded);
            else
                accept(x_reflected, y_reflected);
        }
        else if (y_reflected <= y_currentSimplex[idx_2ndMax])
            accept(x_reflected, y_reflected);
        else { // contraction
            if (y_reflected < y_currentSimplex[idx_max])
                accept(x_reflected, y_reflected);

            std::transform(x_centroid.cbegin(), x_centroid.cend(), guess[idx_max].cbegin(), x_contracted.begin(),
                           [](const vt& v_centroid, const vt& v_guess) -> vt{
                return para_contract * v_guess + (1.0 - para_contract) * v_centroid;
            });

            /// TODO
            /// change to c++17 if with initializer statements
            const vt y_contracted = eval(x_contracted);
            if (y_contracted < y_currentSimplex[idx_max])
                accept(x_contracted, y_contracted);
            else{
                // shrink simplex
                for (size_t i = 0; i < guess.size(); ++i){
                    if (BOOST_UNLIKELY(i == idx_min))
                        continue;

                    std::transform(guess[i].cbegin(), guess[i].cend(), guess[idx_min].cbegin(), guess[i].begin(),
                                   [](const vt& lhs, const vt& rhs) -> vt{ return (lhs + rhs) * 0.5; });

                    y_currentSimplex[i] = eval(guess[i]);
                }
            }
        }
        if (optimizationFinish())
            break;
    }
    idx_min = 0;
    for (size_t i = 1; i < y_currentSimplex.size(); ++i){
        if (y_currentSimplex[i] < y_currentSimplex[idx_min])
            idx_min = i;
    }
    double requiredTime = double(duration_cast<microseconds>(system_clock::now() - start).count()) / 1000.0;
    std::string timeExtension = "ms";

#define TMP_TIME_RATIO(nextRatio, nextExtension)                               \
    if (requiredTime > nextRatio) {                                            \
        requiredTime /= nextRatio;                                             \
        timeExtension = nextExtension;

    TMP_TIME_RATIO(1000.0, "s")
        TMP_TIME_RATIO(60.0, "min")
            TMP_TIME_RATIO(60.0, "h")
                TMP_TIME_RATIO(24.0, "days")
                    TMP_TIME_RATIO(7.0, "weeks")
    }   }   }   }   }
#undef TMP_TIME_RATIO

    std::cout << "\nDownhillsimplex finished!\nrequired iterations:  " << iterationCounter
              << "\nrequired   time    :  " << double(int(requiredTime * 100) / 100.0) << ' '
              << timeExtension << '\n' << std::endl;
    return guess.at(idx_min);
}

#endif // DOWNHILL_SIMPLEX_HPP_HPP


#ifndef SEGMENT_3D_HPP
#define SEGMENT_3D_HPP

#include <vector>
#include <Eigen/Dense>

struct Segment3D
{
    Eigen::Vector3d pt0, pt1;

    Segment3D(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1) : pt0(p0), pt1(p1) { }

    void generate_data(std::vector<Eigen::Vector3d>& data, int num_points)
    {
        Eigen::Vector3d v = pt1 - pt0;
        for(int i = 0; i < num_points; ++i)
        {
            data.push_back(pt0 + ((1.0/(num_points-1))*i)*v);
        }
    }
};

#endif // SEGMENT_3D_HPP


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE fold
#include <boost/test/unit_test.hpp>

#include <type_traits>
#include "foldr.hpp"
#include "foldl.hpp"

template<typename left, typename right>
struct max;

template<typename int_type, int_type left, int_type right>
struct max<std::integral_constant<int_type, left>, std::integral_constant<int_type, right>>
{
	using type = std::integral_constant<int_type, left < right ? right : left>;
};

template<typename List, typename Type> struct append;

template<template<typename...> class List, typename... Types, typename Type>
struct append<List<Types...>, Type>
{
	using type = List<Types..., Type>;
};

template<typename...> struct list_t;

BOOST_AUTO_TEST_CASE(fold_max_test)
{
	using list1 =
		std::tuple<
			std::integral_constant<unsigned, 1>,
			std::integral_constant<unsigned, 2>,
			std::integral_constant<unsigned, 3>,
			std::integral_constant<unsigned, 4>,
			std::integral_constant<unsigned, 5>,
			std::integral_constant<unsigned, 6>
		>;

	BOOST_CHECK( (meta::foldr<max, std::integral_constant<unsigned,0>, list1>::type::value == 6) );
	BOOST_CHECK( (meta::foldl<max, std::integral_constant<unsigned,0>, list1>::type::value == 6) );
}

BOOST_AUTO_TEST_CASE(fold_append_test)
{
	using list2 = std::tuple<int,float, double>;
	BOOST_CHECK( (
		std::is_same<
			typename meta::foldl<append, std::tuple<>, list2>::type,
			std::tuple<int, float, double>
		>::value == true
	) );
}