/*

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


#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;
}


// 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;
}


//==============================================================================
//         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


#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


/***********************************************************************/
/**                                                                    */
/** vector2d.hpp                                                       */
/**                                                                    */
/** Copyright (c) 2016, Service Robotics Lab.                          */
/**                     http://robotics.upo.es                         */
/**                                                                    */
/** All rights reserved.                                               */
/**                                                                    */
/** Authors:                                                           */
/** Ignacio Perez-Hurtado (maintainer)                                 */
/** Jesus Capitan                                                      */
/** Fernando Caballero                                                 */
/** Luis Merino                                                        */
/**                                                                    */
/** This software may be modified and distributed under the terms      */
/** of the BSD license. See the LICENSE file for details.              */
/**                                                                    */
/** http://www.opensource.org/licenses/BSD-3-Clause                    */
/**                                                                    */
/***********************************************************************/

#ifndef _VECTOR2D_HPP_
#define _VECTOR2D_HPP_

#include <iostream>
#include <cmath>
//#include <geometry_msgs/Point.h>
#include <boost/functional/hash.hpp>

#include "angle.hpp"

namespace utils
{
class Vector2d
{
public:
  Vector2d() : x(0), y(0)
  {
  }
  Vector2d(double x, double y) : x(x), y(y)
  {
  }
  virtual ~Vector2d()
  {
  }
  double operator()(int index) const
  {
    return index == 0 ? x : y;
  }
  double operator[](int index) const
  {
    return index == 0 ? x : y;
  }
  bool operator==(const Vector2d& other) const
  {
    return x == other.x && y == other.y;
  }
  bool operator<(const Vector2d& other) const
  {
    return x < other.x || (x == other.x && y < other.y);
  }
  double getX() const
  {
    return x;
  }
  double getY() const
  {
    return y;
  }

  /*geometry_msgs::Point toPoint() const
  {
    geometry_msgs::Point p;
    p.x = x;
    p.y = y;
    p.z = 0;
    return p;
  }*/

  Vector2d& set(double x, double y)
  {
    Vector2d::x = x;
    Vector2d::y = y;
    return *this;
  }
  Vector2d& setX(double x)
  {
    Vector2d::x = x;
    return *this;
  }

  Vector2d& setY(double y)
  {
    Vector2d::y = y;
    return *this;
  }

  Vector2d& incX(double inc_x)
  {
    x += inc_x;
    return *this;
  }

  Vector2d& incY(double inc_y)
  {
    y += inc_y;
    return *this;
  }

  Vector2d& inc(double inc_x, double inc_y)
  {
    x += inc_x;
    y += inc_y;
    return *this;
  }

  const Angle angle() const
  {
    return Angle::fromRadian(std::atan2(y, x));
  }

  Angle angleTo(const Vector2d& other) const
  {
    return other.angle() - angle();
  }

  double squaredNorm() const
  {
    return x * x + y * y;
  }

  double norm() const
  {
    return std::sqrt(squaredNorm());
  }

  double dot(const Vector2d& other) const
  {
    return x * other.x + y * other.y;
  }

  Vector2d& normalize()
  {
    double n = norm();
    if (n > 0)
    {
      x /= n;
      y /= n;
    }
    return *this;
  }

  Vector2d normalized() const
  {
    Vector2d v(*this);
    v.normalize();
    return v;
  }

  Vector2d& operator*=(double scalar)
  {
    x *= scalar;
    y *= scalar;
    return *this;
  }
  Vector2d operator*(double scalar) const
  {
    return Vector2d(x * scalar, y * scalar);
  }

  Vector2d& operator/=(double scalar)
  {
    x /= scalar;
    y /= scalar;
    return *this;
  }
  Vector2d operator/(double scalar) const
  {
    return Vector2d(x / scalar, y / scalar);
  }

  Vector2d leftNormalVector() const
  {
    return Vector2d(-y, x);
  }

  Vector2d rightNormalVector() const
  {
    return Vector2d(y, -x);
  }

  Vector2d& operator+=(const Vector2d& other)
  {
    set(x + other.x, y + other.y);
    return *this;
  }
  Vector2d operator+(const Vector2d& other) const
  {
    return Vector2d(x + other.x, y + other.y);
  }
  Vector2d& operator-=(const Vector2d& other)
  {
    set(x - other.x, y - other.y);
    return *this;
  }
  Vector2d operator-(const Vector2d& other) const
  {
    return Vector2d(x - other.x, y - other.y);
  }
  Vector2d operator-() const
  {
    return Vector2d(-x, -y);
  }

  static const Vector2d& Zero()
  {
    static Vector2d zero;
    return zero;
  }

private:
  double x;
  double y;
};
}

inline utils::Vector2d operator*(double scalar, const utils::Vector2d& v)
{
  utils::Vector2d w(v);
  w *= scalar;
  return w;
}

