/*
 * FullyIndependentTrainingConditional.cpp
 *
 *  Created on: Nov 8, 2016
 *      Author: menhorn
 */

#include <FullyIndependentTrainingConditional.hpp>
#include <random>
#include <assert.h>
#include <algorithm>
#include <iostream>
#include <limits.h>
#include <cmath>

 #include <Eigen/Dense>

//--------------------------------------------------------------------------------
FullyIndependentTrainingConditional::FullyIndependentTrainingConditional(int n,
		double &delta_input) :
		GaussianProcess(n, delta_input, nullptr) {

}
//--------------------------------------------------------------------------------
FullyIndependentTrainingConditional::FullyIndependentTrainingConditional(int n,
		double &delta_input, std::vector<double> gp_parameters_input) :
		GaussianProcess(n, delta_input, nullptr, gp_parameters_input) {

}
//--------------------------------------------------------------------------------
double FullyIndependentTrainingConditional::evaluate_kernel ( VectorXd const &x,
                                          VectorXd const &y )
{
  return evaluate_kernel ( x, y, gp_parameters );
}
//--------------------------------------------------------------------------------
double FullyIndependentTrainingConditional::evaluate_kernel ( VectorXd const &x,
                                          VectorXd const &y,
                                          std::vector<double> const &p )
{
  dist = 0e0;
  for ( int i = 0; i < dim; ++i )
    dist += pow( (x(i) - y(i)), 2e0) /  p.at( i+1 );
  kernel_evaluation = exp(-dist / 2e0 );

  return kernel_evaluation * p.at( 0 ) ;
}

double FullyIndependentTrainingConditional::evaluate_kernel1D_exp_term(double const &x,
                              double const &y,
                              double const &l ){
	return exp(-0.5*((x-y)*(x-y))/ l);
}

void FullyIndependentTrainingConditional::derivate_K_u_u_wrt_uik(std::vector<double> const &p,
																	 int const &i,
																	 int const &k,
																	 MatrixXd &deriv_matrix ){
	const int nb_u_nodes = u.rows();
	deriv_matrix.resize(nb_u_nodes, nb_u_nodes);
	deriv_matrix.setZero();
	double C = 0;
	for(int j = 0; j < nb_u_nodes; ++j){
		C = p[0]; //sigma_f^2
		for(int d = 0; d < dim; ++d){
			C *= evaluate_kernel1D_exp_term(u(i, d), u(j, d), p[1+d]);
		}
		deriv_matrix(i,j) = C * (-1) * (u(i, k)-u(j, k))/p[k+1];
	}
	for(int j = 0; j < nb_u_nodes; ++j){
		deriv_matrix(j,i) = deriv_matrix(i,j);
	}
	/*//Good
	MatrixXd K_u_u_fd1(nb_u_nodes, nb_u_nodes);
	MatrixXd K_u_u_fd2(nb_u_nodes, nb_u_nodes);

	MatrixXd ufd1(nb_u_nodes, dim);
	MatrixXd ufd2(nb_u_nodes, dim);
	for(int j = 0; j < nb_u_nodes; ++j){
		for(int d = 0; d < dim; ++d){
			ufd1(j,d) = u(j,d);
			ufd2(j,d) = u(j,d);
		}
	}
	ufd1(i, k) += 0.1;
	ufd2(i, k) -= 0.1;
	for (int h = 0; h < nb_u_nodes; ++h) {
		for (int j = 0; j < nb_u_nodes; ++j) {
			K_u_u_fd1(h, j) = evaluate_kernel(ufd1.row(h), ufd1.row(j));
			K_u_u_fd2(h, j) = evaluate_kernel(ufd2.row(h), ufd2.row(j));
		}
	}
	K_u_u_fd1 = 1.0/0.2 * (K_u_u_fd1-K_u_u_fd2);
	std::cout << "Kufdot " << deriv_matrix << std::endl;
	std::cout << "K_u_f_fd1 " << K_u_u_fd1 << std::endl;
	exit(-1);
	*/
}

void FullyIndependentTrainingConditional::derivate_K_u_f_wrt_uik(std::vector<double> const &p,
																	 int const &i,
																	 int const &k,
																	 MatrixXd &deriv_matrix ){
	const int nb_u_nodes = u.rows();
	deriv_matrix.resize(nb_u_nodes, nb_gp_nodes);
	deriv_matrix.setZero();
	double C = 0;
	for(int j = 0; j < nb_gp_nodes; ++j){
		C = p[0]; //sigma_f^2
		for(int d = 0; d < dim; ++d){
			C *= evaluate_kernel1D_exp_term(u(i, d), gp_nodes_eigen(j, d), p[1+d]);
		}
		deriv_matrix(i,j) = C * (-1) * (u(i, k)-gp_nodes_eigen(j, k))/p[k+1];
	}

	/*Good
	MatrixXd K_f_u_fd1(nb_gp_nodes, nb_u_nodes);
	MatrixXd K_f_u_fd2(nb_gp_nodes, nb_u_nodes);

	MatrixXd ufd1(nb_u_nodes, dim);
	MatrixXd ufd2(nb_u_nodes, dim);
	for(int j = 0; j < nb_u_nodes; ++j){
		for(int d = 0; d < dim; ++d){
			ufd1(j,d) = u(j,d);
			ufd2(j,d) = u(j,d);
		}
	}
	ufd1(i, k) += 0.1;
	ufd2(i, k) -= 0.1;
	for (int h = 0; h < nb_u_nodes; ++h) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			K_f_u_fd1(j, h) = evaluate_kernel(gp_nodes_eigen.row(j), ufd1.row(h));
			K_f_u_fd2(j, h) = evaluate_kernel(gp_nodes_eigen.row(j), ufd2.row(h));
		}
	}
	K_f_u_fd1 = 1.0/0.2 * (K_f_u_fd1-K_f_u_fd2);
	std::cout << "Kufdot " << deriv_matrix << std::endl;
	std::cout << "K_f_u_fd1 " << K_f_u_fd1 << std::endl;
	exit(-1);
	*/

}

void FullyIndependentTrainingConditional::derivate_K_u_f_wrt_sigmaf(std::vector<double> const &p,
                                                    MatrixXd &deriv_matrix){
	const int nb_u_nodes = u.rows();
	deriv_matrix.resize(nb_u_nodes, nb_gp_nodes);
	deriv_matrix.setZero();
	double C = 0;
	//Set up matrix d(K_u_f)/d(sigma_f^2)
	for (int i = 0; i < nb_u_nodes; ++i) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			C = 1.0;
			for(int d = 0; d < dim; ++d){
				C *= evaluate_kernel1D_exp_term(u(i, d), gp_nodes_eigen(j, d), p[1+d]);
			}
			deriv_matrix(i,j) = C;
		}
	}
	/*Good
	MatrixXd K_f_u_fd1(nb_gp_nodes, nb_u_nodes);
	MatrixXd K_f_u_fd2(nb_gp_nodes, nb_u_nodes);

	std::vector<double> gpfd1 = gp_parameters;
	gpfd1[0] += 0.1;
	std::vector<double> gpfd2 = gp_parameters;
	gpfd2[0] -= 0.1;
	for (int h = 0; h < nb_u_nodes; ++h) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			K_f_u_fd1(j, h) = evaluate_kernel(gp_nodes_eigen.row(j), u.row(h), gpfd1);
			K_f_u_fd2(j, h) = evaluate_kernel(gp_nodes_eigen.row(j), u.row(h), gpfd2);
		}
	}
	K_f_u_fd1 = 1.0/0.2 * (K_f_u_fd1-K_f_u_fd2);
	std::cout << "Kufdot " << deriv_matrix << std::endl;
	std::cout << "K_u_f_fd1 " << K_f_u_fd1 << std::endl;
	exit(-1);
	*/
}

void FullyIndependentTrainingConditional::derivate_K_f_f_wrt_sigmaf(std::vector<double> const &p,
                                                    MatrixXd &deriv_matrix){
	deriv_matrix.resize(nb_gp_nodes, nb_gp_nodes);
	deriv_matrix.setZero();
	double C = 0;
	//Set up matrix d(K_u_f)/d(sigma_f^2)
	for (int i = 0; i < nb_gp_nodes; ++i) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			C = 1.0;
			for(int d = 0; d < dim; ++d){
				C *= evaluate_kernel1D_exp_term(gp_nodes_eigen(i, d), gp_nodes_eigen(j, d), p[1+d]);
			}
			deriv_matrix(i,j) = C;
		}
	}
	/*Good
	MatrixXd K_f_f_fd1(nb_gp_nodes, nb_gp_nodes);
	MatrixXd K_f_f_fd2(nb_gp_nodes, nb_gp_nodes);

	std::vector<double> gpfd1 = gp_parameters;
	gpfd1[0] += 0.1;
	std::vector<double> gpfd2 = gp_parameters;
	gpfd2[0] -= 0.1;
	for (int h = 0; h < nb_gp_nodes; ++h) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			K_f_f_fd1(j, h) = evaluate_kernel(gp_nodes_eigen.row(j), gp_nodes_eigen.row(h), gpfd1);
			K_f_f_fd2(j, h) = evaluate_kernel(gp_nodes_eigen.row(j), gp_nodes_eigen.row(h), gpfd2);
		}
	}
	K_f_f_fd1 = 1.0/0.2 * (K_f_f_fd1-K_f_f_fd2);
	std::cout << "Kufdot " << deriv_matrix << std::endl;
	std::cout << "K_f_f_fd1 " << K_f_f_fd1 << std::endl;
	exit(-1);
	*/
}

void FullyIndependentTrainingConditional::derivate_K_u_u_wrt_sigmaf(std::vector<double> const &p,
                                                    MatrixXd &deriv_matrix){
	const int nb_u_nodes = u.rows();
	deriv_matrix.resize(nb_u_nodes, nb_u_nodes);
	deriv_matrix.setZero();
	double C = 0;
	//Set up matrix d(K_u_f)/d(sigma_f^2)
	for (int i = 0; i < nb_u_nodes; ++i) {
		for (int j = 0; j < nb_u_nodes; ++j) {
			C = 1.0;//sigma_f^2
			for(int d = 0; d < dim; ++d){
				C *= evaluate_kernel1D_exp_term(u(i, d), u(j, d), p[1+d]);
			}
			deriv_matrix(i,j) = C;
		}
	}
	/*//Good
	MatrixXd K_u_u_fd1(nb_u_nodes, nb_u_nodes);
	MatrixXd K_u_u_fd2(nb_u_nodes, nb_u_nodes);

	std::vector<double> gpfd1 = gp_parameters;
	gpfd1[0] += 0.1;
	std::vector<double> gpfd2 = gp_parameters;
	gpfd2[0] -= 0.1;
	for (int h = 0; h < nb_u_nodes; ++h) {
		for (int j = 0; j < nb_u_nodes; ++j) {
			K_u_u_fd1(j, h) = evaluate_kernel(u.row(j), u.row(h), gpfd1);
			K_u_u_fd2(j, h) = evaluate_kernel(u.row(j), u.row(h), gpfd2);
		}
	}
	K_u_u_fd1 = 1.0/0.2 * (K_u_u_fd1-K_u_u_fd2);
	std::cout << "Kuudot " << deriv_matrix << std::endl;
	std::cout << "K_u_u_fd1 " << K_u_u_fd1 << std::endl;
	exit(-1);
	*/
}

void FullyIndependentTrainingConditional::derivate_K_u_f_wrt_l(std::vector<double> const &p,
																	 int const &k,
                                        							MatrixXd &deriv_matrix){
	const int nb_u_nodes = u.rows();
	deriv_matrix.resize(nb_u_nodes, nb_gp_nodes);
	deriv_matrix.setZero();
	double C = 0;
	//Set up matrix d(K_u_f)/d(sigma_f^2)
	for (int i = 0; i < nb_u_nodes; ++i) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			C = p[0];
			for(int d = 0; d < dim; ++d){
				C *= evaluate_kernel1D_exp_term(u(i, d), gp_nodes_eigen(j, d), p[1+d]);
			}
			deriv_matrix(i,j) = C * (u(i, k)-gp_nodes_eigen(j, k))*(u(i, k)-gp_nodes_eigen(j, k))/
											(2*p[k+1]*p[k+1]);
		}
	}
	/*//Good
	MatrixXd K_f_u_fd1(nb_gp_nodes, nb_u_nodes);
	MatrixXd K_f_u_fd2(nb_gp_nodes, nb_u_nodes);

	std::vector<double> gpfd1 = gp_parameters;
	gpfd1[k+1] += 0.1;
	std::vector<double> gpfd2 = gp_parameters;
	gpfd2[k+1] -= 0.1;
	for (int h = 0; h < nb_u_nodes; ++h) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			K_f_u_fd1(j, h) = evaluate_kernel(gp_nodes_eigen.row(j), u.row(h), gpfd1);
			K_f_u_fd2(j, h) = evaluate_kernel(gp_nodes_eigen.row(j), u.row(h), gpfd2);
		}
	}
	K_f_u_fd1 = 1.0/0.2 * (K_f_u_fd1-K_f_u_fd2);
	std::cout << "Kufdot " << deriv_matrix << std::endl;
	std::cout << "K_u_f_fd1 " << K_f_u_fd1 << std::endl;
	exit(-1);
	*/
}

void FullyIndependentTrainingConditional::derivate_K_f_f_wrt_l(std::vector<double> const &p,
																	 int const &k,
                                        							MatrixXd &deriv_matrix){
	deriv_matrix.resize(nb_gp_nodes, nb_gp_nodes);
	deriv_matrix.setZero();
	double C = 0;
	//Set up matrix d(K_u_f)/d(sigma_f^2)
	for (int i = 0; i < nb_gp_nodes; ++i) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			C = p[0];
			for(int d = 0; d < dim; ++d){
				C *= evaluate_kernel1D_exp_term(gp_nodes_eigen(i, d), gp_nodes_eigen(j, d), p[1+d]);
			}
			deriv_matrix(i,j) = C * (gp_nodes_eigen(i, k)-gp_nodes_eigen(j, k))*
									(gp_nodes_eigen(i, k)-gp_nodes_eigen(j, k))/
											(2*p[k+1]*p[k+1]);
		}
	}
	/*//Good
	MatrixXd K_f_f_fd1(nb_gp_nodes, nb_gp_nodes);
	MatrixXd K_f_f_fd2(nb_gp_nodes, nb_gp_nodes);

	std::vector<double> gpfd1 = gp_parameters;
	gpfd1[k+1] += 0.1;
	std::vector<double> gpfd2 = gp_parameters;
	gpfd2[k+1] -= 0.1;
	for (int h = 0; h < nb_gp_nodes; ++h) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			K_f_f_fd1(j, h) = evaluate_kernel(gp_nodes_eigen.row(j), gp_nodes_eigen.row(h), gpfd1);
			K_f_f_fd2(j, h) = evaluate_kernel(gp_nodes_eigen.row(j), gp_nodes_eigen.row(h), gpfd2);
		}
	}
	K_f_f_fd1 = 1.0/0.2 * (K_f_f_fd1-K_f_f_fd2);
	std::cout << "Kffdot " << deriv_matrix << std::endl;
	std::cout << "K_f_f_fd1 " << K_f_f_fd1 << std::endl;
	exit(-1);
	*/
}

void FullyIndependentTrainingConditional::derivate_K_u_u_wrt_l(std::vector<double> const &p,
                                         int const &k,
                                    MatrixXd &deriv_matrix){
	const int nb_u_nodes = u.rows();
	deriv_matrix.resize(nb_u_nodes, nb_u_nodes);
	deriv_matrix.setZero();
	double C = 0;
	//Set up matrix d(K_u_f)/d(sigma_f^2)
	for (int i = 0; i < nb_u_nodes; ++i) {
		for (int j = 0; j < nb_u_nodes; ++j) {
			C = p[0];
			for(int d = 0; d < dim; ++d){
				C *= evaluate_kernel1D_exp_term(u(i, d), u(j, d), p[1+d]);
			}
			deriv_matrix(i,j) = C * (u(i, k)-u(j, k))*(u(i, k)-u(j, k))/
											(2*p[k+1]*p[k+1]);
		}
	}
	/*//Good
	MatrixXd K_u_u_fd1(nb_u_nodes, nb_u_nodes);
	MatrixXd K_u_u_fd2(nb_u_nodes, nb_u_nodes);

	std::vector<double> gpfd1 = gp_parameters;
	gpfd1[k+1] += 0.1;
	std::vector<double> gpfd2 = gp_parameters;
	gpfd2[k+1] -= 0.1;
	for (int h = 0; h < nb_u_nodes; ++h) {
		for (int j = 0; j < nb_u_nodes; ++j) {
			K_u_u_fd1(j, h) = evaluate_kernel(u.row(j), u.row(h), gpfd1);
			K_u_u_fd2(j, h) = evaluate_kernel(u.row(j), u.row(h), gpfd2);
		}
	}
	K_u_u_fd1 = 1.0/0.2 * (K_u_u_fd1-K_u_u_fd2);
	std::cout << "Kuudot " << deriv_matrix << std::endl;
	std::cout << "K_u_u_fd1 " << K_u_u_fd1 << std::endl;
	exit(-1);
	*/
}

void FullyIndependentTrainingConditional::compute_Kuf_and_Kuu() {
	int nb_u_nodes = u.rows();
	//Set up matrix K_u_f and K_f_u
	K_u_f.resize(nb_u_nodes, nb_gp_nodes);
	for (int i = 0; i < nb_u_nodes; ++i) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			K_u_f(i, j) = evaluate_kernel(u.row(i), gp_nodes_eigen.row(j));
		}
	}
	//Set up matrix K_u_u
	K_u_u.resize(nb_u_nodes, nb_u_nodes);
	for (int i = 0; i < nb_u_nodes; ++i) {
		for (int j = 0; j < nb_u_nodes; ++j) {
			K_u_u(i, j) = evaluate_kernel(u.row(i), u.row(j));
			if (i == j)
				K_u_u(i, j) += K_u_u_nugget;
		}
	}
	LLTofK_u_u.compute(K_u_u);
}

 void FullyIndependentTrainingConditional::compute_Qff(const MatrixXd& K_f_u, VectorXd& diag_Q_f_f) {
	int nb_u_nodes = u.rows();
	MatrixXd K_u_u_u_f = LLTofK_u_u.solve(K_u_f);

	diag_Q_f_f.resize(nb_gp_nodes);
	for (int i = 0; i < nb_gp_nodes; ++i) {
		diag_Q_f_f(i) = 0.0;
		for (int j = 0; j < nb_u_nodes; ++j) {
			diag_Q_f_f(i) += (K_f_u(i, j) * K_u_u_u_f(j, i));
		}
	}
}

void FullyIndependentTrainingConditional::compute_Kff(VectorXd& diag_K_f_f) {
	diag_K_f_f.resize(nb_gp_nodes);
	for (int i = 0; i < nb_gp_nodes; ++i) {
		diag_K_f_f(i) = evaluate_kernel(gp_nodes_eigen.row(i),
				gp_nodes_eigen.row(i));
	}
}

void FullyIndependentTrainingConditional::compute_diff_Kff_Qff(const VectorXd& diag_K_f_f,
		const VectorXd& diag_Q_f_f, VectorXd& diff_Kff_Qff) {
	diff_Kff_Qff.resize(nb_gp_nodes);
	for (int i = 0; i < nb_gp_nodes; ++i) {
		diff_Kff_Qff(i) = (diag_K_f_f(i) - diag_Q_f_f(i));
	}
}

void FullyIndependentTrainingConditional::compute_Lambda(const VectorXd& diff_Kff_Qff, const std::vector<double>& noise) {
	Lambda.resize(nb_gp_nodes);
	for (int i = 0; i < nb_gp_nodes; ++i) {
		Lambda(i) = (diff_Kff_Qff(i) + pow(noise.at(i) / 2e0 + noise_regularization, 2e0));
	}
}

void FullyIndependentTrainingConditional::compute_Lambda_times_Kfu(const MatrixXd& K_f_u, MatrixXd& Lambda_K_f_u) {
	int nb_u_nodes = u.rows();
	Lambda_K_f_u.resize(nb_gp_nodes, nb_u_nodes);
	for (int i = 0; i < nb_gp_nodes; i++) {
		for (int j = 0; j < nb_u_nodes; j++) {
			Lambda_K_f_u(i, j) = ((1.0 / Lambda(i)) * K_f_u(i, j));
		}
	}
}

void FullyIndependentTrainingConditional::compute_KufLambdaKfu(const MatrixXd& Lambda_K_f_u, MatrixXd& K_u_f_Lambda_f_u) {
	int nb_u_nodes = u.rows();
	K_u_f_Lambda_f_u.resize(nb_u_nodes, nb_u_nodes);
	K_u_f_Lambda_f_u = K_u_f * Lambda_K_f_u;
}

void FullyIndependentTrainingConditional::compute_LambdaInvF(VectorXd& LambdaInv_f) {
	LambdaInv_f.resize(nb_gp_nodes);
	for (int i = 0; i < nb_gp_nodes; ++i) {
		LambdaInv_f(i) = 1.0 / Lambda(i) * scaled_function_values[i];
	}
}

void FullyIndependentTrainingConditional::compute_LambdaDot(const MatrixXd& Kffdot,
                                    const MatrixXd& Kufdot,
                                    const MatrixXd& Kfudot,
                                    const MatrixXd& Kuudot,
                                    VectorXd& LambdaDot){
	LambdaDot.resize(nb_gp_nodes);
	LambdaDot.setZero();
	MatrixXd KfudotKuinvKuf = Kfudot * LLTofK_u_u.solve(K_u_f);
	MatrixXd K_f_u = K_u_f.transpose();
	//MatrixXd KfuKuinvKufdot = K_f_u * LLTofK_u_u.solve(Kufdot);
	MatrixXd KfuKuinfKudotKuinvKuf = K_f_u*(LLTofK_u_u.solve(Kuudot*(LLTofK_u_u.solve(K_u_f))));

	for(int i = 0; i < nb_gp_nodes; ++i){
		LambdaDot(i) = Kffdot(i,i) - 2*KfudotKuinvKuf(i,i) + KfuKuinfKudotKuinvKuf(i,i);// - KfuKuinvKufdot(i,i);
	}

}

void FullyIndependentTrainingConditional::compute_GammaDotDoubleBar(const MatrixXd& Kffdot,
																	const MatrixXd& Kufdot,
																	const MatrixXd& Kfudot,
																	const MatrixXd& Kuudot,
																	VectorXd& GammaRes){
	GammaRes.resize(nb_gp_nodes);
	GammaRes.setZero();
	MatrixXd KfudotKuinvKuf = Kfudot * LLTofK_u_u.solve(K_u_f);
	MatrixXd K_f_u = K_u_f.transpose();
	MatrixXd KfuKuinvKufdot = K_f_u * LLTofK_u_u.solve(Kufdot);
	MatrixXd KfuKuinfKudotKuinvKuf = K_f_u*(LLTofK_u_u.solve(Kuudot*(LLTofK_u_u.solve(K_u_f))));

	double diag_ii;
	for(int i = 0; i < nb_gp_nodes; ++i){
		diag_ii = Kffdot(i,i) - KfudotKuinvKuf(i,i) + KfuKuinfKudotKuinvKuf(i,i) - KfuKuinvKufdot(i,i);
		GammaRes(i) = 1/Lambda(i) * diag_ii;
	}

}

bool FullyIndependentTrainingConditional::test_for_parameter_estimation(const int& nb_values,
                                                const int& update_interval_length,
                                                const int& next_update,
                                                const std::vector<int>& update_at_evaluations){

  bool do_parameter_estimation = GaussianProcess::test_for_parameter_estimation(nb_values, update_interval_length, next_update, update_at_evaluations);
  if (do_parameter_estimation){
  	std::cout << "Regular update step." << std::endl;
  	return do_parameter_estimation;
  }

  //check if induced points are still in bounds;
  IOFormat HeavyFmt(FullPrecision, 0, ", ", ";\n", "", "", "[", "]");
  double dist = 0.0;
  const double constraint_ball_radius_squared = constraint_ball_radius*constraint_ball_radius;
  for(int i = 0; i < u.rows(); ++i){
  	dist = 0.0;
  	for(int j = 0; j < u.cols(); ++j){
  		dist += (u(i,j)-constraint_ball_center(j))*(u(i,j)-constraint_ball_center(j));
  	}
  	dist = sqrt(dist);
	if(dist > (constraint_ball_radius*1.000001)){
  		std::cout << "Induced point out of bounds! Index: " << i << " Radius: " << constraint_ball_radius << " Dist: " << dist << std::endl;
  		for(int j = 0; j < u.cols(); ++j){
  			std::cout << "u: " << u(i, j) << " |c: " << constraint_ball_center(j) << std::endl;
  		}
  		do_parameter_estimation = true;
  		return do_parameter_estimation;
  	}
  }
  do_parameter_estimation = false;

  return do_parameter_estimation;
}

//--------------------------------------------------------------------------------
void FullyIndependentTrainingConditional::build(
		std::vector<std::vector<double> > const &nodes,
		std::vector<double> const &values, std::vector<double> const &noise) {

	int nb_u_nodes = u.rows();

	if (nb_u_nodes > 0) {
		std::cout << "FITC build with [" << nodes.size() << "," << nb_u_nodes
			<< "]" << std::endl;
		std::cout << "With Parameters: " << std::endl;
	    for ( int i = 0; i < dim+1; ++i )
	      std::cout << "gp_param = " << gp_parameters[i] << std::endl;
	    std::cout << std::endl;
		nb_gp_nodes = nodes.size();
		gp_nodes.clear();
		gp_noise.clear();
		gp_nodes_eigen.resize(nb_gp_nodes, dim);
		gp_noise_eigen.resize(nb_gp_nodes);
		for (int i = 0; i < nb_gp_nodes; ++i) {
			gp_nodes.push_back(nodes.at(i));
			gp_noise.push_back(noise.at(i));
			for(int j = 0; j < dim; ++j){
				gp_nodes_eigen(i,j) = nodes[i][j];
			}
			gp_noise_eigen(i) = noise[i];
		}

		//Set up matrix K_u_f and K_f_u
		compute_Kuf_and_Kuu();
		MatrixXd K_f_u = K_u_f.transpose();

		VectorXd diag_Q_f_f;
		compute_Qff(K_f_u, diag_Q_f_f);

		VectorXd diag_K_f_f;
		compute_Kff(diag_K_f_f);

		VectorXd diff_Kff_Qff; //this is different for FITC, DTC, SoR
		compute_diff_Kff_Qff(diag_K_f_f, diag_Q_f_f, diff_Kff_Qff);

		compute_Lambda(diff_Kff_Qff, noise);

		MatrixXd Lambda_K_f_u;
		compute_Lambda_times_Kfu(K_f_u, Lambda_K_f_u);

		MatrixXd K_u_f_Lambda_f_u;
		compute_KufLambdaKfu(Lambda_K_f_u, K_u_f_Lambda_f_u);

		L_eigen.compute(K_u_u + K_u_f_Lambda_f_u);

		scaled_function_values.clear();
		scaled_function_values.resize(nb_gp_nodes);
		for (int i = 0; i < nb_gp_nodes; i++) {
			scaled_function_values.at(i) = values.at(i);
		}

		VectorXd LambdaInv_f;
		compute_LambdaInvF(LambdaInv_f);

		VectorXd alpha_eigen_rhs = K_u_f * LambdaInv_f;

		//Solve Sigma_not_inv^(-1)*alpha
		alpha_eigen = L_eigen.solve(alpha_eigen_rhs);

	} else {
		GaussianProcess::build(nodes, values, noise);
	}
	return;
}

//--------------------------------------------------------------------------------
void FullyIndependentTrainingConditional::update(std::vector<double> const &x,
		double &value, double &noise) {

	int nb_u_nodes = u.rows();
	if (nb_u_nodes > 0) {
		std::cout << "FITC update [" << gp_nodes.size()+1 << "," << nb_u_nodes <<"]" << std::endl;
		//std::cout << "#Update" << std::endl;
		std::vector<std::vector<double> > temp_nodes;
		temp_nodes.resize(gp_nodes.size());
		std::vector<double> temp_values;
		std::vector<double> temp_noise;
		for (int i = 0; i < gp_nodes.size(); ++i) {
			for (int j = 0; j < gp_nodes.at(i).size(); ++j) {
				temp_nodes.at(i).push_back(gp_nodes.at(i).at(j));
			}
			temp_values.push_back(scaled_function_values.at(i));
			temp_noise.push_back(gp_noise.at(i));
		}
		temp_nodes.push_back(x);
		temp_noise.push_back(noise);
		temp_values.push_back(value);
		//  scaled_function_values.push_back ( ( value -  min_function_value ) /
		//                                     ( 5e-1*( max_function_value-min_function_value ) ) - 1e0 );

		this->build(temp_nodes, temp_values, temp_noise);
	} else {
		GaussianProcess::update(x, value, noise);
	}
	return;
}
//--------------------------------------------------------------------------------

void FullyIndependentTrainingConditional::evaluate(std::vector<double> const &x,
		double &mean, double &variance) {
	int nb_u_nodes = u.rows();
	if (nb_u_nodes > 0) {
		//std::cout << "FITC evalute [" << gp_nodes_eigen.rows() << "," << nb_u_nodes <<"]" << std::endl;
		VectorXd x_eigen;
		x_eigen.resize(x.size());
		for(int i = 0; i < x.size(); ++i){
			x_eigen(i) = x[i];
		}
		K0_eigen.resize(nb_u_nodes);
		for (int i = 0; i < nb_u_nodes; i++) {
			K0_eigen(i) = evaluate_kernel(x_eigen, u.row(i));
		}

//		std::cout << "alpha= (K_u_u + K_u_f Lambda_inv K_f_u)\(K_u_f*LambdaInv*f):" << std::endl;
//		VectorOperations::print_vector(alpha);
		mean = K0_eigen.dot(alpha_eigen);
//		std::cout << "mean:" << mean << std::endl;
		//std::cout << "Mean: " << mean << std::endl;

		double variance_term3 = K0_eigen.dot(L_eigen.solve(K0_eigen));

		double variance_term2 = K0_eigen.dot(LLTofK_u_u.solve(K0_eigen));
		/*
		 std::cout << "Variance: " << variance << std::endl;
		 std::cout << "######################################" << std::endl;
		 evaluate_counter++;
		 assert(evaluate_counter<20*3);
//		 */
		//exit(-1);

//		std::cout << "K**:" << evaluate_kernel(x,x) << std::endl;
		variance = evaluate_kernel(x_eigen,x_eigen)-variance_term2+variance_term3;
		 //std::cout << "FITC evalute [" << gp_nodes_eigen.rows() << "," << nb_u_nodes <<"] mean,variance " << mean << ", " << variance << std::endl;
//		std::cout << "variance:" << variance << std::endl;
	} else {
		GaussianProcess::evaluate(x, mean, variance);
	}
	return;

}