namespace std
{
inline ostream& operator<<(ostream& stream, const utils::Vector2d& v)
{
  stream << "(" << v.getX() << "," << v.getY() << ")";
  return stream;
}

template <>
struct hash<utils::Vector2d>
{
  size_t operator()(const utils::Vector2d& v) const
  {
    using boost::hash_value;
    using boost::hash_combine;
    std::size_t seed = 0;
    hash_combine(seed, hash_value(v[0]));
    hash_combine(seed, hash_value(v[1]));
    return seed;
  }
};
}

#endif


// remainder_tree_array.cpp : This file contains the 'main' function. Program execution begins and ends there.
//

#include <iostream>
#include <chrono>
#include <cmath>
#include <cassert>
#include <random>
#include <vector>
#include <NTL/ZZ.h>
#include <NTL/vector.h>

using namespace std;
using namespace std::chrono;
using namespace NTL;
/*
 * To compile and run in Linux:
 * g++ -o remainder_tree_array remainder_tree_array.cpp -lntl -lgmp -pthread
 * ./remainder_tree_array
 *
 */

/*
 * Returns C[], an array of residues A0 mod m0, A0A1 mod m0m1, etc.
 * A: array of A0, A1, ...
 * m: array of m0, m1, ...
 */
void remainder_tree(Vec<ZZ> &C, Vec<ZZ> &A, Vec<ZZ> &m);
void print_tree(Vec<ZZ> &tree);
void complexity_graph(int N, int d);

int main()
{

	//complexity_graph(1<<20, 1);

	int bound = 1<<20;

	// Testing Wilson's Theorem
	Vec<ZZ> A;
	A.SetLength(bound);
	Vec<ZZ> m;
	m.SetLength(bound);

	for(int i = 0; i < bound; i++){
		A[i] = i+1;
		m[i] = ProbPrime(ZZ(i+1)) ? i+1 : 1;
	}

	/*for(int i = 0; i < A.length(); i++){
		cout << A[i] << " ";
	}
	cout << endl;

	for(int i = 0; i < m.length(); i++){
		cout << m[i] << " ";
	}
	cout << endl;
	*/
	Vec<ZZ> C;
	C.SetLength(bound);

	remainder_tree(C, A, m);

	/*for(int i = 0; i < C.length(); i++){
		cout << C[i] << " ";
	}
	cout << endl;
	*/

}

/*
 * Original Remainder Tree method
 * No space optimizations
 */

void remainder_tree(Vec<ZZ> &C, Vec<ZZ> &A, Vec<ZZ> &m) {
	// Assert that lengths of A, m, C match
	assert(A.length() == m.length());
	assert(A.length() == C.length());

	// Set N = length of input arrays
	int N = A.length();

	// Don't do anything if arrays are trivial
	if(N == 0){
		return;
	}

	// Index of leaf at the bottom left
	int leftmost = 1 << ((int)ceil(log2(N)));

	// Declare trees (always of length 2N for any N)
	Vec<ZZ> ATree;
	ATree.SetLength(2 * N);
	Vec<ZZ> mTree;
	mTree.SetLength(2 * N);
	Vec<ZZ> CTree;
	CTree.SetLength(2 * N);

	/*
	 * For example when N=11 the leaves are in this order:
	 *     / \       /\   /\    /\
	 *    /   \     /  7 8  9 10  11
	 *   /\   /\   /\
	 *  1  2 3  4 5  6
	 *
	 */

	// Initialize the leaves in ATree and mTree
	for (int i = leftmost; i < 2 * N; i++) { // leaves on lowest layer
		ATree[i] = A[i - leftmost];
		mTree[i] = m[i - leftmost];
	}
	for (int i = N; i < leftmost; i++) { // leaves on second lowest layer
		ATree[i] = A[i + N - leftmost];
		mTree[i] = m[i + N - leftmost];
	}

	// Calculate the rest of the product tree mTree
	for (int i = N - 1; i > 0; i--) {
		mTree[i] = mTree[2 * i] * mTree[2 * i + 1]; // parent is product of leaves
	}

	// Calculate the rest of the product tree aTree, taking mod mTree[1] = m[0]*...*m[N-1]
	for(int i = N - 1; i > 0; i--) {
		if ((i & (i+1)) != 0) { // Don't do calculation if on a node in right-most branch
			ATree[i] = (ATree[2 * i] * ATree[2 * i + 1]) % mTree[1]; // parent is product of leaves mod mTree[1]
		}
	}

	// Calculate accumulating remainder tree
	CTree[1] = 1;
	for (int i = 1; i < N; i++) {
		CTree[2 * i] = CTree[i] % mTree[2 * i]; // Left branch
		CTree[2 * i + 1] = (CTree[i] * ATree[2 * i]) % mTree[2 * i + 1]; // Right branch
	}

	//print_tree(ATree);
	//print_tree(mTree);
	//print_tree(CTree);

	for (int i = leftmost; i < 2 * N; i++) {
		C[i - leftmost] = CTree[i];
	}
	for (int i = N; i < leftmost; i++) {
		C[i + N - leftmost] = CTree[i];
	}

	return;
}

/*
 * Prints a tree given in Vec<ZZ> form
 */