void FullyIndependentTrainingConditional::get_induced_nodes(
		std::vector<std::vector<double> > &induced_nodes) const {
	induced_nodes.resize(u.rows());
	for (int i = 0; i < u.rows(); ++i) {
		for (int j = 0; j < dim; ++j){
			induced_nodes[i].push_back(u(i, j));
		}
	}

	return;
}

void FullyIndependentTrainingConditional::sample_u(const int& nb_u_nodes) {

	u.resize(nb_u_nodes, dim);

	u.setConstant(-1);
	std::vector<int> u_idx_left_over;
	std::vector<int> u_idx_from_active;
	//Set distribution for sampling the indices for the u samples
	std::random_device rd;
	int random_seed = rd();//1;////rd();
	std::mt19937 random_generator(random_seed);
	std::vector<double> nodes_weights_vector;

	u.resize(nb_u_nodes, dim);
	std::normal_distribution<double> dis_radius(0.0, 1.0);
	std::uniform_real_distribution<double> dis_unit_radius(0.0, 1.0);
	std::uniform_real_distribution<double> dis_trust_radius(-constraint_ball_radius, constraint_ball_radius);
	VectorXd cur_u(dim);
	double cur_U_i, radius, rand_radius, n_minus_1_angle;
	bool not_found_point = true;
	for(int i = 0; i < nb_u_nodes; ++i){
		not_found_point = true;
		radius = -1.0;
		while(not_found_point){
			not_found_point = true;
			radius = 0.0;
			for(int j = 0; j < dim; ++j){
				cur_u(j) = dis_trust_radius(random_generator);
				radius += cur_u(j) *cur_u(j);
			}
			radius = sqrt(radius);
			if(radius < constraint_ball_radius){
				for(int j = 0; j < dim; ++j){
					u(i, j) = cur_u(j) + constraint_ball_center(j);
				}
				not_found_point = false;
			}
		}
	}
	return;
}

void FullyIndependentTrainingConditional::clear_u(){
	u.resize(0,0);
}

void FullyIndependentTrainingConditional::copy_data_to_members( std::vector< std::vector<double> > const &nodes,
                                                  std::vector<double> const &values,
                                                  std::vector<double> const &noise){
  nb_gp_nodes = nodes.size();
  gp_nodes.clear();
  gp_noise.clear();
	gp_nodes_eigen.resize(nb_gp_nodes, dim);
	gp_noise_eigen.resize(nb_gp_nodes);
  for ( int i = 0; i < nb_gp_nodes; ++i ) {
    gp_nodes.push_back ( nodes.at(i) );
    gp_noise.push_back ( noise.at(i) );
	for(int j = 0; j < dim; ++j){
		gp_nodes_eigen(i,j) = nodes[i][j];
	}
	gp_noise_eigen(i) = noise[i];
}

//  auto minmax = std::minmax_element(values.begin(), values.end());
//  min_function_value = values.at((minmax.first - values.begin()));
//  max_function_value = values.at((minmax.second - values.begin()));

  L.clear();
  L.resize( nb_gp_nodes );
  for ( int i = 0; i < nb_gp_nodes; ++i)
    L.at(i).resize( i+1 );

  scaled_function_values.resize(nb_gp_nodes);
  scaled_function_values_eigen.resize(nb_gp_nodes);
  for ( int i = 0; i < nb_gp_nodes; ++i) {
    scaled_function_values.at(i) = values.at(i);
    scaled_function_values_eigen(i) = values.at(i);
//    scaled_function_values.at(i) = values.at(i) - min_function_value;
//    scaled_function_values.at(i) /= 5e-1*( max_function_value-min_function_value );
//    scaled_function_values.at(i) -= 1e0;
  }

}

void FullyIndependentTrainingConditional::set_optimizer(std::vector<double> const &values, nlopt::opt*& local_opt, nlopt::opt*& global_opt){

  optimize_global = true;
  optimize_local = false;

  int dimp1 = gp_parameters_hp.size();

  lb.resize(dimp1);
  ub.resize(dimp1);

  int offset;
  if(dimp1==1 + dim){
  		auto minmax = std::minmax_element(values.begin(), values.end());
	    min_function_value = values.at((minmax.first - values.begin()));
	    max_function_value = fabs(values.at((minmax.second - values.begin())));
	    if ( fabs(min_function_value) > max_function_value )
	    max_function_value = fabs( min_function_value );

		  lb[0] = 1e-3;
		  ub[0] = 1e3;
		  lb[0] = max_function_value - 1e2;
		  if ( lb[0] < 1e-3 ) lb[0] = 1e-3;
		  ub[0] = max_function_value + 1e2;
		  if ( ub[0] > 1e3 ) ub[0] = 1e3;
		  if ( ub[0] <= lb[0]) lb[0] = 1e-3;
		//if (ub[0] < 1.0) ub[0] = 1.0;
		double delta_threshold = *delta;
		if (delta_threshold < 1e-2) delta_threshold = 1e-2;
		for (int i = 0; i < dim; ++i) {
		    lb[i+1] = 1e-2 * delta_threshold; // 1e1
		    ub[i+1] = 2.0 * delta_threshold; // 1e2
		}
	  	for (int i = 0; i < dim+1; ++i) {
  		  if ( gp_parameters_hp[i] <= lb[i] ) {
  			std::cout << "LS Too small: " << gp_parameters_hp[i] << " for " << lb[i] << std::endl;
      		gp_parameters_hp[i] = 1.0001 * lb[i];
      		}
	      if ( gp_parameters_hp[i] >= ub[i] ) {
	  			std::cout << "LS Too big: " << gp_parameters_hp[i] << " for " << ub[i] << std::endl;
	      		gp_parameters_hp[i] = 0.9999 * ub[i];
	      }
	      if ( gp_parameters_hp[i] <= lb[i] ||  gp_parameters_hp[i] >= ub[i]){
			std::cout << "LS still in between: " << gp_parameters_hp[i];
	      	gp_parameters_hp[i] = lb[i] + (ub[i]-lb[i])/0.5;
	      	std::cout << "LS fixed: "<< gp_parameters_hp[i] << std::endl;
        	}
        }

  }else{
	  if(gp_parameters_hp.size() > u.rows()*dim){ //optimizing also over lengthscale and sigma_f
	  	auto minmax = std::minmax_element(values.begin(), values.end());
	    min_function_value = values.at((minmax.first - values.begin()));
	    max_function_value = fabs(values.at((minmax.second - values.begin())));
	    if ( fabs(min_function_value) > max_function_value )
	    max_function_value = fabs( min_function_value );
	  	lb[0] = 1e-3;
		  ub[0] = 1e3;
		  lb[0] = max_function_value - 1e2;
		  if ( lb[0] < 1e-3 ) lb[0] = 1e-3;
		  ub[0] = max_function_value + 1e2;
		  if ( ub[0] > 1e3 ) ub[0] = 1e3;
		  if ( ub[0] <= lb[0]) lb[0] = 1e-3;
		//if (ub[0] < 1.0) ub[0] = 1.0;
		double delta_threshold = *delta;
		if (delta_threshold < 1e-2) delta_threshold = 1e-2;
		for (int i = 0; i < dim; ++i) {
		    lb[i+1] = 1e-2 * delta_threshold; // 1e1
		    ub[i+1] = 2.0 * delta_threshold; // 1e2
		}
	    offset = 1+dim;
	  }else{
	  	offset = 0;
	  }
	  //Set box constraints such that the constraint ball is inside
	  std::vector<double> lb_u(dim);
	  std::vector<double> ub_u(dim);
	  for (int i = 0; i < dim; ++i) {
	      lb_u[i] = constraint_ball_center[i] - 1.5*constraint_ball_radius;
	      ub_u[i] = constraint_ball_center[i] + 1.5*constraint_ball_radius;
	  }
	  for (int i = 0; i < dim; ++i) {
		  for(int j = offset + i*u.rows(); j < offset + (i+1)*u.rows(); ++j){
	          lb[j] = lb_u[i];
	          ub[j] = ub_u[i];
		  }
	  }

	  if(gp_parameters_hp.size() > u.rows()*dim){//optimizing also over lengthscale and sigma_f
		  if (gp_parameters_hp[0] < 0e0) {
		  	std::cout << "In here: " << gp_parameters_hp[0] << std::endl;
		    gp_parameters_hp[0] = max_function_value;
		    for (int i = 1; i < dim+1; ++i) {
		      gp_parameters_hp[i] = (lb[i]*5e-1 + 5e-1*ub[i]);
		    }
		  } else {
		    for (int i = 0; i < dim+1; ++i) {
		      if ( gp_parameters_hp[i] <= lb[i] ) {
		  			std::cout << "2Too small: " << gp_parameters_hp[i] << std::endl;
		      		gp_parameters_hp[i] = 1.0001 * lb[i];
		      	}
		      if ( gp_parameters_hp[i] >= ub[i] ) {
		  			std::cout << "2Too big: " << gp_parameters_hp[i] << std::endl;
		      		gp_parameters_hp[i] = 0.9999 * ub[i];
		      }
		      if ( gp_parameters_hp[i] <= lb[i] ||  gp_parameters_hp[i] >= ub[i]){
				std::cout << "2still in between: " << gp_parameters_hp[i];
		      	gp_parameters_hp[i] = lb[i] + (ub[i]-lb[i])/0.5;
		      	std::cout << "2fixed: "<< gp_parameters_hp[i] << std::endl;
		      }
		    }
		  }
		}

	for (int i = offset; i < dimp1; ++i) {
      if ( gp_parameters_hp[i] <= lb[i] ) {
  			std::cout << "U Too small: " << gp_parameters_hp[i] << std::endl;
      		gp_parameters_hp[i] = 1.0001 * lb[i];
      	}
      if ( gp_parameters_hp[i] >= ub[i] ) {
  			std::cout << "U Too big: " << gp_parameters_hp[i] << std::endl;
      		gp_parameters_hp[i] = 0.9999 * ub[i];
      }
      if ( gp_parameters_hp[i] <= lb[i] ||  gp_parameters_hp[i] >= ub[i]){
		std::cout << "U still in between: " << gp_parameters_hp[i];
      	gp_parameters_hp[i] = lb[i] + (ub[i]-lb[i])/0.5;
      	std::cout << "U fixed: "<< gp_parameters_hp[i] << std::endl;
      }
    	}
	}

  local_opt = new nlopt::opt(nlopt::LD_MMA, dimp1);
  global_opt = new nlopt::opt(nlopt::GN_ISRES, dimp1);

  global_opt->set_lower_bounds( lb );
  global_opt->set_upper_bounds( ub );
  global_opt->set_maxtime(1.0);
  //global_opt->set_maxeval(10000);

  local_opt->set_lower_bounds( lb );
  local_opt->set_upper_bounds( ub );
  local_opt->set_maxtime(60.0);
  local_opt->set_maxeval(1000);
}

void FullyIndependentTrainingConditional::run_optimizer(std::vector<double> const &values){
	double optval;

  int exitflag;

  nlopt::opt* local_opt;
  nlopt::opt* global_opt;

  set_optimizer(values, local_opt, global_opt);

  int dimp1 = gp_parameters_hp.size();

  std::vector<double> tol(dimp1);
  for(int i = 0; i < dimp1; ++i){
  	tol[i] = 0.0;
  }
  if (optimize_global){
  	  print = 0;
 	  std::cout << "Global optimization" << std::endl;
	  exitflag=-20;
	  global_opt->add_inequality_mconstraint(trust_region_constraint, gp_pointer, tol);
	  global_opt->set_min_objective( parameter_estimation_objective, gp_pointer);
	  exitflag = global_opt->optimize(gp_parameters_hp, optval);

	  std::cout << "exitflag = "<< exitflag<<std::endl;
  	  std::cout << "Function calls: " << print << std::endl;
	  std::cout << "OPTVAL .... " << optval << std::endl;
	  //for ( int i = 0; i < 1+dim; ++i )
	    //std::cout << "gp_param = " << gp_parameters_hp[i] << std::endl;
	  //std::cout << std::endl;
  }
  if (optimize_local){
  	  print = 0;
  	  std::cout << "Local optimization" << std::endl;
	  exitflag=-20;
	  //try {
	  local_opt->add_inequality_mconstraint(trust_region_constraint, gp_pointer, tol);
	  local_opt->set_min_objective( parameter_estimation_objective_w_gradients, gp_pointer);
	  exitflag = local_opt->optimize(gp_parameters_hp, optval);

	  std::cout << "exitflag = "<< exitflag<<std::endl;
  	  std::cout << "Function calls: " << print << std::endl;
	  std::cout << "OPTVAL .... " << optval << std::endl;
	  //for ( int i = 0; i < 1+dim; ++i )
	    //std::cout << "gp_param = " << gp_parameters_hp[i] << std::endl;
	  //std::cout << std::endl;
  }

  delete local_opt;
  delete global_opt;

  return;
}

void FullyIndependentTrainingConditional::update_induced_points(){
  int u_counter;
  int offset = 1+dim;
  for (int i = 0; i < dim; ++i) {
  	  u_counter = 0;
  	  for(int j = offset + i*u.rows(); j < offset + (i+1)*u.rows(); ++j){
            u(u_counter,i) = gp_parameters[j];
            u_counter++;
  	  }
  }
}

void FullyIndependentTrainingConditional::estimate_hyper_parameters ( std::vector< std::vector<double> > const &nodes,
                                                  std::vector<double> const &values,
                                                  std::vector<double> const &noise ){
	this->estimate_hyper_parameters_ls_only(nodes, values, noise);
}

void FullyIndependentTrainingConditional::estimate_hyper_parameters_all ( std::vector< std::vector<double> > const &nodes,
                                                  std::vector<double> const &values,
                                                  std::vector<double> const &noise )
{
  if (u.rows() > 0) {
	  std::cout << "FITC Estimator" << std::endl;
	  copy_data_to_members(nodes, values, noise);

	  gp_pointer = this;

	  sample_u(u.rows());

	  set_hyperparameters();

	  run_optimizer(values);

	  copy_hyperparameters();

	  for ( int i = 0; i < 1+dim; ++i )
	    std::cout << "gp_param = " << gp_parameters[i] << std::endl;
  }else{
		GaussianProcess::estimate_hyper_parameters(nodes, values, noise);
  }

  return;
}

void FullyIndependentTrainingConditional::estimate_hyper_parameters_induced_only ( std::vector< std::vector<double> > const &nodes,
                                                  std::vector<double> const &values,
                                                  std::vector<double> const &noise )
{
  if (u.rows() > 0) {
	  std::cout << "FITC Estimator" << std::endl;
	  copy_data_to_members(nodes, values, noise);

	  gp_pointer = this;

	  sample_u(u.rows());

	  set_hyperparameters_induced_only();

	  run_optimizer(values);

	  copy_hyperparameters();

	  for ( int i = 0; i < 1+dim; ++i )
	    std::cout << "gp_param = " << gp_parameters[i] << std::endl;

  }else{
		GaussianProcess::estimate_hyper_parameters(nodes, values, noise);
  }

  return;
}
void FullyIndependentTrainingConditional::estimate_hyper_parameters_ls_only ( std::vector< std::vector<double> > const &nodes,
                                                      std::vector<double> const &values,
                                                      std::vector<double> const &noise ){
  if (u.rows() > 0) {
	  std::cout << "FITC Estimator" << std::endl;
	  copy_data_to_members(nodes, values, noise);

	  gp_pointer = this;

	  sample_u(u.rows());

	  set_hyperparameters_ls_only();

	  run_optimizer(values);

	  copy_hyperparameters();

	  for ( int i = 0; i < 1+dim; ++i )
	    std::cout << "gp_param = " << gp_parameters[i] << std::endl;

  }else{
		GaussianProcess::estimate_hyper_parameters(nodes, values, noise);
  }

  return;
}
//--------------------------------------------------------------------------------