void print_tree(Vec<ZZ> &tree){
	int top = 1;
	int counter = 0;
	for(int i = 1; i < tree.length(); i++){
		cout << tree[i] << " ";
		counter++;
		if (counter == top){
			cout << endl;
			counter = 0;
			top *= 2;
		}
	}
	cout << endl;
}

/*
 * Gives data points on size of input vs. computation time.
 * N = max size of data, d = number of data points
 */

void complexity_graph(int N, int d){
	vector<int> x;
	vector<int> y;

	int interval = N/d;
	int B = 0;
	while(B <= N){

		int testSize = B;
		int numSize = B;

		Vec<ZZ> test_A;
		test_A.SetLength(testSize);
		Vec<ZZ> test_m;
		test_m.SetLength(testSize);
		for (int i = 0; i < testSize; i++) {
			test_A[i] = rand() % numSize + 1;
			test_m[i] = rand() % numSize + 1;
		}
		/*
		for (int i = 0; i < testSize; i++) {
			cout << test_A[i] << " ";
		}
		cout << endl;

		for (int i = 0; i < testSize; i++) {
			cout << test_m[i] << " ";
		}
		cout << endl;
		*/

		uint64_t start = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
		Vec<ZZ> test_C;
		test_C.SetLength(testSize);
		remainder_tree(test_C, test_A, test_m);

		/*for (int i = 0; i < testSize; i++) {
			cout << test_C[i] << " ";
		}
		cout << endl;
		*/

		uint64_t end = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();

		x.push_back(B);
		y.push_back(end-start);

		B += interval;
	}

	for(int i = 0; i < x.size(); i++){
		cout << x[i] << ", ";
	}
	cout << endl;

	for(int i = 0; i < y.size(); i++){
		cout << y[i] << ", ";
	}
	cout << endl;

}

// Run program: Ctrl + F5 or Debug > Start Without Debugging menu
// Debug program: F5 or Debug > Start Debugging menu

// Tips for Getting Started:
//   1. Use the Solution Explorer window to add/manage files
//   2. Use the Team Explorer window to connect to source control
//   3. Use the Output window to see build output and other messages
//   4. Use the Error List window to view errors
//   5. Go to Project > Add New Item to create new code files, or Project > Add Existing Item to add existing code files to the project
//   6. In the future, to open this project again, go to File > Open > Project and select the .sln file


/*!
 * @file dirichlet_bc.hpp
 * @brief Contains implementation of Dirichlet boundary conditions.
 * @author Konrad Simon
 * @date August 2019
 */

#ifndef INCLUDE_DIRICHLET_BC_HPP_
#define INCLUDE_DIRICHLET_BC_HPP_

// Deal.ii
#include <deal.II/base/function.h>

// STL
#include <cmath>
#include <fstream>

// My Headers
#include "coefficients.h"

namespace Coefficients
{
using namespace dealii;

/*!
 * @class DirichletBC
 * @brief Class implements scalar Dirichlet conditions.
 */
template <int dim>
class DirichletBC : public Function<dim>
{
public:
	DirichletBC() : Function<dim>() {}

	virtual double value(const Point<dim> &p,
						const unsigned int component = 0) const override;
	virtual void value_list(const std::vector<Point<dim>> &points,
								std::vector<double>  &values,
								const unsigned int component = 0) const override;
};

template <int dim>
double
DirichletBC<dim>::value(const Point<dim>& p,
							   const unsigned int /*component*/) const
{
	double return_value = (p(0)-0.5) * (p(0)-0.5) + (p(1)-0.5) * (p(1)-0.5);

	return return_value;
}

template <int dim>
void
DirichletBC<dim>::value_list(const std::vector<Point<dim>> &points,
								std::vector<double>  &values,
								const unsigned int /*component = 0*/) const
{
	Assert (points.size() == values.size(),
			ExcDimensionMismatch (points.size(), values.size()) );

	for ( unsigned int p=0; p<points.size(); ++p)
	{
		values[p] = (points[p](0)-0.5) * (points[p](0)-0.5) + (points[p](1)-0.5) * (points[p](1)-0.5);
	} // end ++p
}

} // end namespace Coefficients

#endif /* INCLUDE_DIRICHLET_BC_HPP_ */


#ifndef __KALMANFILTER_HPP__
#define __KALMANFILTER_HPP__

#include <Eigen/Dense>
#include <utility>

class KalmanFilter {
 public:
 	struct KalmanState {
 		Eigen::VectorXd state;
 		Eigen::MatrixXd errorCovariance;
 	};

 	KalmanFilter(
    const int pStateDimensionality,
    const int pMeasurementDimensionality);

  void setNaturalModel(const Eigen::MatrixXd& pNewModel);
  void setControlModel(const Eigen::MatrixXd& pNewModel);
  void setTransitionModel(const Eigen::MatrixXd& pNewModel);
  void setStateNoiseCovariance(const Eigen::MatrixXd& pNewCovariance);
  void setMeasurementNoiseCovariance(const Eigen::MatrixXd& pNewCovariance);