void FullyIndependentTrainingConditional::set_hyperparameters(){
  int dimp1 = 1+dim+u.rows()*dim;
  gp_parameters_hp.resize(dimp1);
  int u_counter;
  int offset = dim + 1;
  for (int i = 0; i < dim + 1; ++i){
  	gp_parameters_hp[i] = gp_parameters[i];
  }
  for (int i = 0; i < dim; ++i) {
  	  u_counter = 0;
  	  for(int j = offset + i*u.rows(); j < offset + (i+1)*u.rows(); ++j){
            gp_parameters_hp[j] = u(u_counter,i);
            u_counter++;
  	  }
  }
  return;
}

void FullyIndependentTrainingConditional::set_hyperparameters_ls_only(){
  int dimp1 = dim+1;
  gp_parameters_hp.resize(dimp1);
  for (int i = 0; i < dimp1; ++i){
  	gp_parameters_hp[i] = gp_parameters[i];
  }
  return;
}

void FullyIndependentTrainingConditional::set_hyperparameters_induced_only(){
  int dimp1 = u.rows()*dim;
  gp_parameters_hp.resize(dimp1);
  int u_counter;
  for (int i = 0; i < dim; ++i) {
  	  u_counter = 0;
  	  for(int j = i*u.rows(); j < (i+1)*u.rows(); ++j){
            gp_parameters_hp[j] = u(u_counter,i);
            u_counter++;
  	  }
  }
  return;
}

void FullyIndependentTrainingConditional::copy_hyperparameters(){
  int dimp1 = gp_parameters_hp.size();
  int u_counter;
  int offset;
  if(gp_parameters_hp.size()==dim+1){
	for (int i = 0; i < dim + 1; ++i){
  	  gp_parameters[i] = gp_parameters_hp[i];
    }
  }else{
	  if(gp_parameters_hp.size() > u.rows()*dim){//optimizing also over lengthscale and sigma_f, copy them back
	    for (int i = 0; i < dim + 1; ++i){
	  	  gp_parameters[i] = gp_parameters_hp[i];
	    }
	    offset = 1+dim;
	  }else{
	  	offset = 0;
	  }
	  for (int i = 0; i < dim; ++i) {
	  	  u_counter = 0;
	  	  for(int j = offset + i*u.rows(); j < offset + (i+1)*u.rows(); ++j){
	        u(u_counter,i) = gp_parameters_hp[j];
	        u_counter++;
	  	  }
	  }
  }
  return;
}

//--------------------------------------------------------------------------------
double FullyIndependentTrainingConditional::parameter_estimation_objective(std::vector<double> const &x,
                                                       std::vector<double> &grad,
                                                       void *data)
{
  FullyIndependentTrainingConditional *d = reinterpret_cast<FullyIndependentTrainingConditional*>(data);
  int offset;
  std::vector<double> local_params(d->dim+1);
  if(x.size()==1+d->dim){
  	  offset = 0;
  	  for(int i = 0; i < local_params.size(); ++i){
	  		local_params[i] = x[i];
  	  }
  }else{
	  if(x.size() > d->u.rows()*d->dim){
	  	offset = 1+d->dim;
	  	for(int i = 0; i < local_params.size(); ++i){
	  		local_params[i] = x[i];
	  	}
	  }else{
	  	offset = 0;
	  	for(int i = 0; i < local_params.size(); ++i){
	  		local_params[i] = d->gp_parameters[i];
	  	}
	  }
	  int u_counter;
	  for (int i = 0; i < d->dim; ++i) {
	  	  u_counter = 0;
	  	  for(int j = offset + i*d->u.rows(); j < offset + (i+1)*d->u.rows(); ++j){
	            d->u(u_counter,i) = x[j];
	            u_counter++;
	  	  }
	  }
  }

  //Compute Kuf, Kuu
  //Set up matrix K_u_f and K_f_u
	int nb_gp_nodes = d->gp_nodes.size();
	int nb_u_nodes = d->u.rows();
	d->K_u_f.resize(nb_u_nodes, nb_gp_nodes);
	//std::cout << d->u.rows() << " " << nb_gp_nodes << " " << d->gp_nodes_eigen.rows() << std::endl;
	for (int i = 0; i < nb_u_nodes; ++i) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			d->K_u_f(i,j) = d->evaluate_kernel(d->u.row(i), d->gp_nodes_eigen.row(j), local_params);
		}
	}
	MatrixXd K_f_u = d->K_u_f.transpose();
	//std::cout << "Kuf\n" << d->K_u_f << std::endl;

	//Set up matrix K_u_u
	d->K_u_u.resize(nb_u_nodes, nb_u_nodes);
	for (int i = 0; i < nb_u_nodes; ++i) {
		for (int j = 0; j < nb_u_nodes; ++j) {
			d->K_u_u(i,j) = d->evaluate_kernel(d->u.row(i), d->u.row(j), local_params);
			if(i==j)
				d->K_u_u(i,j) += d->Kuu_opt_nugget;
		}
	}
	//std::cout << "Kuu\n" << d->K_u_u << std::endl;
	d->LLTofK_u_u.compute(d->K_u_u);
	MatrixXd K_u_u_u_f = d->LLTofK_u_u.solve(d->K_u_f);

	VectorXd diag_Q_f_f;
	diag_Q_f_f.resize(nb_gp_nodes);

	for (int i = 0; i < nb_gp_nodes; ++i) {
		diag_Q_f_f(i) = 0.0;
		for (int j = 0; j < nb_u_nodes; ++j) {
			diag_Q_f_f(i) += (K_f_u(i,j) * K_u_u_u_f(j,i));
		}
	}
	VectorXd diag_K_f_f;
	diag_K_f_f.resize(nb_gp_nodes);
	for (int i = 0; i < nb_gp_nodes; ++i) {
		diag_K_f_f(i) = d->evaluate_kernel(d->gp_nodes_eigen.row(i),
									 d->gp_nodes_eigen.row(i), local_params);
	}

	d->Lambda.resize(nb_gp_nodes);
	//std::cout << "noise\n" << std::endl;
	for (int i = 0; i < nb_gp_nodes; ++i) {
		d->Lambda(i) = (diag_K_f_f(i) - diag_Q_f_f(i) + pow( d->gp_noise.at(i) / 2e0 + d->noise_regularization, 2e0 ));
	}
	//std::cout << std::endl;

	MatrixXd Lambda_K_f_u;
	Lambda_K_f_u.resize(nb_gp_nodes, nb_u_nodes);
	for(int i = 0; i < nb_gp_nodes; i++){
		for(int j = 0; j < nb_u_nodes; j++){
			Lambda_K_f_u(i,j) = ((1.0/(d->Lambda(i) + d->Lambda_opt_nugget)) * K_f_u(i,j));
		}
	}
	MatrixXd K_u_f_Lambda_f_u;
	K_u_f_Lambda_f_u.resize(nb_u_nodes, nb_u_nodes);
	K_u_f_Lambda_f_u = d->K_u_f*Lambda_K_f_u;

	MatrixXd Sigma = d->K_u_u + K_u_f_Lambda_f_u;
	for (int i = 0; i < nb_u_nodes; ++i) {
		//Sigma(i,i) += nugget;
	}
	d->L_eigen.compute(Sigma);
	double L12 = 0.0;//-log(d->K_u_u.determinant());
	for (int i = 0; i < d->u.rows(); ++i){
		L12 += log(d->LLTofK_u_u.vectorD()(i));
    	//std::cout << L1 << "L11-" << i << std::endl;
	}

	double det_Leigen = 0.0;
	for (int i = 0; i < d->u.rows(); ++i){
		det_Leigen += log(d->L_eigen.vectorD()(i));
    	//std::cout << d->L_eigen.matrixL()(i,i) << " " << log(d->L_eigen.matrixL()(i,i)) << "det_Leigen-" << i << std::endl;
	}

	double L11 = 0.0;
    //std::cout << L1 << "L12 " << std::endl;
	for (int i = 0; i < d->nb_gp_nodes; ++i){
		L11 += log(d->Lambda(i));
		//std::cout << "Lambda: " << i << " " << d->Lambda(i) <<" "<<log(d->Lambda(i)) << std::endl;
	}
    double L1 = 0.0;
	//L1 = 0.5*L11 + (2*0.5)*det_Leigen + 0.5*det_LeigenD - (2*0.5)*L12 - 0.5*L12D;
	L1 = 0.5*L11 + 0.5*det_Leigen - 0.5*L12;

	MatrixXd Q_f_f = K_f_u*K_u_u_u_f;
	for(int i = 0; i < nb_gp_nodes; i++){
		Q_f_f(i,i) += d->Lambda(i)+ d->Qff_opt_nugget;
	}
	LLT<MatrixXd> LLTofQ_f_f(Q_f_f);
	double L2 = 0.5*d->scaled_function_values_eigen.dot(LLTofQ_f_f.solve(d->scaled_function_values_eigen));

  	//std::cout << d->scaled_function_values_eigen << std::endl;
  double result = L1 + L2;

  if (std::isinf(result) || std::isnan(result)){
  	std::cout << "Result is inf or nan" << std::endl;
  	std::cout << "L11 " << L11 << " " << 'x' << std::endl;
    std::cout << "L12 " << L12 << std::endl; //<< " " << log(d->K_u_u.determinant()) << std::endl;
    std::cout << "L13 " << det_Leigen << std::endl;// << " " << log(Sigma.determinant()) << std::endl;
    std::cout << L1 << ' ' << L2 << std::endl;
  	result = std::numeric_limits<double>::infinity();
  	if(d->Kuu_opt_nugget < d->nugget_max){
  		d->Kuu_opt_nugget *= 10;
  		d->Lambda_opt_nugget *= 10;
  		d->Qff_opt_nugget *= 10;
  	}
  }else{
  	if(d->Kuu_opt_nugget > d->nugget_min){
  		d->Kuu_opt_nugget *= 0.1;
  		d->Lambda_opt_nugget *= 0.1;
  		d->Qff_opt_nugget *= 0.1;
  	}
  }
  if ((d->print%1000)==0){
	  //for ( int i = 0; i < d->dim + 1; ++i )
	  //  std::cout << "gp_param = " << x[i] << std::endl;
	  //for(int j = offset; j < offset + d->u.rows(); ++j)
	//		std::cout << "gp_param = " << x[j] <<","<<x[j+ d->u.rows()]<< std::endl;

  	//std::cout << d->print <<" Objective: " << L1 << " " << L2 << " "<< result<< std::endl;
   }

  d->print++;
  return result;

}

double FullyIndependentTrainingConditional::parameter_estimation_objective_w_gradients(std::vector<double> const &x,
                                                       std::vector<double> &grad,
                                                       void *data)
{

  FullyIndependentTrainingConditional *d = reinterpret_cast<FullyIndependentTrainingConditional*>(data);
  int offset;
  std::vector<double> local_params(d->dim+1);
  if(x.size()==1+d->dim){
  	  offset = 0;
  	  for(int i = 0; i < local_params.size(); ++i){
	  		local_params[i] = x[i];
  	  }
  }else{
	  if(x.size() > d->u.rows()*d->dim){
	  	offset = 1+d->dim;
	  	for(int i = 0; i < local_params.size(); ++i){
	  		local_params[i] = x[i];
	  	}
	  }else{
	  	offset = 0;
	  	for(int i = 0; i < local_params.size(); ++i){
	  		local_params[i] = d->gp_parameters[i];
	  	}
	  }
	  int u_counter;
	  for (int i = 0; i < d->dim; ++i) {
	  	  u_counter = 0;
	  	  for(int j = offset + i*d->u.rows(); j < offset + (i+1)*d->u.rows(); ++j){
	            d->u(u_counter,i) = x[j];
	            u_counter++;
	  	  }
	  }
  }

  //Compute Kuf, Kuu
  //Set up matrix K_u_f and K_f_u
	int nb_gp_nodes = d->gp_nodes.size();
	int nb_u_nodes = d->u.rows();
	d->K_u_f.resize(nb_u_nodes, nb_gp_nodes);
	for (int i = 0; i < nb_u_nodes; ++i) {
		for (int j = 0; j < nb_gp_nodes; ++j) {
			d->K_u_f(i,j) = d->evaluate_kernel(d->u.row(i), d->gp_nodes_eigen.row(j), local_params);
		}
	}
	MatrixXd K_f_u = d->K_u_f.transpose();
	//std::cout << "Kuf\n" << d->K_u_f << std::endl;

	//Set up matrix K_u_u
	d->K_u_u.resize(nb_u_nodes, nb_u_nodes);
	for (int i = 0; i < nb_u_nodes; ++i) {
		for (int j = 0; j < nb_u_nodes; ++j) {
			d->K_u_u(i,j) = d->evaluate_kernel(d->u.row(i), d->u.row(j), local_params);
			if(i==j)
				d->K_u_u(i,j) += d->Kuu_opt_nugget;
		}
	}
	//std::cout << "Kuu\n" << d->K_u_u << std::endl;
	d->LLTofK_u_u.compute(d->K_u_u);
	MatrixXd K_u_u_u_f = d->LLTofK_u_u.solve(d->K_u_f);

	VectorXd diag_Q_f_f;
	diag_Q_f_f.resize(nb_gp_nodes);

	for (int i = 0; i < nb_gp_nodes; ++i) {
		diag_Q_f_f(i) = 0.0;
		for (int j = 0; j < nb_u_nodes; ++j) {
			diag_Q_f_f(i) += (K_f_u(i,j) * K_u_u_u_f(j,i));
		}
	}
	VectorXd diag_K_f_f;
	diag_K_f_f.resize(nb_gp_nodes);
	for (int i = 0; i < nb_gp_nodes; ++i) {
		diag_K_f_f(i) = d->evaluate_kernel(d->gp_nodes_eigen.row(i),
									 d->gp_nodes_eigen.row(i), local_params);
	}
	/*std::cout << "diag_K_f_f\n" << diag_K_f_f << std::endl;
	std::cout << "diag_Q_f_f\n" << diag_Q_f_f << std::endl;*/

	d->Lambda.resize(nb_gp_nodes);
	//std::cout << "noise\n" << std::endl;
	for (int i = 0; i < nb_gp_nodes; ++i) {
		d->Lambda(i) = (diag_K_f_f(i) - diag_Q_f_f(i) + pow( d->gp_noise.at(i) / 2e0 + d->noise_regularization, 2e0 ));
	}
	//std::cout << std::endl;

	MatrixXd Lambda_K_f_u;
	Lambda_K_f_u.resize(nb_gp_nodes, nb_u_nodes);
	for(int i = 0; i < nb_gp_nodes; i++){
		for(int j = 0; j < nb_u_nodes; j++){
			Lambda_K_f_u(i,j) = ((1.0/(d->Lambda(i) + d->Lambda_opt_nugget)) * K_f_u(i,j));
		}
	}
	MatrixXd K_u_f_Lambda_f_u;
	K_u_f_Lambda_f_u.resize(nb_u_nodes, nb_u_nodes);
	K_u_f_Lambda_f_u = d->K_u_f*Lambda_K_f_u;

	MatrixXd Sigma = d->K_u_u + K_u_f_Lambda_f_u;
	for (int i = 0; i < nb_u_nodes; ++i) {
		//Sigma(i,i) += nugget;
	}
	d->L_eigen.compute(Sigma);
	double L12 = 0.0;//-log(d->K_u_u.determinant());
	for (int i = 0; i < d->u.rows(); ++i){
		L12 += log(d->LLTofK_u_u.vectorD()(i));
    	//std::cout << L1 << "L11-" << i << std::endl;
	}

	double det_Leigen = 0.0;
	for (int i = 0; i < d->u.rows(); ++i){
		det_Leigen += log(d->L_eigen.vectorD()(i));
    	//std::cout << d->L_eigen.matrixL()(i,i) << " " << log(d->L_eigen.matrixL()(i,i)) << "det_Leigen-" << i << std::endl;
	}

	double L11 = 0.0;
    //std::cout << L1 << "L12 " << std::endl;
	for (int i = 0; i < d->nb_gp_nodes; ++i){
		L11 += log(d->Lambda(i));
		//std::cout << "Lambda: " << i << " " << d->Lambda(i) <<" "<<log(d->Lambda(i)) << std::endl;
	}
    double L1 = 0.0;
	//L1 = 0.5*L11 + (2*0.5)*det_Leigen + 0.5*det_LeigenD - (2*0.5)*L12 - 0.5*L12D;
	L1 = 0.5*L11 + 0.5*det_Leigen - 0.5*L12;
	MatrixXd Q_f_f = K_f_u*K_u_u_u_f;
	for(int i = 0; i < nb_gp_nodes; i++){
		Q_f_f(i,i) += d->Lambda(i)+ d->Qff_opt_nugget;
	}
	LLT<MatrixXd> LLTofQ_f_f(Q_f_f);
	double L2 = 0.5*d->scaled_function_values_eigen.dot(LLTofQ_f_f.solve(d->scaled_function_values_eigen));

  	//std::cout << d->scaled_function_values_eigen << std::endl;
  double result = L1 + L2;

  if (std::isinf(result) || std::isnan(result)){
  	std::cout << "Result is inf or nan" << std::endl;
  	std::cout << "L11 " << L11 << " " << 'x' << std::endl;
    std::cout << "L12 " << L12 << std::endl; //<< " " << log(d->K_u_u.determinant()) << std::endl;
    std::cout << "L13 " << det_Leigen << std::endl;// << " " << log(Sigma.determinant()) << std::endl;
    std::cout << L1 << ' ' << L2 << std::endl;
  	result = std::numeric_limits<double>::infinity();
  	if(d->Kuu_opt_nugget < d->nugget_max){
  		d->Kuu_opt_nugget *= 10;
  		d->Lambda_opt_nugget *= 10;
  		d->Qff_opt_nugget *= 10;
  	}
  }else{
  	if(d->Kuu_opt_nugget > d->nugget_min){
  		d->Kuu_opt_nugget *= 0.1;
  		d->Lambda_opt_nugget *= 0.1;
  		d->Qff_opt_nugget *= 0.1;
  	}
  }
  if ((d->print%1)==0){
	  /*for ( int i = 0; i < d->dim + 1; ++i )
	    std::cout << "gp_param = " << x[i] << std::endl;
	  for(int j = offset; j < offset + d->u.rows(); ++j)
			std::cout << "gp_param = " << x[j] <<","<<x[j+ d->u.rows()]<< std::endl;*/

  	//std::cout << d->print <<" Objective: " << L1 << " " << L2 << " "<< result<< std::endl;
  }

	//Gradient computation:
	int dim_grad = x.size();
	grad.resize(dim_grad);

	for(int i = 0; i < grad.size(); i++){

		MatrixXd Kffdot;
		MatrixXd Kufdot;
		MatrixXd Kfudot;
		MatrixXd Kuudot;
		VectorXd LambdaDot;
		if(dim_grad == d->dim + 1){
			if(i == 0){//grad sigma_f
				d->derivate_K_u_u_wrt_sigmaf(local_params, Kuudot);
				d->derivate_K_f_f_wrt_sigmaf(local_params, Kffdot);
				d->derivate_K_u_f_wrt_sigmaf(local_params, Kufdot);
				Kfudot = Kufdot.transpose();
			}else{//grad length parameter
				d->derivate_K_u_u_wrt_l(local_params, i-1, Kuudot);
				d->derivate_K_f_f_wrt_l(local_params, i-1, Kffdot);
				d->derivate_K_u_f_wrt_l(local_params, i-1, Kufdot);
				Kfudot = Kufdot.transpose();
			}
		}else{
			if( dim_grad > d->u.rows()*d->dim){//if we optimize also for sigma_f and lengthscales
				if(i == 0){//grad sigma_f
					d->derivate_K_u_u_wrt_sigmaf(local_params, Kuudot);
					d->derivate_K_f_f_wrt_sigmaf(local_params, Kffdot);
					d->derivate_K_u_f_wrt_sigmaf(local_params, Kufdot);
					Kfudot = Kufdot.transpose();
				}else if(i > 0 && i < 1 + d->dim){//grad length parameter
					d->derivate_K_u_u_wrt_l(local_params, i-1, Kuudot);
					d->derivate_K_f_f_wrt_l(local_params, i-1, Kffdot);
					d->derivate_K_u_f_wrt_l(local_params, i-1, Kufdot);
					Kfudot = Kufdot.transpose();
				}else{//grad u
					int uidx = (i-offset)%d->u.rows();
					int udim = (int) ((i-offset)/d->u.rows());
					d->derivate_K_u_u_wrt_uik(local_params, uidx, udim, Kuudot);
					d->derivate_K_u_f_wrt_uik(local_params, uidx, udim, Kufdot);
					Kffdot.resize(nb_gp_nodes, nb_gp_nodes);
					Kffdot.setZero();
					Kfudot = Kufdot.transpose();
				}
		    }else{
		    	int uidx = (i-offset)%d->u.rows();
				int udim = (int) ((i-offset)/d->u.rows());
				d->derivate_K_u_u_wrt_uik(local_params, uidx, udim, Kuudot);
				d->derivate_K_u_f_wrt_uik(local_params, uidx, udim, Kufdot);
				Kffdot.resize(nb_gp_nodes, nb_gp_nodes);
				Kffdot.setZero();
				Kfudot = Kufdot.transpose();
		    }
	    }
		d->compute_LambdaDot(Kffdot, Kufdot, Kfudot, Kuudot, LambdaDot);

		//L1-term: dL3/dtheta = d(Lambda+noise^2)/dtheta = tr((Lambda+noise^2)^(-1)*LambdaDot)
		double dL1 = 0.0;
		VectorXd LambdaInv(nb_gp_nodes);
		for(int j = 0; j < nb_gp_nodes; ++j){
			LambdaInv(j) = 1.0/d->Lambda(j);
			dL1 += LambdaInv(j) * LambdaDot(j);
		}

		//L2-term: dL2/dtheta= tr(Kuu^(-1) * Kuudot)
		double dL2 = d->LLTofK_u_u.solve(Kuudot).trace();

		//L3-term: d(log(det(Kuu+Kuf*(Lambda+noise^2)^(-1)*Kfu)))/dtheta
		MatrixXd LambdaInvDiag = LambdaInv.asDiagonal();
		MatrixXd LambdaDotDiag = LambdaDot.asDiagonal();
		MatrixXd dSigma = Kuudot
							+ 2*Kufdot*LambdaInvDiag*K_f_u
							- d->K_u_f*LambdaInvDiag*LambdaDotDiag*LambdaInvDiag*K_f_u;
							//+ d->K_u_f*LambdaInvDiag*Kfudot;
		double dL3 = (d->L_eigen.solve(dSigma)).trace();

		//L4-term: d(yT*Q*y)/dtheta
		MatrixXd KfudotKuinvKuf = Kfudot * d->LLTofK_u_u.solve(d->K_u_f);
		MatrixXd KfuKuinvKufdot = K_f_u * d->LLTofK_u_u.solve(Kufdot);
		MatrixXd KfuKuinfKudotKuinvKuf = K_f_u*(d->LLTofK_u_u.solve(Kuudot*(d->LLTofK_u_u.solve(d->K_u_f))));
		MatrixXd dQ = KfudotKuinvKuf - KfuKuinfKudotKuinvKuf + KfuKuinvKufdot + LambdaDotDiag;
		double dL4 = (-1)*d->scaled_function_values_eigen.dot(
						LLTofQ_f_f.solve(dQ*LLTofQ_f_f.solve(
							d->scaled_function_values_eigen)));

		grad[i] = 0.5*(dL1 - dL2 + dL3 + dL4);

		/*JacobiSVD<MatrixXd> svdKuuInv(d->K_u_u.inverse());
		double condKuu = svdKuuInv.singularValues()(0)
		    / svdKuuInv.singularValues()(svdKuuInv.singularValues().size()-1);
		JacobiSVD<MatrixXd> svdQ(Q_f_f.inverse());
		double condQff = svdQ.singularValues()(0)
		    / svdQ.singularValues()(svdQ.singularValues().size()-1);
		JacobiSVD<MatrixXd> svdLambdaInv(LambdaInvDiag);
		double condLambda = svdLambdaInv.singularValues()(0)
		    / svdLambdaInv.singularValues()(svdLambdaInv.singularValues().size()-1);
		JacobiSVD<MatrixXd> svddQ(dQ);
		double conddQ = svddQ.singularValues()(0)
		    / svddQ.singularValues()(svddQ.singularValues().size()-1);
		std::cout << "Condition numbers: \n";
		std::cout << "Kuuinv: " << condKuu << " Qff: " << condQff
			<< " LambdaInv: " << condLambda << " dQ: " << conddQ << std::endl;
		*/
		//if ((d->print%1)==0){
		// 	std::cout << "Grad: " << i << "|" << 0.5*dL1 << " + "<< 0.5*dL2 << " + "<< 0.5*dL3 << " + "<< 0.5*dL4 << " = " << grad[i] << std::endl;
		//}
		//exit(-1);
	}

  d->print++;

  return result;

}

void FullyIndependentTrainingConditional::set_constraint_ball_radius(const double& radius){
	constraint_ball_radius = radius;
}

void FullyIndependentTrainingConditional::set_constraint_ball_center(const std::vector<double>& center){
	constraint_ball_center.resize(center.size());
	for(int i = 0; i < center.size(); ++i){
		constraint_ball_center(i) = center[i];
	}
}

void FullyIndependentTrainingConditional::trust_region_constraint(unsigned int m, double* c, unsigned int n, const double* x, double* grad,
                                                     			void *data){
	FullyIndependentTrainingConditional *d = reinterpret_cast<FullyIndependentTrainingConditional*>(data);

	int offset;
	if(m == d->dim + 1){
		for (int i = 0; i < d->dim+1; ++i) {
			c[i] = -1;
		}
	}else{
		if(m > d->u.rows()*d->dim){
		    offset = 1+d->dim;
		}else{
			offset = 0;
		}
		int u_counter;

		for (int i = 0; i < offset; ++i) {
			c[i] = -1;
		}
		MatrixXd u_intern(d->u.rows(), d->u.cols());
		for (int i = 0; i < d->dim; ++i) {
		  u_counter = 0;
		  for(int j = offset + i*d->u.rows(); j < offset + (i+1)*d->u.rows(); ++j){
	        u_intern(u_counter, i) = x[j];
	        u_counter++;
		  }
		}
		VectorXd c_intern(u_intern.rows());
		VectorXd dist(d->dim);
		for (int i = 0; i < u_intern.rows(); ++i) {
			for (int j = 0; j < d->dim; ++j) {
				dist(j) = (u_intern(i, j)-d->constraint_ball_center(j));
			}
	    	c_intern(i) = sqrt( dist.dot(dist) ) - d->constraint_ball_radius;
		}
		for (int i = 0; i < d->dim; ++i) {
		    u_counter = 0;
		  	for(int j = offset + i*d->u.rows(); j < offset + (i+1)*d->u.rows(); ++j){
		    c[j] = c_intern(u_counter);
	        u_counter++;
		  	}
		}
	}
  	return;
}

void FullyIndependentTrainingConditional::decrease_nugget(){
	if(K_u_u_nugget > K_u_u_nugget_min){
		K_u_u_nugget *= 0.1;
	}
	std::cout << "FITC: Decrease nugget to " << K_u_u_nugget << std::endl;
  return;
}
bool FullyIndependentTrainingConditional::increase_nugget(){
	if(K_u_u_nugget <= K_u_u_nugget_max){
		K_u_u_nugget *= 10;
	}
	std::cout << "FITC: Increase nugget to " << K_u_u_nugget << std::endl;
	if(K_u_u_nugget > K_u_u_nugget_max){
		return true;
	}
  return false;
}


#pragma once

#include "Tableau.hpp"
#include "Solids.hpp"
#include "SM_utils.hpp"

#include <Eigen/Dense>

#include <vector>
#include <stdexcept>
#include <cstddef>

namespace AQSystemSolver {
    class ReplacementDict{
    private:
        //which terms have been replaced out of existence
        SM_utils::flat_set<Eigen::Index> columns;
        SM_utils::flat_set<Eigen::Index> columnsNotReplaced;
        SM_utils::IncreasingPQ<Eigen::Index> nextRowToFill;
        std::vector<Eigen::Index> columnToRow;//because columns is not in order, and we always append to our replacement, this tells which column goes to which row
        Tableau<> unsimplifiedReplacement;
        Tableau<> replacement;
        Tableau<> replacementWithoutColumns;

        void groupTerms(){
            for(auto column : columns){
                replacement.groupTerm<1>(column, columnToRow[column]);
            }
        }

        [[nodiscard]] auto substituteTerms(){
            bool replacedAnything=false;
            const Tableau<> copyReplacement=replacement;
            for(auto column : columns){
                if(std::any_of(SM_utils::NestingIterator(columnToRow, columns.begin()), SM_utils::NestingIterator(columnToRow, columns.end()), [&](Eigen::Index row) { return replacement.getCoefficient(row, column)!=0; })) {
                    //std::cout<<"replaced"<<std::endl<<std::endl;
                    replacedAnything=true;
                    replacement.substituteRowAndCol<true>(copyReplacement, columnToRow[column], copyReplacement, column);
                }
            }
            return replacedAnything;
        }

        void simplify(){
            do{
                groupTerms();
            } while(substituteTerms());
            //the last substitute terms doesn't change anything so we dont need to regroup terms
            replacementWithoutColumns=replacement.reducedCopy(Eigen::all, columnsNotReplaced);
        }

        void simplifyFromUnsimplified(){
            replacement=unsimplifiedReplacement;
            simplify();
        }

        [[nodiscard]] auto addColumn(const Eigen::Index column){
            columnToRow[column]=nextRowToFill.top();
            nextRowToFill.pop();
            columns.insert(column);
            columnsNotReplaced.erase(column);
            return columnToRow[column];
        }

        void removeColumn(const Eigen::Index column){
            nextRowToFill.push(columnToRow[column]);
            columns.erase(column);
            columnsNotReplaced.insert(column);
        }

        void solveForTermAndAddToRow(const Eigen::RowVectorXd& rowVect, double constant, Eigen::Index term, Eigen::Index row){
            unsimplifiedReplacement.assignRow(row, rowVect, constant);
            unsimplifiedReplacement.groupTerm<0>(row, term);
            replacement.assignRowFromTableau(row, unsimplifiedReplacement, row);
        }

        void addSolidInternal(Solid* solid){
            auto columnIt=std::find_if(columnsNotReplaced.begin(), columnsNotReplaced.end(), [&](Eigen::Index term){ return solid->row.coeff(term)!=0.0; });
            if(columnIt==columnsNotReplaced.end()){
                throw std::runtime_error("Couldn't find a clean column. This is probably a Gibbs Rule violation.");
            }
            const Eigen::Index column=*columnIt;
            Eigen::Index row=addColumn(column);
            solid->column=column;
            solveForTermAndAddToRow(solid->row, solid->constant, column, row);
        }