	KalmanState KalmanFilterIteration(
		const KalmanState& pPreviousState,
		const Eigen::MatrixXd& pMeasurementVector,
		const Eigen::VectorXd& pControlVector);

 	Eigen::MatrixXd computeStatePrediction(
 		const Eigen::VectorXd& pPreviousState,
 		const Eigen::VectorXd& pControlVector);

 	Eigen::MatrixXd computeObservationPrediction(
 		const Eigen::VectorXd& pStatePrediction);

 	Eigen::MatrixXd computeErrorCovariancePrediction(
 		const Eigen::MatrixXd& pPreviousPredictionCovariance);

 	Eigen::MatrixXd computeKalmanGainFactor(
 		const Eigen::MatrixXd& pPredictionCovarianceEstimate);

 	Eigen::MatrixXd computeStateEstimate(
 		const Eigen::VectorXd& pStatePrediction,
 		const Eigen::MatrixXd& pKalmanGainFactor,
 		const Eigen::VectorXd& pMeasurementVector,
 		const Eigen::VectorXd& pObservationPrediction);

 	Eigen::MatrixXd computeErrorCovariance(
 		const Eigen::MatrixXd& pPredictionCovarianceEstimate,
 		const Eigen::MatrixXd& pKalmanGainFactor);

 private:
  int mStateDimensionality;       // aka n
  int mMeasurementDimensionality; // aka m
  Eigen::MatrixXd mNaturalModel;  // aka A; n x n; describes how the system
                                  // evolves naturally, i.e. without controls
                                  // or noise
  Eigen::MatrixXd mControlModel;  // aka B; n x n; describes how controls alter
                                  // the system
  Eigen::MatrixXd mTransitionModel; // aka H; m x n; describes how to map from
                                    // a state to an observation
  Eigen::VectorXd mStateNoise;    // aka \epsilon; n x 1; has covariance Q
  Eigen::MatrixXd mStateNoiseCovariance;  // aka Q;
  Eigen::VectorXd mMeasurementNoise; // aka \sigma; m x 1; has covariance R
  Eigen::MatrixXd mMeasurementNoiseCovariance;  // aka R; m x m
};

#endif //__KALMANFILTER_HPP__


// Boost.Geometry

// Copyright (c) 2015-2017 Oracle and/or its affiliates.

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

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

#ifndef BOOST_GEOMETRY_FORMULAS_THOMAS_INVERSE_HPP
#define BOOST_GEOMETRY_FORMULAS_THOMAS_INVERSE_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>

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

#include <boost/geometry/formulas/differential_quantities.hpp>
#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/result_inverse.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates,
       Forsyth-Andoyer-Lambert type approximation with second order terms.
\author See
    - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
      http://www.dtic.mil/docs/citations/AD0627893
    - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
      http://www.dtic.mil/docs/citations/AD0703541
*/
template <
    typename CT,
    bool EnableDistance,
    bool EnableAzimuth,
    bool EnableReverseAzimuth = false,
    bool EnableReducedLength = false,
    bool EnableGeodesicScale = false
>
class thomas_inverse
{
    static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
    static const bool CalcAzimuths = EnableAzimuth || EnableReverseAzimuth || CalcQuantities;
    static const bool CalcFwdAzimuth = EnableAzimuth || CalcQuantities;
    static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;

public:
    typedef result_inverse<CT> result_type;