    public:
        [[nodiscard]] auto size() const {
            assert(static_cast<Eigen::Index>(columns.size())==replacement.rows());
            return columns.size();
        }
        [[nodiscard]] auto cols() const {
            return replacement.cols();
        }

        void addSolid(Solid* solid){
            addSolidInternal(solid);
            //mostly already simplifed, just the last row, but we still have to iterate over all the columns
            simplify();
        }

        void removeSolid(Solid* solid){
            removeColumn(solid->column);

            solid->column=-1;

            //TODO(SoAsEr) Plug row back in and then simplify
            simplifyFromUnsimplified();
        }

        void addSolidSystem(const SolidSystem& solidSystem){
            assert(cols()==solidSystem.cols());
            replacement.conservativeResize(replacement.rows()+solidSystem.size(), cols());
            unsimplifiedReplacement.conservativeResize(replacement.rows()+solidSystem.size(), cols());
            for(Solid * solid : solidSystem.getSolidsPresent()) {
                addSolidInternal(solid); //avoid simplifying on every loop
            }
            simplify();
        }

        [[nodiscard]] auto createReplacedTableau(const /*TableauType*/ auto& orig) const {
            auto replaced=orig.reducedCopy(Eigen::all, columnsNotReplaced);
            if(orig.rows()) [[likely]] {
                for(auto column : columns){
                    replaced.template substituteRowAndCol<false>(replacementWithoutColumns, columnToRow[column], orig, column);
                }
            }
            return replaced;
        }

        ReplacementDict(const /*std::ranges::range*/ auto& columns_, const Tableau<>& replacement_):
        columnsNotReplaced{SM_utils::CountingIterator(0), SM_utils::CountingIterator(replacement_.cols())},
        nextRowToFill{0},
        columnToRow(replacement_.cols()),
        unsimplifiedReplacement{replacement_},
        replacement{replacement_}
        {
            Eigen::Index i{0};
            for(const Eigen::Index column: columns_) {
                const Eigen::Index row=addColumn(column);
                assert(replacement_.getCoefficient(i, column)!=0);
                solveForTermAndAddToRow(replacement_.getCoefficients().row(i), replacement_.getConstant(i), column, row);
                ++i;
            }
            simplifyFromUnsimplified();
        }
    };
} //namespace AQSystemSolver


/* boost random/cauchy_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP
#define BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <iostream>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/detail/config.hpp>

namespace boost {

#if defined(__GNUC__) && (__GNUC__ < 3)
// Special gcc workaround: gcc 2.95.x ignores using-declarations
// in template classes (confirmed by gcc author Martin v. Loewis)
  using std::tan;
#endif

// Cauchy distribution:

/**
 * The cauchy distribution is a continuous distribution with two
 * parameters, sigma and median.
 *
 * It has \f$p(x) = \frac{\sigma}{\pi(\sigma^2 + (x-m)^2)}\f$
 */
template<class RealType = double>
class cauchy_distribution
{
public:
  typedef RealType input_type;
  typedef RealType result_type;

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
  BOOST_STATIC_ASSERT(!std::numeric_limits<RealType>::is_integer);
#endif

  /**
   * Constructs a \cauchy_distribution with the paramters @c median
   * and @c sigma.
   */
  explicit cauchy_distribution(result_type median_arg = result_type(0),
                               result_type sigma_arg = result_type(1))
    : _median(median_arg), _sigma(sigma_arg) { }

  // compiler-generated copy ctor and assignment operator are fine

  /**
   * Returns: the "median" parameter of the distribution
   */
  result_type median() const { return _median; }
  /**
   * Returns: the "sigma" parameter of the distribution
   */
  result_type sigma() const { return _sigma; }
  /**
   * Effects: Subsequent uses of the distribution do not depend
   * on values produced by any engine prior to invoking reset.
   */
  void reset() { }

  /**
   * Returns: A random variate distributed according to the
   * cauchy distribution.
   */
  template<class Engine>
  result_type operator()(Engine& eng)
  {
    // Can we have a boost::mathconst please?
    const result_type pi = result_type(3.14159265358979323846);
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::tan;
#endif
    return _median + _sigma * tan(pi*(eng()-result_type(0.5)));
  }

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  /**
   * Writes the parameters of the distribution to a @c std::ostream.
   */
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const cauchy_distribution& cd)
  {
    os << cd._median << " " << cd._sigma;
    return os;
  }

  /**
   * Reads the parameters of the distribution from a @c std::istream.
   */
  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, cauchy_distribution& cd)
  {
    is >> std::ws >> cd._median >> std::ws >> cd._sigma;
    return is;
  }
#endif

private:
  result_type _median, _sigma;
};

} // namespace boost

#endif // BOOST_RANDOM_CAUCHY_DISTRIBUTION_HPP


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include "litCheckMacros.h"

#include "rttbBaseType.h"
#include "rttbDoseAccessorInterface.h"
#include "rttbDicomDoseAccessor.h"
#include "rttbDicomFileDoseAccessorGenerator.h"
#include "rttbArithmetic.h"
#include "rttbNullPointerException.h"
#include "rttbInvalidParameterException.h"
#include "rttbBinaryFunctorAccessor.h"

namespace rttb
{
	namespace testing
	{
		typedef core::DoseAccessorInterface::Pointer DoseAccessorPointer;
		typedef  algorithms::BinaryFunctorAccessor<algorithms::arithmetic::doseOp::Add>
		BinaryFunctorAccessorAddType;
		typedef  algorithms::BinaryFunctorAccessor<algorithms::arithmetic::doseOp::AddWeighted>
		BinaryFunctorAccessorAddWeightedType;

		/*! @brief BinaryFunctorAccessorTest - tests functors of two accessors
				1) test constructor
				2) test getDoseAt
			*/

		int BinaryFunctorAccessorTest(int argc, char* argv[])
		{
			PREPARE_DEFAULT_TEST_REPORTING;

			std::string RTDOSE_FILENAME;
			std::string RTDOSE2_FILENAME;

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

			if (argc > 2)
			{
				RTDOSE2_FILENAME = argv[2];
			}

			DoseAccessorPointer spDoseAccessorNull;

			DoseAccessorPointer spDoseAccessor = io::dicom::DicomFileDoseAccessorGenerator(
			        RTDOSE_FILENAME.c_str()).generateDoseAccessor();
			DoseAccessorPointer spDoseAccessor2 = io::dicom::DicomFileDoseAccessorGenerator(
			        RTDOSE2_FILENAME.c_str()).generateDoseAccessor();

			algorithms::arithmetic::doseOp::Add addOP;

			algorithms::arithmetic::doseOp::AddWeighted addWeightedOP(1.0, 10.0);
			algorithms::arithmetic::doseOp::AddWeighted addWeightedTwoOP(2.0, 2.0);

			//1) Check constructor

			CHECK_THROW_EXPLICIT(BinaryFunctorAccessorAddType(spDoseAccessorNull, spDoseAccessor, addOP),
			                     core::NullPointerException);
			CHECK_THROW_EXPLICIT(BinaryFunctorAccessorAddType(spDoseAccessor, spDoseAccessorNull, addOP),
			                     core::NullPointerException);
			CHECK_THROW_EXPLICIT(BinaryFunctorAccessorAddType(spDoseAccessorNull, spDoseAccessorNull,
			                     addOP), core::NullPointerException);
			CHECK_THROW_EXPLICIT(BinaryFunctorAccessorAddType(spDoseAccessor, spDoseAccessor2, addOP),
			                     core::InvalidParameterException);

			CHECK_NO_THROW(BinaryFunctorAccessorAddType(spDoseAccessor, spDoseAccessor, addOP));
			CHECK_NO_THROW(BinaryFunctorAccessorAddWeightedType(spDoseAccessor, spDoseAccessor,
			               addWeightedOP));

			auto spBinaryFunctorDoseAccessorAdd = boost::make_shared<BinaryFunctorAccessorAddType>(spDoseAccessor, spDoseAccessor, addOP);
			auto spBinaryFunctorDoseAccessorAddWeighted = boost::make_shared<BinaryFunctorAccessorAddWeightedType>(spDoseAccessor, spDoseAccessor, addWeightedOP);
			auto spBinaryFunctorDoseAccessorAddWeightedTwo = boost::make_shared<BinaryFunctorAccessorAddWeightedType>(spDoseAccessor, spDoseAccessor, addWeightedTwoOP);

			//2) Test getDoseAt()
			int lastIndex = spBinaryFunctorDoseAccessorAdd->getGeometricInfo().getNumberOfVoxels() - 1;
			VoxelGridID aId[3] = { 5, 6067, lastIndex };
			VoxelGridIndex3D aIndex[3] = {VoxelGridIndex3D(5, 0, 0), VoxelGridIndex3D(37, 0, 2), VoxelGridIndex3D(spBinaryFunctorDoseAccessorAdd->getGeometricInfo().getNumColumns() - 1, spBinaryFunctorDoseAccessorAdd->getGeometricInfo().getNumRows() - 1, spBinaryFunctorDoseAccessorAdd->getGeometricInfo().getNumSlices() - 1)};

			for (int i = 0; i < 3; ++i)
			{
				CHECK_EQUAL(spBinaryFunctorDoseAccessorAdd->getValueAt(aId[i]), 4.0);
				CHECK_EQUAL(spBinaryFunctorDoseAccessorAddWeighted->getValueAt(aId[i]), 22.0);
				CHECK_EQUAL(spBinaryFunctorDoseAccessorAdd->getValueAt(aIndex[i]),
				            spBinaryFunctorDoseAccessorAdd->getValueAt(aId[i]));
				CHECK_EQUAL(spBinaryFunctorDoseAccessorAddWeighted->getValueAt(aIndex[i]),
				            spBinaryFunctorDoseAccessorAddWeighted->getValueAt(aId[i]));
				CHECK_EQUAL(spBinaryFunctorDoseAccessorAdd->getValueAt(aId[i]) * 2.0,
				            spBinaryFunctorDoseAccessorAddWeightedTwo->getValueAt(aId[i]));
			}

			VoxelGridID aIdInvalid(spBinaryFunctorDoseAccessorAdd->getGeometricInfo().getNumberOfVoxels());
			VoxelGridIndex3D aIndexInvalid(spBinaryFunctorDoseAccessorAdd->getGeometricInfo().getNumColumns(),
			                               spBinaryFunctorDoseAccessorAdd->getGeometricInfo().getNumRows(),
			                               spBinaryFunctorDoseAccessorAdd->getGeometricInfo().getNumSlices());
			CHECK_EQUAL(spBinaryFunctorDoseAccessorAdd->getValueAt(aIdInvalid), -1.0);
			CHECK_EQUAL(spBinaryFunctorDoseAccessorAdd->getValueAt(aIndexInvalid), -1.0);
			CHECK_EQUAL(spBinaryFunctorDoseAccessorAddWeighted->getValueAt(aIdInvalid), -1.0);
			CHECK_EQUAL(spBinaryFunctorDoseAccessorAddWeighted->getValueAt(aIndexInvalid), -1.0);

			RETURN_AND_REPORT_TEST_SUCCESS;
		}
	}
}


#include <NTL/mat_poly_lzz_p.h>

NTL_START_IMPL

void CharPoly(zz_pX& f, const mat_zz_p& M)
{
   long n = M.NumRows();
   if (M.NumCols() != n)
      LogicError("CharPoly: nonsquare matrix");

   if (n == 0) {
      set(f);
      return;
   }

   zz_p t;

   if (n == 1) {
      SetX(f);
      negate(t, M(1, 1));
      SetCoeff(f, 0, t);
      return;
   }

   mat_zz_p H;

   H = M;

   long i, j, m;
   zz_p u, t1;

   for (m = 2; m <= n-1; m++) {
      i = m;
      while (i <= n && IsZero(H(i, m-1)))
         i++;

      if (i <= n) {
         t = H(i, m-1);
         if (i > m) {
            swap(H(i), H(m));
            // swap columns i and m
            for (j = 1; j <= n; j++)
               swap(H(j, i), H(j, m));
         }

         for (i = m+1; i <= n; i++) {
            div(u, H(i, m-1), t);
            for (j = m; j <= n; j++) {
               mul(t1, u, H(m, j));
               sub(H(i, j), H(i, j), t1);
            }

            for (j = 1; j <= n; j++) {
               mul(t1, u, H(j, i));
               add(H(j, m), H(j, m), t1);
            }
         }
      }
   }

   vec_zz_pX F;
   F.SetLength(n+1);
   zz_pX T;
   T.SetMaxLength(n);

   set(F[0]);
   for (m = 1; m <= n; m++) {
      LeftShift(F[m], F[m-1], 1);
      mul(T, F[m-1], H(m, m));
      sub(F[m], F[m], T);

      set(t);
      for (i = 1; i <= m-1; i++) {
         mul(t, t, H(m-i+1, m-i));
         mul(t1, t, H(m-i, m));
         mul(T, F[m-i-1], t1);
         sub(F[m], F[m], T);
      }
   }

   f = F[n];
}

NTL_END_IMPL


#include <iostream>
#include <dlib/matrix.h>
using namespace dlib;
using namespace std;
int main() {
  matrix<double,3,1> y;
  matrix<double> M(3,3);

  M = 54.2, 7.4, 12.1, 1, 2, 3, 5.9, 0.05, 1;
  y = 3.5, 1.2, 7.8;

  matrix<double> x = inv(M)*y;
  cout << "x: \n" << x << endl;
  cout << "M*x - y: \n" << M*x -y << endl;

  matrix<double,0,1> runtime_sized_column_vector;
  matrix<double,1,0> runtime_sized_row_vector;
  runtime_sized_column_vector.set_size(3);
  x.set_size(3,4);
  cout << M(0,1) << endl;
  cout << y(1) << endl;

  return 0;
}


/**
 * \copyright
 * Copyright (c) 2015, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "fem_ele_std.h"

#include <Eigen/Eigen>

#include "rf_mmp_new.h"
#include "rf_pcs.h"

namespace FiniteElement
{

void CFiniteElementStd::AssembleTHResidual()
{
	const unsigned c_dim = dim;
	const int c_nnodes = nnodes;
	const double dt = pcs->Tim->time_step_length;
	const double theta = pcs->m_num->ls_theta;
	const bool hasGravity =
		(coordinate_system) % 10 == 2 && FluidProp->CheckGravityCalculation();
	const double g_const = hasGravity ? gravity_constant : .0;
	const bool useSUPG = (pcs->m_num->ele_supg_method > 0);
	const bool useLumpedMass = (pcs->m_num->ele_mass_lumping > 0);
	const bool isTransient =
		(this->pcs->tim_type == FiniteElement::TIM_TRANSIENT);
	const bool isMatrixFlowInactive = pcs->deactivateMatrixFlow;
	const bool isMatrixElement = (MeshElement->GetDimension() == pcs->m_msh->GetMaxElementDim());

	Matrix t_transform_tensor(3, 3);
	if (dim > MediaProp->geo_dimension)
	{
		if (MeshElement->getTransformTensor() == NULL)
		{
			std::cout << "***Error: Geometric dimension in MMP is not "
						 "consistent with element."
					  << "\n";
			exit(0);
		}
		t_transform_tensor.LimitSize(dim, dim);
		MeshElement->getTransformTensor()->GetTranspose(t_transform_tensor);
	}

	Eigen::VectorXd nodal_p0(nnodes), nodal_p1(nnodes);
	Eigen::VectorXd nodal_T0(nnodes), nodal_T1(nnodes);
	for (int i = 0; i < nnodes; i++)
	{
		nodal_p0[i] = pcs->GetNodeValue(nodes[i], idxp0);
		nodal_p1[i] = pcs->GetNodeValue(nodes[i], idxp1);
		nodal_T0[i] = pcs->GetNodeValue(nodes[i], idxT0);
		nodal_T1[i] = pcs->GetNodeValue(nodes[i], idxT1);
	}

	// Calculate matrices
	const int offset_p = 0;
	const int offset_T = c_nnodes;
	(*RHS) = 0.0;          // Residual

//#define TH_DEBUG
#ifdef TH_DEBUG
	// for debugging
	Mass->LimitSize(nnodes, nnodes);
	Laplace->LimitSize(nnodes, nnodes);
	Advection->LimitSize(nnodes, nnodes);
	(*Mass) = .0;
	(*Laplace) = .0;
	(*Advection) = .0;
#endif
	double* tmp_k_tensor = MediaProp->PermeabilityTensor(Index);
	if (c_dim > MediaProp->geo_dimension)
	{
		Matrix local_tensor(c_dim, c_dim), temp_tensor(c_dim, c_dim),
			global_tensor(c_dim, c_dim);
		const unsigned c_ele_dim = ele_dim;
		for (size_t i = 0; i < c_ele_dim; i++)
			for (size_t j = 0; j < c_ele_dim; j++)
				local_tensor(i, j) = tmp_k_tensor[j + i * c_ele_dim];
		// cout << "K':" << endl; local_tensor.Write();
		local_tensor.multi(t_transform_tensor, temp_tensor);
		for (size_t i = 0; i < c_dim; i++)
			for (size_t j = 0; j < c_dim; j++)
				for (size_t k = 0; k < c_dim; k++)
					global_tensor(i, j) +=
						(*MeshElement->getTransformTensor())(i, k) *
						temp_tensor(k, j);
		// cout << "K:" << endl; global_tensor.Write();
		for (size_t i = 0; i < c_dim; i++)
			for (size_t j = 0; j < c_dim; j++)
				tmp_k_tensor[c_dim * i + j] = global_tensor(i, j);
	}
	double const* const k_tensor = tmp_k_tensor;
	const double Ss =
		MediaProp->StorageFunction(Index, unit, theta);
	double dummy[3] = {};
	double const* const lambda_tensor =
		MediaProp->HeatDispersionTensorNew(0, dummy);
	Eigen::MatrixXd k(c_dim, c_dim);
	for (unsigned i=0; i<c_dim; i++)
		for (unsigned j=0; j<c_dim; j++)
			k(i,j) = k_tensor[i*c_dim + j];
	Eigen::MatrixXd lambda(c_dim, c_dim);
	for (unsigned i=0; i<c_dim; i++)
		for (unsigned j=0; j<c_dim; j++)
			lambda(i,j) = lambda_tensor[i*c_dim + j];
	Eigen::Vector3d vec_g;
	vec_g << 0, 0, -g_const;

	//======================================================================
	Eigen::VectorXd r_p(c_nnodes), r_T(c_nnodes);
	r_p.setZero();
	r_T.setZero();

	//======================================================================
	// Mass lumping
	if (isTransient && useLumpedMass)
	{
		const double vol =
			MeshElement->GetVolume() * MeshElement->GetFluxArea();
		// Center of the reference element
		SetCenterGP();
		ComputeShapefct(1);
		double const* const c_shapefct = shapefct;
		Eigen::RowVectorXd N(c_nnodes);
		N.setZero();
		for (int i=0; i<c_nnodes; i++)
			N(i) = c_shapefct[i];
		const double gp_p1 = N * nodal_p1;
		const double gp_T1 = N * nodal_T1;
		double var[3] = {};
		var[0] = gp_p1;
		var[1] = gp_T1;
		const double rhocp = MediaProp->HeatCapacity(Index, theta, this, var);
		const double fkt = vol / (double)nnodes;
		r_T += rhocp * (nodal_T1 - nodal_T0)/dt * fkt;
	}

	//======================================================================
	// Loop over Gauss points
	int gp_r, gp_s, gp_t;
	double var[3] = {};
	Eigen::RowVectorXd N(c_nnodes);
	Eigen::RowVectorXd W_T(c_nnodes), W_SUPG(c_nnodes);
	Eigen::MatrixXd dN(c_dim, c_nnodes);
	for (gp = 0; gp < nGaussPoints; gp++)
	{
		//---------------------------------------------------------
		//  Get local coordinates and weights
		//  Compute Jacobian matrix and its determinate
		//---------------------------------------------------------
		const double fkt = GetGaussData(gp, gp_r, gp_s, gp_t);
		// Compute geometry
		ComputeShapefct(1);
		ComputeGradShapefct(1);
		double const* const c_shapefct = shapefct;
		double const* const c_dshapefct = dshapefct;
		N.setZero();
		dN.setZero();
		for (int i=0; i<c_nnodes; i++)
			N(i) = c_shapefct[i];
		for (unsigned i = 0; i < c_dim; i++)
			for (int j = 0; j < c_nnodes; j++)
				dN(i,j) = c_dshapefct[i * c_nnodes + j];
		W_T = N;

		//---------------------------------------------------------
		//  Get state variables
		//---------------------------------------------------------
		const double gp_p0 = N * nodal_p0;
		const double gp_T0 = N * nodal_T0;
		const double gp_p1 = N * nodal_p1;
		const double gp_T1 = N * nodal_T1;
		Eigen::VectorXd grad_p1 = dN * nodal_p1;
		Eigen::VectorXd grad_T1 = dN * nodal_T1;

		//---------------------------------------------------------
		//  Get material properties
		//---------------------------------------------------------
		var[0] = gp_p1;
		var[1] = gp_T1;
		// Fluid properties
		const double rho_w = FluidProp->Density(var);
		const double vis = FluidProp->Viscosity(var);
		const double cp_w = FluidProp->SpecificHeatCapacity(var);
		// Medium properties
		const double rhocp = MediaProp->HeatCapacity(Index, theta, this, var);

		//---------------------------------------------------------
		//  Set velocity
		//---------------------------------------------------------
		Eigen::VectorXd grad_h1 = grad_p1;
		if (hasGravity)
			grad_h1 -= rho_w * vec_g;
		Eigen::VectorXd q = - k / vis * grad_h1;

		//---------------------------------------------------------
		//  SUPG coefficients
		//---------------------------------------------------------
		if (useSUPG)
		{
			W_SUPG.setZero();
			double supg_tau = 0;
			CalcSUPGWeightingFunction(&q[0], gp, supg_tau, &W_SUPG[0]);
			W_T += supg_tau * W_SUPG;
		}

		//---------------------------------------------------------
		//  Assemble Liquid flow equation
		//  original: N^T*S*N*dp/dt + dN^T*k/mu*dN*p+dN^T*rho*g*z = 0
		//---------------------------------------------------------
		// Rp += [1/dt*N^T*Ss*N + theta*dN^T*k/mu*dN]*p1 -
		// [1/dt*N^T*Ss*N - (1-theta)*dN^T*k/mu*dN]*p0 +
		// dN^T*k/mu*rho*g*z
		if (isTransient)
		{
			// 1/dt*N^T*Ss*(p1-p0)
			r_p.noalias() += N.transpose() * Ss * (gp_p1 - gp_p0)/dt * fkt;
		}
		// - dN^T*vel
		r_p.noalias() += - fkt * dN.transpose() * q;

		//---------------------------------------------------------
		//  Assemble Heat transport equation
		//---------------------------------------------------------

		// Rt += [1/dt*N^T*Cp*N + theta*(dN^T*lambda*dN+N^T*Cp_w*dN)]*T1 -
		// [1/dt*N^T*Cp*N - (1-theta)*(dN^T*lambda*dN+N^T*Cp_w*dN)]*T0
		if (isTransient && !useLumpedMass)
		{
			r_T.noalias() += W_T.transpose() * fkt * rhocp * (gp_T1 - gp_T0)/dt;
		}
		r_T.noalias() += fkt * dN.transpose() * lambda * grad_T1;
		r_T.noalias() += fkt * W_T.transpose() * rho_w * cp_w * q.transpose() * grad_T1;
	}

	if (isMatrixElement && isMatrixFlowInactive)
		r_p.setZero();

	for (int i = 0; i < c_nnodes; i++)
	{
		(*RHS)(offset_p + i) = r_p[i];
		(*RHS)(offset_T + i) = r_T[i];
	}

	if (pcs->scaleEQS)
	{
		for (int ii = 0; ii < 2; ii++)
		{
			const double scale_eqs = pcs->vec_scale_eqs[ii];
			for (int i = 0; i < c_nnodes; i++)
				(*RHS)(ii* c_nnodes + i) *= scale_eqs;
		}
	}

	// RHS should be - residual
	(*RHS) *= -1.;

#ifdef NEW_EQS
	for (size_t ii = 0; ii < pcs->GetPrimaryVNumber(); ii++)
		for (long i = 0; i < c_nnodes; i++)
			eqs_rhs[NodeShift[ii] + eqs_number[i]] += (*RHS)(i + ii * c_nnodes);
#endif
}

void CFiniteElementStd::AssembleTHJacobian()
{
	const unsigned c_dim = dim;
	const int c_nnodes = nnodes;
	const double dt = pcs->Tim->time_step_length;
	const double theta = pcs->m_num->ls_theta;
	const bool hasGravity =
		(coordinate_system) % 10 == 2 && FluidProp->CheckGravityCalculation();
	const double g_const = hasGravity ? gravity_constant : .0;
	const bool useSUPG = (pcs->m_num->ele_supg_method > 0);
	const bool useLumpedMass = (pcs->m_num->ele_mass_lumping > 0);
	const bool isTransient =
		(this->pcs->tim_type == FiniteElement::TIM_TRANSIENT);
	const bool isMatrixFlowInactive = pcs->deactivateMatrixFlow;
	const bool isMatrixElement = (MeshElement->GetDimension() == pcs->m_msh->GetMaxElementDim());

	Matrix t_transform_tensor(3, 3);
	if (dim > MediaProp->geo_dimension)
	{
		if (MeshElement->getTransformTensor() == NULL)
		{
			std::cout << "***Error: Geometric dimension in MMP is not "
						 "consistent with element."
					  << "\n";
			exit(0);
		}
		t_transform_tensor.LimitSize(dim, dim);
		MeshElement->getTransformTensor()->GetTranspose(t_transform_tensor);
	}

	Eigen::VectorXd nodal_p0(nnodes), nodal_p1(nnodes);
	Eigen::VectorXd nodal_T0(nnodes), nodal_T1(nnodes);
	for (int i = 0; i < nnodes; i++)
	{
		nodal_p0[i] = pcs->GetNodeValue(nodes[i], idxp0);
		nodal_p1[i] = pcs->GetNodeValue(nodes[i], idxp1);
		nodal_T0[i] = pcs->GetNodeValue(nodes[i], idxT0);
		nodal_T1[i] = pcs->GetNodeValue(nodes[i], idxT1);
	}

	// Calculate matrices
	const int offset_p = 0;
	const int offset_T = c_nnodes;
	(*StiffMatrix) = 0.0;  // Jacobian

//#define TH_DEBUG
#ifdef TH_DEBUG
	// for debugging
	Mass->LimitSize(nnodes, nnodes);
	Laplace->LimitSize(nnodes, nnodes);
	Advection->LimitSize(nnodes, nnodes);
	(*Mass) = .0;
	(*Laplace) = .0;
	(*Advection) = .0;
#endif
	double* tmp_k_tensor = MediaProp->PermeabilityTensor(Index);
	if (c_dim > MediaProp->geo_dimension)
	{
		Matrix local_tensor(c_dim, c_dim), temp_tensor(c_dim, c_dim),
			global_tensor(c_dim, c_dim);
		const unsigned c_ele_dim = ele_dim;
		for (size_t i = 0; i < c_ele_dim; i++)
			for (size_t j = 0; j < c_ele_dim; j++)
				local_tensor(i, j) = tmp_k_tensor[j + i * c_ele_dim];
		// cout << "K':" << endl; local_tensor.Write();
		local_tensor.multi(t_transform_tensor, temp_tensor);
		for (size_t i = 0; i < c_dim; i++)
			for (size_t j = 0; j < c_dim; j++)
				for (size_t k = 0; k < c_dim; k++)
					global_tensor(i, j) +=
						(*MeshElement->getTransformTensor())(i, k) *
						temp_tensor(k, j);
		// cout << "K:" << endl; global_tensor.Write();
		for (size_t i = 0; i < c_dim; i++)
			for (size_t j = 0; j < c_dim; j++)
				tmp_k_tensor[c_dim * i + j] = global_tensor(i, j);
	}
	double const* const k_tensor = tmp_k_tensor;
	const double Ss = MediaProp->StorageFunction(Index, unit, theta);
	double dummy[3] = {};
	double const* const lambda_tensor =
		MediaProp->HeatDispersionTensorNew(0, dummy);
	Eigen::MatrixXd k(c_dim, c_dim);
	for (unsigned i=0; i<c_dim; i++)
		for (unsigned j=0; j<c_dim; j++)
			k(i,j) = k_tensor[i*c_dim + j];
	Eigen::MatrixXd lambda(c_dim, c_dim);
	for (unsigned i=0; i<c_dim; i++)
		for (unsigned j=0; j<c_dim; j++)
			lambda(i,j) = lambda_tensor[i*c_dim + j];
	Eigen::Vector3d vec_g;
	vec_g << 0, 0, -g_const;

	//======================================================================
	Eigen::MatrixXd J_pp(c_nnodes, c_nnodes), J_pT(c_nnodes, c_nnodes);
	Eigen::MatrixXd J_Tp(c_nnodes, c_nnodes), J_TT(c_nnodes, c_nnodes);
	J_pp.setZero();
	J_pT.setZero();
	J_Tp.setZero();
	J_TT.setZero();

	//======================================================================
	// Mass lumping
	if (isTransient && useLumpedMass)
	{
		const double vol =
			MeshElement->GetVolume() * MeshElement->GetFluxArea();
		// Center of the reference element
		SetCenterGP();
		ComputeShapefct(1);
		double const* const c_shapefct = shapefct;
		Eigen::RowVectorXd N(c_nnodes);
		N.setZero();
		for (int i=0; i<c_nnodes; i++)
			N(i) = c_shapefct[i];
		const double gp_p1 = N * nodal_p1;
		const double gp_T1 = N * nodal_T1;
		double var[3] = {};
		var[0] = gp_p1;
		var[1] = gp_T1;
		const double rhocp = MediaProp->HeatCapacity(Index, theta, this, var);
		const double drho_w_dp = FluidProp->drhodP(var);
		const double drho_w_dT = FluidProp->drhodT(var);
		const double cp_w = FluidProp->SpecificHeatCapacity(var);
		const double porosity = MediaProp->Porosity(Index, theta);
		const double drhocp_dp = porosity * cp_w * drho_w_dp;
		const double drhocp_dT = porosity * cp_w * drho_w_dT;
		const double fkt = vol / (double)nnodes;
		J_TT.diagonal().setConstant(1/dt * rhocp * fkt);
		J_TT += fkt * drhocp_dT * (nodal_T1 - nodal_T0)/dt * N;
		J_Tp += fkt * drhocp_dp * (nodal_T1 - nodal_T0)/dt * N;
	}

	//======================================================================
	// Loop over Gauss points
	int gp_r, gp_s, gp_t;
	double var[3] = {};
	Eigen::RowVectorXd N(c_nnodes);
	Eigen::MatrixXd dN(c_dim, c_nnodes);
	Eigen::RowVectorXd W_T(c_nnodes), W_SUPG(c_nnodes);
	for (gp = 0; gp < nGaussPoints; gp++)
	{
		//---------------------------------------------------------
		//  Get local coordinates and weights
		//  Compute Jacobian matrix and its determinate
		//---------------------------------------------------------
		const double fkt = GetGaussData(gp, gp_r, gp_s, gp_t);
		// Compute geometry
		ComputeShapefct(1);
		ComputeGradShapefct(1);
		double const* const c_shapefct = shapefct;
		double const* const c_dshapefct = dshapefct;
		N.setZero();
		dN.setZero();
		for (int i=0; i<c_nnodes; i++)
			N(i) = c_shapefct[i];
		for (unsigned i = 0; i < c_dim; i++)
			for (int j = 0; j < c_nnodes; j++)
				dN(i,j) = c_dshapefct[i * c_nnodes + j];
		W_T = N;

		//---------------------------------------------------------
		//  Get state variables
		//---------------------------------------------------------
		// const double gp_p0 = N * nodal_p0;
		const double gp_T0 = N * nodal_T0;
		const double gp_p1 = N * nodal_p1;
		const double gp_T1 = N * nodal_T1;
		//const double gp_dp = gp_p1 - gp_p0;
		const double gp_dT = gp_T1 - gp_T0;
		//Eigen::VectorXd grad_p0 = dN * nodal_p0;
		Eigen::VectorXd grad_p1 = dN * nodal_p1;
		//Eigen::VectorXd grad_T0 = dN * nodal_T0;
		Eigen::VectorXd grad_T1 = dN * nodal_T1;
		if (pcs->m_num->nls_jacobian_level == 1)
			grad_T1.setZero();

		//---------------------------------------------------------
		//  Get material properties
		//---------------------------------------------------------
		var[0] = gp_p1;
		var[1] = gp_T1;
		// Fluid properties
		const double rho_w = FluidProp->Density(var);
		const double drho_w_dp = FluidProp->drhodP(var);
		const double drho_w_dT = FluidProp->drhodT(var);
		const double vis = FluidProp->Viscosity(var);
		const double dvis_dp = FluidProp->dViscositydP(var);
		const double dvis_dT = FluidProp->dViscositydT(var);
		const double cp_w = FluidProp->SpecificHeatCapacity(var);
		// Medium properties
		const double rhocp = MediaProp->HeatCapacity(Index, theta, this, var);
		const double porosity = MediaProp->Porosity(Index, theta);
		const double drhocp_dp = porosity * cp_w * drho_w_dp; //TODO d(cp)/dp
		const double drhocp_dT = porosity * cp_w * drho_w_dT;

		//---------------------------------------------------------
		//  Set velocity
		//---------------------------------------------------------
		Eigen::VectorXd grad_h1 = grad_p1;
		if (hasGravity)
			grad_h1 -= rho_w * vec_g;
		Eigen::VectorXd q = - k / vis * grad_h1;

		//-----------------------------------------
		// Derivatives of flow velocity and heat flux
		//-----------------------------------------
		Eigen::MatrixXd dq_dp = - k / vis * dN;
		if (hasGravity && drho_w_dp != 0.0)
			dq_dp.noalias() += - k / vis * drho_w_dp * vec_g * N;
		if (dvis_dp != 0.0)
			dq_dp.noalias() += - dvis_dp / vis * q * N;
		Eigen::MatrixXd dq_dT = - dvis_dT * q * N;
		if (hasGravity && drho_w_dT != 0.0)
			dq_dT.noalias() += - k / vis * drho_w_dT * vec_g * N;

		Eigen::MatrixXd djDiff_dT = - lambda * dN;
		Eigen::MatrixXd djAdv_dT = rhocp * q.transpose() * dN;
		if (dq_dT.size() > 0)
			djAdv_dT.noalias() += rhocp * grad_T1.transpose() * dq_dT;
		if (drho_w_dT != .0)
			djAdv_dT.noalias() += drhocp_dT * q.transpose() * grad_T1 * N;
		Eigen::MatrixXd djAdv_dp = rhocp * grad_T1.transpose() * dq_dp;
		if (drho_w_dp != .0)
			djAdv_dp.noalias() += drhocp_dp * q.transpose() * grad_T1 * N;

		//---------------------------------------------------------
		//  SUPG coefficients
		//---------------------------------------------------------
		if (useSUPG)
		{
			W_SUPG.setZero();
			double supg_tau = 0;
			CalcSUPGWeightingFunction(&q[0], gp, supg_tau, &W_SUPG[0]);
			W_T += supg_tau * W_SUPG;
		}

		//---------------------------------------------------------
		//  Assemble Jacobian
		//---------------------------------------------------------

		if (isTransient)
			J_pp.noalias() += 1/dt * fkt * N.transpose() * Ss * N;
		J_pp.noalias() += - fkt * dN.transpose() * dq_dp;
		if (dq_dT.size() > 0)
			J_pT.noalias() += - fkt * dN.transpose() * dq_dT;

		if (isTransient && !useLumpedMass)
			J_TT.noalias() += 1/dt * W_T.transpose() * (rhocp + gp_dT * drhocp_dT) * N * fkt;
		J_TT.noalias() += - dN.transpose() * djDiff_dT * fkt;
		J_TT.noalias() += W_T.transpose() * djAdv_dT * fkt;
		if (isTransient && !useLumpedMass)
			J_Tp.noalias() += 1/dt * W_T.transpose() * (gp_dT * drhocp_dp) * N * fkt;
		J_Tp.noalias() += W_T.transpose() * djAdv_dp * fkt;
	}

	if (isMatrixElement && isMatrixFlowInactive)
	{
		J_pp.setZero();
		J_pT.setZero();
		J_Tp.setZero();
	}

	for (int i=0; i<c_nnodes; i++)
	{
		for (int j=0; j<c_nnodes; j++)
		{
			(*StiffMatrix)(offset_p + i, offset_p + j) = J_pp(i, j);
			(*StiffMatrix)(offset_p + i, offset_T + j) = J_pT(i, j);
			(*StiffMatrix)(offset_T + i, offset_p + j) = J_Tp(i, j);
			(*StiffMatrix)(offset_T + i, offset_T + j) = J_TT(i, j);
		}
	}

	if (pcs->scaleUnknowns || pcs->scaleEQS)
	{
		// scale p by 1e6
		const double scale_p =
			1. / pcs->vec_scale_dofs[0] * pcs->vec_scale_eqs[0];
		for (long i = 0; i < c_nnodes; i++)
			for (long j = 0; j < c_nnodes; j++)
			{
				(*StiffMatrix)(i, j) *= scale_p;
				(*StiffMatrix)(i, j + c_nnodes) *= scale_p;
				//				(*StiffMatrix)(i+c_nnodes,j) *= scale_p;
			}
		// scale T
		const double scale_T =
			1. / pcs->vec_scale_dofs[1] * pcs->vec_scale_eqs[1];
		for (long i = 0; i < c_nnodes; i++)
			for (long j = 0; j < c_nnodes; j++)
			{
				(*StiffMatrix)(i + c_nnodes, j) *= scale_T;
				(*StiffMatrix)(i + c_nnodes, j + c_nnodes) *= scale_T;
				//				(*StiffMatrix)(i,j+c_nnodes) *= scale_T;
				//				(*StiffMatrix)(i+c_nnodes,j+c_nnodes) *=
				// scale_T;
			}
	}
}

}  // end namespace


#include <iostream>
#include <boost/math/common_factor.hpp>
using namespace std;

int main() {
	// your code goes here
	int t;
	cin>>t;
	while(t--)
	{
	    long long a,b;
	    cin>>a>>b;
	    cout<<boost::math::gcd(a,b)<<" "<<boost::math::lcm(a,b)<<"\n" ;
	}
	return 0;
}


#include "headers/Triangle.hpp"
#include "headers/Object.hpp"
#include "headers/Hittable.hpp"
#include <armadillo>
#include <vector>

using namespace arma;
using namespace std;

Triangle::Triangle (vec triangulo){

    mat transf;
    vec kd, ks, aux;

    transf << triangulo(16) << triangulo(17) << triangulo(18) << triangulo(19) << endr
	   << triangulo(20) << triangulo(21) << triangulo(22) << triangulo(23) << endr
           << triangulo(24) << triangulo(25) << triangulo(26) << triangulo(27) << endr
           << 0 << 0 << 0 << 1;

    this->setTransf(transf);

    aux << triangulo(0) << triangulo(1) << triangulo(2) << 1;
    aux = transf * aux;
    aux.shed_row(3);

    this->setA(aux);
//    this->setA(triangulo(0) << triangulo(1) << triangulo(2) << 1);
//    this->setA(transf * this->getA());
//    this->setA(this->getA().shed_row(3));

    aux << triangulo(3) << triangulo(4) << triangulo(5) << 1;
    aux = transf * aux;
    aux.shed_row(3);

    this->setB(aux);

//    this->setB(triangulo(3) << triangulo(4) << triangulo(5) << 1);
//    this->setB(transf * this->getB());
//    this->setB(this->getB().shed_row(3));

    aux << triangulo(6) << triangulo(7) << triangulo(8) << 1;
    aux = transf * aux;
    aux.shed_row(3);

    this->setC(aux);

//    this->setC(triangulo(6) << triangulo(7) << triangulo(8) << 1);
//    this->setC(transf * this->getC());
//    this->setC(this->getC().shed_row(3));

    kd << triangulo(9) << triangulo(10) << triangulo(11);
    ks << triangulo(12) << triangulo(13) << triangulo(14);

    this->setKd(kd);
    this->setKs(ks);

    this->setP(triangulo(15));

    this->setTipo(1);

    this->setCentro((this->getA() + this->getB() + this->getC())/3);

    this->setNormal(cross(this->getB() - this->getA(), this->getC() - this->getA()));
}

bool Triangle::colide(const vec &d, double &T, const vec &origem){

    vec tuv;
    mat matriz1, matriz2;

    matriz1.insert_cols(0, origem-d);
    matriz1.insert_cols(1, this->B - this->A);
    matriz1.insert_cols(2, this->C - this->A);

    if ((det(matriz1)>0.000001) || (det(matriz1)<-0.000001)){

        matriz2 = origem - this->A;

	tuv = matriz1.i() * matriz2;

	if ((tuv(1)>=0) && (tuv(1)<=1) && (tuv(2)>=0) && (tuv(2)<=1) && (tuv(1)+tuv(2)<=1)){

            T=tuv(0);
            setNormal(normalise(getNormal()));

            return true;
	}
    }

    return false;
}


#include <Eigen/Core>
#include <list>
#include <map>
#include <random>
#include "rose499/world.hpp"

namespace geom = boost::geometry;

using Point = World::Point;
using TaggedPoint = std::pair< Point, int >;

namespace
{
    std::mt19937_64 engine(0x1EA5ECED);
}

std::list< Point > planRRT(     World::RTree knownWorld,
                                double xMax,
                                double yMax,
                                Point initialPoint,
                                double direction,
                                Point goal,
                                double radius )
{
    std::uniform_real_distribution<> samplerX(0, xMax), samplerY(0, yMax);

    TaggedPoint lastPoint(initialPoint, 0);
    double closestDist = std::max(xMax, yMax);

    geom::index::rtree< TaggedPoint, geom::index::linear<5, 1> > setPoints;
    std::map< uint32_t, uint32_t > backEdges;
    std::vector< TaggedPoint > listPoints;
    std::vector< TaggedPoint > setNearest;
    std::vector< std::pair<World::Box, int> > setCollisions;

    setPoints.insert( lastPoint );
    listPoints.push_back( lastPoint );
    int indTaggedPoint = 1;
    while( Eigen::Vector2d( lastPoint.first.get<0>() - goal.get<0>(),
                            lastPoint.first.get<1>() - goal.get<1>() ).norm() >= radius )
    {
        setNearest.clear();
        setCollisions.clear();

        std::normal_distribution<> importanceSamplerX(goal.get<0>(), closestDist),
                                   importanceSamplerY(goal.get<1>(), closestDist);
        auto x = importanceSamplerX(engine);
        auto y = importanceSamplerY(engine);

      if( x > xMax || x < 0 || y > yMax || y < 0 ) { continue; }

        auto randomPoint = TaggedPoint(Point(x, y), indTaggedPoint);
        Eigen::Vector2d eigRandomPoint(randomPoint.first.get<0>(), randomPoint.first.get<1>());

        // Just make sure this isn't a crap point that is inside an obstacle.
        knownWorld.query( geom::index::intersects(randomPoint.first), std::back_inserter(setCollisions));

      if( !setCollisions.empty() ) { continue; }

        // find nearest point in our set
        setPoints.query( geom::index::nearest(randomPoint.first, 10), std::back_inserter(setNearest) );

        // Filter out points that are either too close OR do not fan out from the source point.
        auto endSetNearest =
            std::remove_if(
                setNearest.begin(),
                setNearest.end(),
                [direction, eigRandomPoint, &backEdges, &listPoints]( TaggedPoint nearest ) -> bool {
                    Eigen::Vector2d eigNearest(nearest.first.get<0>(), nearest.first.get<1>());
                    Eigen::Vector2d eigFrom(std::cos(direction), std::sin(direction));
                    Eigen::Vector2d vecLeaving = eigRandomPoint - eigNearest;
                    Eigen::Vector2d vecEntering = eigFrom;

                    if( nearest.second != 0 )
                    {
                        auto fromPoint = listPoints[backEdges[nearest.second]];
                        eigFrom = Eigen::Vector2d(fromPoint.first.get<0>(), fromPoint.first.get<1>());
                        vecEntering = eigNearest - eigFrom;
                    }

                    double proj = vecLeaving.dot(vecEntering) / (vecLeaving.norm() * vecEntering.norm());

                    return (vecLeaving.dot(vecEntering) < 0)
                        || (std::acos(proj) > std::atan2(1, 1))
                        ;//|| (vecLeaving.norm() < 1);
                }
            );
        setNearest.erase(endSetNearest, setNearest.end());

      if( setNearest.empty() ) { continue; }

        std::sort(  setNearest.begin(),
                    setNearest.end(),
                    [eigRandomPoint](TaggedPoint a, TaggedPoint b) -> bool {
                        return  (Eigen::Vector2d(a.first.get<0>(), a.first.get<1>()) - eigRandomPoint).squaredNorm()
                                <
                                (Eigen::Vector2d(b.first.get<0>(), b.first.get<1>()) - eigRandomPoint).squaredNorm();
                    } );
        TaggedPoint nearestPoint = setNearest.front();

        // gotta make sure the path is obstacle free
        geom::model::segment< World::Point > lineTo(nearestPoint.first, randomPoint.first);
        knownWorld.query( geom::index::intersects(lineTo), std::back_inserter(setCollisions));

      if( !setCollisions.empty() ) { continue; }

        closestDist = std::min( closestDist,
                        std::max( std::abs(nearestPoint.first.get<0>() - goal.get<0>()),
                                  std::abs(nearestPoint.first.get<1>() - goal.get<1>()) )
                      );

        setPoints.insert(randomPoint);
        listPoints.push_back(randomPoint);
        backEdges[randomPoint.second] = nearestPoint.second;
        indTaggedPoint++;
        lastPoint = randomPoint;
    }

    // Build path from back edges
    std::list< Point > path;
    int tag = 0;
    while( (tag = lastPoint.second) != 0 )
    {
        path.push_front(lastPoint.first);
        lastPoint = listPoints[backEdges[tag]];
    }
    path.push_front(lastPoint.first);
    return path;
}