    template <typename T1, typename T2, typename Spheroid>
    static inline result_type apply(T1 const& lon1,
                                    T1 const& lat1,
                                    T2 const& lon2,
                                    T2 const& lat2,
                                    Spheroid const& spheroid)
    {
        result_type result;

        // coordinates in radians

        if ( math::equals(lon1, lon2) && math::equals(lat1, lat2) )
        {
            return result;
        }

        CT const c0 = 0;
        CT const c1 = 1;
        CT const c2 = 2;
        CT const c4 = 4;

        CT const pi_half = math::pi<CT>() / c2;
        CT const f = formula::flattening<CT>(spheroid);
        CT const one_minus_f = c1 - f;

//        CT const tan_theta1 = one_minus_f * tan(lat1);
//        CT const tan_theta2 = one_minus_f * tan(lat2);
//        CT const theta1 = atan(tan_theta1);
//        CT const theta2 = atan(tan_theta2);

        CT const theta1 = math::equals(lat1, pi_half) ? lat1 :
                          math::equals(lat1, -pi_half) ? lat1 :
                          atan(one_minus_f * tan(lat1));
        CT const theta2 = math::equals(lat2, pi_half) ? lat2 :
                          math::equals(lat2, -pi_half) ? lat2 :
                          atan(one_minus_f * tan(lat2));

        CT const theta_m = (theta1 + theta2) / c2;
        CT const d_theta_m = (theta2 - theta1) / c2;
        CT const d_lambda = lon2 - lon1;
        CT const d_lambda_m = d_lambda / c2;

        CT const sin_theta_m = sin(theta_m);
        CT const cos_theta_m = cos(theta_m);
        CT const sin_d_theta_m = sin(d_theta_m);
        CT const cos_d_theta_m = cos(d_theta_m);
        CT const sin2_theta_m = math::sqr(sin_theta_m);
        CT const cos2_theta_m = math::sqr(cos_theta_m);
        CT const sin2_d_theta_m = math::sqr(sin_d_theta_m);
        CT const cos2_d_theta_m = math::sqr(cos_d_theta_m);
        CT const sin_d_lambda_m = sin(d_lambda_m);
        CT const sin2_d_lambda_m = math::sqr(sin_d_lambda_m);

        CT const H = cos2_theta_m - sin2_d_theta_m;
        CT const L = sin2_d_theta_m + H * sin2_d_lambda_m;
        CT const cos_d = c1 - c2 * L;
        CT const d = acos(cos_d);
        CT const sin_d = sin(d);

        CT const one_minus_L = c1 - L;

        if ( math::equals(sin_d, c0)
          || math::equals(L, c0)
          || math::equals(one_minus_L, c0) )
        {
            return result;
        }

        CT const U = c2 * sin2_theta_m * cos2_d_theta_m / one_minus_L;
        CT const V = c2 * sin2_d_theta_m * cos2_theta_m / L;
        CT const X = U + V;
        CT const Y = U - V;
        CT const T = d / sin_d;
        CT const D = c4 * math::sqr(T);
        CT const E = c2 * cos_d;
        CT const A = D * E;
        CT const B = c2 * D;
        CT const C = T - (A - E) / c2;

        CT const f_sqr = math::sqr(f);
        CT const f_sqr_per_64 = f_sqr / CT(64);

        if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
        {
            CT const n1 = X * (A + C*X);
            CT const n2 = Y * (B + E*Y);
            CT const n3 = D*X*Y;

            CT const delta1d = f * (T*X-Y) / c4;
            CT const delta2d = f_sqr_per_64 * (n1 - n2 + n3);

            CT const a = get_radius<0>(spheroid);

            //result.distance = a * sin_d * (T - delta1d);
            result.distance = a * sin_d * (T - delta1d + delta2d);
        }

        if ( BOOST_GEOMETRY_CONDITION(CalcAzimuths) )
        {
            // NOTE: if both cos_latX == 0 then below we'd have 0 * INF
            // it's a situation when the endpoints are on the poles +-90 deg
            // in this case the azimuth could either be 0 or +-pi
            // but above always 0 is returned

            CT const F = c2*Y-E*(c4-X);
            CT const M = CT(32)*T-(CT(20)*T-A)*X-(B+c4)*Y;
            CT const G = f*T/c2 + f_sqr_per_64 * M;

            // TODO:
            // If d_lambda is close to 90 or -90 deg then tan(d_lambda) is big
            // and F is small. The result is not accurate.
            // In the edge case the result may be 2 orders of magnitude less
            // accurate than Andoyer's.
            CT const tan_d_lambda = tan(d_lambda);
            CT const Q = -(F*G*tan_d_lambda) / c4;
            CT const d_lambda_m_p = (d_lambda + Q) / c2;
            CT const tan_d_lambda_m_p = tan(d_lambda_m_p);

            CT const v = atan2(cos_d_theta_m, sin_theta_m * tan_d_lambda_m_p);
            CT const u = atan2(-sin_d_theta_m, cos_theta_m * tan_d_lambda_m_p);

            CT const pi = math::pi<CT>();

            if (BOOST_GEOMETRY_CONDITION(EnableAzimuth))
            {
                CT alpha1 = v + u;
                if (alpha1 > pi)
                {
                    alpha1 -= c2 * pi;
                }

                result.azimuth = alpha1;
            }

            if (BOOST_GEOMETRY_CONDITION(EnableReverseAzimuth))
            {
                CT alpha2 = pi - (v - u);
                if (alpha2 > pi)
                {
                    alpha2 -= c2 * pi;
                }

                result.reverse_azimuth = alpha2;
            }
        }

        if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
        {
            typedef differential_quantities<CT, EnableReducedLength, EnableGeodesicScale, 2> quantities;
            quantities::apply(lon1, lat1, lon2, lat2,
                              result.azimuth, result.reverse_azimuth,
                              get_radius<2>(spheroid), f,
                              result.reduced_length, result.geodesic_scale);
        }

        return result;
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_THOMAS_INVERSE_HPP


/**
 * @file stableevaluationatapoint.cc
 * @brief NPDE homework StableEvaluationAtAPoint
 * @author Amélie Loher, Erick Schulz & Philippe Peter
 * @date 29.11.2021
 * @copyright Developed at ETH Zurich
 */

#include "stableevaluationatapoint.h"

#include <lf/base/base.h>
#include <lf/fe/fe.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/quad/quad.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <iostream>
#include <memory>

namespace StableEvaluationAtAPoint {

double MeshSize(const std::shared_ptr<const lf::mesh::Mesh> &mesh_p) {
  double mesh_size = 0.0;
  // Find maximal edge length
  for (const lf::mesh::Entity *edge : mesh_p->Entities(1)) {
    // Compute the length of the edge
    double edge_length = lf::geometry::Volume(*(edge->Geometry()));
    mesh_size = std::max(edge_length, mesh_size);
  }
  return mesh_size;
}

Eigen::Vector2d OuterNormalUnitSquare(Eigen::Vector2d x) {
  // Use shortcut: x is on the unit square
  if (x(0) > x(1) && x(0) < 1.0 - x(1)) {
    return Eigen::Vector2d(0.0, -1.0);
  }
  if (x(0) > x(1) && x(0) > 1.0 - x(1)) {
    return Eigen::Vector2d(1.0, 0.0);
  }
  if (x(0) < x(1) && x(0) > 1.0 - x(1)) {
    return Eigen::Vector2d(0.0, 1.0);
  }
  return Eigen::Vector2d(-1.0, 0.0);
}

double FundamentalSolution::operator()(Eigen::Vector2d y) {
  LF_ASSERT_MSG(x_ != y, "G not defined for these coordinates!");
  return -1.0 / (2.0 * M_PI) * std::log((x_ - y).norm());
}

Eigen::Vector2d FundamentalSolution::grad(Eigen::Vector2d y) {
  LF_ASSERT_MSG(x_ != y, "G not defined for these coordinates!");
  return (x_ - y) / (2.0 * M_PI * (x_ - y).squaredNorm());
}

double PointEval(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  double error = 0.0;
#if SOLUTION
  const auto u = [](Eigen::Vector2d x) -> double {
    Eigen::Vector2d one(1.0, 0.0);
    return std::log((x + one).norm());
  };
  const auto gradu = [](Eigen::Vector2d x) -> Eigen::Vector2d {
    Eigen::Vector2d one(1.0, 0.0);
    return (x + one) / (x + one).squaredNorm();
  };
  // Define a Functor for the dot product of grad u(x) * n(x)
  const auto gradu_dot_n = [gradu](const Eigen::Vector2d x) -> double {
    // Determine the normal vector n on the unit square
    Eigen::Vector2d n = OuterNormalUnitSquare(x);
    return gradu(x).dot(n);
  };

  // Compute right hand side
  const Eigen::Vector2d x(0.3, 0.4);
  const double rhs = PSL(mesh_p, gradu_dot_n, x) - PDL(mesh_p, u, x);
  // Compute the error
  error = std::abs(u(x) - rhs);
#else
  //====================
  // Your code goes here
  //====================
#endif
  return error;
}

double Psi::operator()(Eigen::Vector2d y) {
  const double c = M_PI / (0.5 * std::sqrt(2) - 1.0);
  const double dist = (y - center_).norm();

  if (dist <= 0.25 * std::sqrt(2)) {
    return 0.0;
  } else if (dist >= 0.5) {
    return 1.0;
  } else {
    return std::pow(std::cos(c * (dist - 0.5)), 2);
  }
}

Eigen::Vector2d Psi::grad(Eigen::Vector2d y) {
  double c = M_PI / (0.5 * std::sqrt(2) - 1.0);
  double dist = (y - center_).norm();

  if (dist <= 0.25 * std::sqrt(2)) {
    return Eigen::Vector2d(0.0, 0.0);

  } else if (dist >= 0.5) {
    return Eigen::Vector2d(0.0, 0.0);
  } else {
    return -2.0 * std::cos(c * (dist - 0.5)) * std::sin(c * (dist - 0.5)) *
           (c / dist) * (y - center_);
  }
}

double Psi::lapl(Eigen::Vector2d y) {
  double c = M_PI / (0.5 * std::sqrt(2) - 1.0);
  double c2 = c * c;
  double dist = (y - center_).norm();

  if (dist <= 0.25 * std::sqrt(2)) {
    return 0.0;
  } else if (dist >= 0.5) {
    return 0.0;
  } else {
    double sineval = std::sin(c * (dist - 0.5));
    double coseval = std::cos(c * (dist - 0.5));
    return 2 * c2 * sineval * sineval - 2 * c2 * coseval * coseval -
           2 * c * sineval * coseval / dist;
  }
}

double Jstar(std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space,
             Eigen::VectorXd uFE, const Eigen::Vector2d x) {
  double val = 0.0;
  Psi psi(Eigen::Vector2d(0.5, 0.5));
  FundamentalSolution G(x);
#if SOLUTION

  // Mesh covering a unit square domain
  std::shared_ptr<const lf::mesh::Mesh> mesh = fe_space->Mesh();
  // Use midpoint quadrature rule
  const lf::quad::QuadRule qr = lf::quad::make_TriaQR_MidpointRule();
  // Quadrature points
  const Eigen::MatrixXd zeta_ref{qr.Points()};
  // Quadrature weights
  const Eigen::VectorXd w_ref{qr.Weights()};
  // Number of quadrature points
  const lf::base::size_type P = qr.NumPoints();
  // Create mesh function to be evaluated at the quadrature points
  auto uFE_mf = lf::fe::MeshFunctionFE(fe_space, uFE);

  // Loop over all cells
  for (const lf::mesh::Entity *entity : mesh->Entities(0)) {
    // Standard way to apply a local quadrature rule
    const lf::geometry::Geometry &geo{*entity->Geometry()};
    // Quadrature points on actual cell
    const Eigen::MatrixXd zeta{geo.Global(zeta_ref)};
    const Eigen::VectorXd gram_dets{geo.IntegrationElement(zeta_ref)};
    // Values of finite element function on all quadrature points
    auto u_vals = uFE_mf(*entity, zeta_ref);

    // Quadrature loop
    for (int l = 0; l < P; l++) {
      const double w = w_ref[l] * gram_dets[l];
      val += w * (-u_vals[l]) *
             (2.0 * (G.grad(zeta.col(l))).dot(psi.grad(zeta.col(l))) +
              G(zeta.col(l)) * psi.lapl(zeta.col(l)));
    }
  }
#else
  //====================
  // Your code goes here
  //====================
#endif
  return val;
}

double StablePointEvaluation(
    std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space,
    Eigen::VectorXd uFE, const Eigen::Vector2d x) {
  double res = 0.0;

  Eigen::Vector2d center(0.5, 0.5);
  if ((x - center).norm() <= 0.25) {
    res = Jstar(fe_space, uFE, x);
  } else {
    std::cerr << "The point does not fulfill the assumptions" << std::endl;
  }

  return res;
}

double EvaluateFEFunction(
    std::shared_ptr<lf::uscalfe::FeSpaceLagrangeO1<double>> fe_space,
    const Eigen::VectorXd &uFE, Eigen::Vector2d global, double tol) {
  // Extract mesh
  auto mesh_p = fe_space->Mesh();
  // wrap coefficient vector into a FE mesh-function
  lf::fe::MeshFunctionFE mf(fe_space, uFE);

  for (const lf::mesh::Entity *entity_p : mesh_p->Entities(0)) {
    LF_ASSERT_MSG(lf::base::RefEl::kTria() == entity_p->RefEl(),
                  "Function only defined for triangular cells");

    // compute geometric information about the cell
    const lf::geometry::Geometry *geo_p = entity_p->Geometry();
    Eigen::MatrixXd corners = lf::geometry::Corners(*geo_p);

    // transform global coordinates to local coordinates on the cell
    Eigen::Matrix2d A;
    A << corners.col(1) - corners.col(0), corners.col(2) - corners.col(0);
    Eigen::Vector2d b;
    b << global - corners.col(0);
    Eigen::Vector2d loc = A.fullPivLu().solve(b);

    // evaluate meshfunction, if local coordinates lie in the reference triangle
    if (loc(0) >= 0 - tol && loc(1) >= 0 - tol && loc(0) + loc(1) <= 1 + tol) {
      return mf(*entity_p, loc)[0];
    }
  }
  return 0.0;
}

}  // namespace StableEvaluationAtAPoint


#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <armadillo>
#include <cmath>
#include <cstdlib>
#include <iomanip>
#include <time.h>
#include "func.hpp"
using namespace std;

const int max_iter = 200;
const double err_norm = 1e-14;

arma::vec Iterator(long long unsigned int n, arma::vec iter_values, double alpha);
arma::mat derivative_matrix(long long unsigned int n, double x2, double p1, double alpha);
arma::vec method_newton(long long unsigned int n, arma::mat D, double x2, double p1, double alpha);
arma::vec runge_kutta_5(long long unsigned int n, double x1, double x2, double p1, double p2, double alpha);
arma::vec ini_k(arma::vec Y, double alpha, double t, double h);
arma::mat inverse_matrix(arma::mat D);
double fedorenko_norm(arma::mat D, arma::vec iter_values, long long unsigned int n, double alpha);

arma::vec Iterator(long long unsigned int n, arma::vec iter_values, double alpha) {
    arma::mat d_matrix(static_cast<arma::uword>(2), static_cast<arma::uword>(2), arma::fill::zeros);
    double norm;

    for (int iter = 0; iter < max_iter; iter++){
        d_matrix = derivative_matrix(n, iter_values(0), iter_values(1), alpha);
        norm = fedorenko_norm(d_matrix, iter_values, n, alpha);
        if (norm < err_norm) {
            cout << setprecision(2) << defaultfloat << "step = " << iter << endl;
			cout << setprecision(2) << defaultfloat << "alpha = " << alpha << endl;
			cout << "------" << endl;
            cout << setprecision(13) << fixed << "x2(0) = " << iter_values(0) << endl;
			cout << setprecision(13) << fixed << "p1(0) = " << iter_values(1) << endl;
            cout << scientific << "norm = " << norm << endl << endl;
            return iter_values;
        }
        d_matrix = inverse_matrix(d_matrix);
        iter_values = method_newton(n, d_matrix, iter_values(0), iter_values(1), alpha);
    }

    return iter_values;
}

arma::mat derivative_matrix(long long unsigned int n, double x2, double p1, double alpha) {
    arma::vec value(n + 1, arma::fill::zeros);
    arma::mat d_matrix(static_cast<arma::uword>(2), static_cast<arma::uword>(2), arma::fill::zeros);
    double h = 1e-10;

    value = runge_kutta_5(n, 1.0, x2 + h, p1, 0.0, alpha);
    d_matrix(0, 0) += value(n - 1);
    d_matrix(1, 0) += value(n);
    value = runge_kutta_5(n, 1.0, x2 - h, p1, 0.0, alpha);
    d_matrix(0, 0) -= value(n - 1);
	d_matrix(1, 0) -= value(n);
    value = runge_kutta_5(n, 1.0, x2, p1 + h, 0.0, alpha);
    d_matrix(0, 1) += value(n - 1);
    d_matrix(1, 1) += value(n);
	value = runge_kutta_5(n, 1.0, x2, p1 - h, 0.0, alpha);
    d_matrix(0, 1) -= value(n - 1);
    d_matrix(1, 1) -= value(n);

    d_matrix = d_matrix/(2.0 * h);
    return d_matrix;
}

double fedorenko_norm(arma::mat d_matrix, arma::vec iter_values, long long unsigned int n, double alpha) {
    arma::vec value(n + 1, arma::fill::zeros);

    value = runge_kutta_5(n, 1.0, iter_values(0), iter_values(1), 0.0, alpha);

    return sqrt((value(n - 1)*value(n - 1)/(d_matrix(0,0)*d_matrix(0,0) + d_matrix(0,1)*d_matrix(0,1))) + (value(n)*value(n)/(d_matrix(1,0)*d_matrix(1,0) + d_matrix(1,1)*d_matrix(1,1))));
}

arma::mat inverse_matrix(arma::mat d_matrix) {
    double temp = 0.0;
    arma::mat value_matrix(static_cast<arma::uword>(2), static_cast<arma::uword>(2), arma::fill::zeros);

    temp = 1/((d_matrix(0,0)*d_matrix(1,1)) - (d_matrix(0,1)*d_matrix(1,0)));
    value_matrix(0, 0) = temp*d_matrix(1, 1);
    value_matrix(0, 1) = - temp*d_matrix(0, 1);
    value_matrix(1, 0) = - temp*d_matrix(1, 0);
    value_matrix(1, 1) = temp*d_matrix(0, 0);
    return value_matrix;
}

arma::vec method_newton(long long unsigned int n, arma::mat D, double x2, double p1, double alpha) {
    arma::vec value(n + 1, arma::fill::zeros);
    arma::vec iter_values(2, arma::fill::zeros);

    value = runge_kutta_5(n, 1.0, x2, p1, 0.0, alpha);
    iter_values(0) = x2 - D(0, 0) * value(n-1) - D(0, 1) * value(n);
    iter_values(1) = p1 - D(1, 0) * value(n-1) - D(1, 1) * value(n);

    return iter_values;
}

arma::vec runge_kutta_5(long long unsigned int n, double x1, double x2, double p1, double p2, double alpha) {
    arma::vec ODE(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec ODE1(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec ODE2(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec value(n + 1, arma::fill::zeros);
    double h = 1.0/static_cast<double>(n);
    double t = 0.0;
    double err = 1e-15;
    bool working = true;

    ODE(0) = x1;
    ODE(1) = x2;
    ODE(2) = p1;
    ODE(3) = p2;

    while (working) {
        if (t + h >= 1.0) {
            h = 1.0 - t;
            working = false;
        }
		ODE1 = ini_k(ODE, alpha, t, h/2.0);
        ODE2 = ODE1 + ini_k(ODE + ODE1, alpha, t, h/2.0);
        ODE1 = ini_k(ODE, alpha, t, h);

        if (arma::max(arma::abs(ODE1 - ODE2)) > err) {
            h = h/2.0;
        } else
        if (arma::max(arma::abs(ODE1 - ODE2)) < err/16) {
            t += h;
            h = h*1.5;
            ODE += ODE2;
        } else {
            t += h;
            ODE += ODE2;
        }
    }

    value(n-1) = ODE(0);
    value(n) = ODE(1);

    return value;
}

arma::vec ini_k(arma::vec Y, double alpha, double t, double h) {
    arma::vec k1(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec k2(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec k3(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec k4(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec k5(static_cast<arma::uword>(4), arma::fill::zeros);
    arma::vec k6(static_cast<arma::uword>(4), arma::fill::zeros);

    k1 = h * init_func(Y, alpha, t);
    k2 = h * init_func(Y + (k1/2.0), alpha, t);
    k3 = h * init_func(Y + ((k1 + k2)/4.0), alpha, t);
    k4 = h * init_func(Y - k2 + 2.0 * k3, alpha, t);
    k5 = h * init_func(Y + ((7.0 * k1 + 10.0 * k2 + k4)/27.0), alpha, t);
    k6 = h * init_func(Y + ((28.0 * k1 - 125.0 * k2 + 546.0 * k3 + 54.0 * k4 - 378 * k5)/625.0), alpha, t);
    return k1/24.0 + (5.0 * k4)/48.0 + (27.0 * k5)/56.0 + (125.0 * k6)/336.0;
}