#include "Lib/spin_system.h"
#include "lib/log_trace.h"
#include "Lib/simulate.h"
#include "lib/grid.h"
#include "lib/constants.h"
#include "lib/P_lists.h"
#include "Lib/load_parameters.h"

#include <omp.h>

#include <iostream>
#include <filesystem>
#include <unsupported/Eigen/MatrixFunctions>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <vector>
#include <fstream>
#include <string>
#include <vector>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <boost/ref.hpp>
#include <math.h>

#include <chrono>

namespace fs = std::experimental::filesystem;

template<class P_list>
P_list create_8_ns_P_list(P_list const & P_list_2ns) {

	/*
	this function creates propagators that capture an 8ns step from propagators that capture a 2ns step
	*/

	//2ns
	P_list temp(P_list_2ns);
	//4ns
	temp.multiply(temp,true);
	//8ns
	temp.multiply(temp, true);

	return temp;
}

template<class P_list>
P_list repeat_P_list(P_list const & P_list_step,double t_end) {

	/*
	this function repeats the propagators in P_list_step upto a time step t_end is captured
	*/

	P_list temp(P_list_step);
	temp.reset();

	while (temp.t_step < t_end ) {
		temp.multiply(P_list_step, true);
	}

	return temp;
}

template<class P_list>
P_list reverse_P_list(P_list const & P_list_start, P_list const & P_list_step, double t_end) {

	/*
	this function calculates propagators that capture a time step of the length t_end by reverting the
	propgatros in P_list_start with the propagators in P_list_step
	*/

	P_list temp(P_list_start);

	while ( temp.t_step > t_end) {
		temp.multiply(P_list_step, false);

	}

	return temp;
}

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

	try {
		if (argc < 4)
		{
			throw std::runtime_error("Not enough arguments given");
		}
	}
	catch (std::runtime_error &er) {
		std::cout << er.what() << "\n";
	}

	double t_step;
	std::vector<double> taus;
	Spin_System_Doublet_Triplet spin_time_2;
	Spin_System_Doublet_Triplet spin_pulse_2;
	std::vector<double> t;

	Spin_System_Doublet spin_time;
	Spin_System_Doublet spin_pulse;

	auto temp = boost::make_tuple(boost::ref(spin_time), boost::ref(spin_pulse), boost::ref(t_step), boost::ref(taus), boost::ref(t));
	temp = load_parameters_doublet(argv[1]);

	auto temp2 = boost::make_tuple(boost::ref(spin_time_2), boost::ref(spin_pulse_2), boost::ref(t_step), boost::ref(taus), boost::ref(t));
	temp2 = load_parameters_Doublet_Triplet(argv[1]);

	double const t0 = t[0];
	double const t1 = t[1];
	double const t2 = t[2];
	double const t3 = t[3];
	double const t4 = t[4];
	double const t5 = t[5];
	double t_pi_2 = t[1];

	double t_tau = taus[1] - taus[0];

	Log_stream log_stream(argv[2], "integral", t_step);

	std::vector<double> phi_doublet;
	std::vector<double> theta_doublet;
	std::vector<double> weights_doublet;
	auto temp_doublet = boost::make_tuple(boost::ref(phi_doublet), boost::ref(theta_doublet), boost::ref(weights_doublet));
	temp_doublet = grid(argv[3],spin_time_2.symmetrie_doublet, spin_time_2.knots_doublet);

	std::vector<double> phi_triplet;
	std::vector<double> theta_triplet;
	std::vector<double> weights_triplet;
	auto temp_triplet = boost::make_tuple(boost::ref(phi_triplet), boost::ref(theta_triplet), boost::ref(weights_triplet));
	temp_triplet = grid(argv[3], spin_time_2.symmetrie_triplet, spin_time_2.knots_triplet);

	std::vector<double> phi_dipolar;
	std::vector<double> theta_dipolar;
	std::vector<double> weights_dipolar;
	auto temp_dipolar = boost::make_tuple(boost::ref(phi_dipolar), boost::ref(theta_dipolar), boost::ref(weights_dipolar));
	temp_dipolar = grid(argv[3], spin_time_2.symmetrie_dipolar, spin_time_2.knots_dipolar);

	//the propagators for the Doublet system (before Laser excitation) are simulated in advance, they are updated later on capture the shift of the laser flash

	P_list_one_rot<Spin_System_Doublet> P_list_time_2ns(spin_time, t_step, theta_doublet, phi_doublet); P_list_time_2ns.create_P_list();
	P_list_one_rot<Spin_System_Doublet> P_list_time_8ns(create_8_ns_P_list(P_list_time_2ns));
	P_list_one_rot<Spin_System_Doublet> P_list_time_fid1(repeat_P_list(P_list_time_8ns, t2 - t1));
	P_list_one_rot<Spin_System_Doublet> P_list_time_fid2(repeat_P_list(P_list_time_8ns, t4 - t3));

	P_list_one_rot<Spin_System_Doublet> P_list_pulse_2ns(spin_pulse, t_step, theta_doublet, phi_doublet); P_list_pulse_2ns.create_P_list();
	P_list_one_rot<Spin_System_Doublet> P_list_pulse_8ns(create_8_ns_P_list(P_list_pulse_2ns));
	P_list_one_rot<Spin_System_Doublet>  P_list_pulse_pi_2(repeat_P_list(P_list_pulse_2ns, t_pi_2));
	P_list_one_rot<Spin_System_Doublet>  P_list_pulse_pi(P_list_pulse_pi_2);  P_list_pulse_pi.multiply(P_list_pulse_pi, true);

	P_list_one_rot<Spin_System_Doublet> P_list_dummy(P_list_pulse_2ns);
	Spin_System_Doublet::operator_type rho_eq = spin_time.get_rho_equ();

	Spin_System_Doublet::operator_type rho_doublet = Spin_System_Doublet::operator_type::Zero();

	std::vector<std::vector<Spin_System_Doublet::operator_type>> rho_doublets;

	for (auto tau : taus) {

		std::vector<Spin_System_Doublet::operator_type > rho_doublet_angles;

		log_stream.save_x(tau);

		std::cout << tau*1e9 << std::endl;

		if (tau < t0) {  //laser before first pulse

		}
		else if (tau >= t0 && tau < t1) {  //laser during first pulse

			if ((tau - t0) < t_tau) {
				P_list_dummy.copy_P_list(repeat_P_list(P_list_pulse_2ns, tau - t0));
			}
			else {
				P_list_dummy.multiply(P_list_pulse_8ns, true);
			}

		}
		else if (tau >= t1 && tau < t2) { //laser during first fid

			if ((tau - t1) < t_tau) {
				P_list_dummy.copy_P_list(repeat_P_list(P_list_time_2ns, tau - t1));
			}
			else {
				P_list_dummy.multiply(P_list_time_8ns, true);
			}
		}
		else if (tau >= t2 && tau < t3) { //laser during refocussing pulse

			if ((tau - t2) < t_tau) {
				P_list_dummy.copy_P_list(repeat_P_list(P_list_pulse_2ns, tau - t2));
			}
			else {
				P_list_dummy.multiply(P_list_pulse_8ns, true);
			}

		}
		else if (tau >= t3 && tau < t4) { //laser during second fid(echo)

			if ((tau - t3) < t_tau) {
				P_list_dummy.copy_P_list(repeat_P_list(P_list_time_2ns, tau - t3));
			}
			else {
				P_list_dummy.multiply(P_list_time_8ns, true);
			}
		}
		else {}

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

			double w_doublet = weights_doublet[i];
			Spin_System_Doublet::operator_type rho;

			if (tau < t0) {  //laser before first pulse

				rho = rho_eq;

			}
			else if (tau >= t0 && tau < t1) {  //laser during first pulse
				rho = simulate_P(spin_pulse, P_list_dummy.P_list[i], t0, tau, w_doublet, rho_eq, Log_trace_none{});

			}
			else if (tau >= t1 && tau < t2) { //laser during first fid
				rho = simulate_P(spin_pulse, P_list_pulse_pi_2.P_list[i], t0, t1, w_doublet, rho_eq, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_dummy.P_list[i], t1, tau, w_doublet, rho, Log_trace_none{});

			}
			else if (tau >= t2 && tau < t3) { //laser during refocussing pulse
				rho = simulate_P(spin_pulse, P_list_pulse_pi_2.P_list[i], t0, t1, w_doublet, rho_eq, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid1.P_list[i], t1, t2, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_pulse, P_list_dummy.P_list[i], t2, tau, w_doublet, rho, Log_trace_none{});

			}
			else if (tau >= t3 && tau < t4) { //laser during second fid(echo)
				rho = simulate_P(spin_pulse, P_list_pulse_pi_2.P_list[i], t0, t1, w_doublet, rho_eq, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid1.P_list[i], t1, t2, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_pulse, P_list_pulse_pi.P_list[i], t2, t3, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_dummy.P_list[i], t3, tau, w_doublet, rho, Log_trace_none{});

			}
			else if (tau >= t4 && tau < t5) { //laser during echo-detection
				rho = simulate_P(spin_pulse, P_list_pulse_pi_2.P_list[i], t0, t1, w_doublet, rho_eq, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid1.P_list[i], t1, t2, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_pulse, P_list_pulse_pi.P_list[i], t2, t3, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid2.P_list[i], t3, t4, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_2ns.P_list[i], t4, tau, w_doublet, rho, log_stream);

			}
			else if (tau >= t5) { //laser after echo
				rho = simulate_P(spin_pulse, P_list_pulse_pi_2.P_list[i], t0, t1, w_doublet, rho_eq, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid1.P_list[i], t1, t2, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_pulse, P_list_pulse_pi.P_list[i], t2, t3, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_fid2.P_list[i], t3, t4, w_doublet, rho, Log_trace_none{});
				rho = simulate_P(spin_time, P_list_time_2ns.P_list[i], t4, t5, w_doublet, rho, log_stream);
			}
			else {
			}
			rho_doublet_angles.push_back(rho);
		}

		rho_doublets.push_back(rho_doublet_angles);
		log_stream.next();
	}

	log_stream.reset();

	Eigen::Matrix3cd rho_T;

	spin_time_2.create_rho_T_list(theta_triplet, phi_triplet);

	bool first_part=false;

	//the propagators for the Doublet_Triplet system (after Laser excitation) are simulated in advance, they are updated later on capture the shift of the laser flash

	Spin_System_Doublet_Triplet::operator_type rho = Spin_System_Doublet_Triplet::operator_type::Zero();

	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_time_2ns(spin_time_2, t_step, theta_doublet, phi_doublet, theta_triplet, phi_triplet, theta_dipolar, phi_dipolar); P_list_full_time_2ns.create_P_list();
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_time_8ns(create_8_ns_P_list(P_list_full_time_2ns));
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_time_fid1(repeat_P_list(P_list_full_time_8ns, t2 - t1));
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_time_fid2(repeat_P_list(P_list_full_time_8ns, t4 - t3));

	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_pulse_2ns(spin_pulse_2, t_step, theta_doublet, phi_doublet, theta_triplet, phi_triplet, theta_dipolar, phi_dipolar); P_list_full_pulse_2ns.create_P_list();
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_pulse_8ns(create_8_ns_P_list(P_list_full_pulse_2ns));
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_pulse_pi_2(repeat_P_list(P_list_full_pulse_2ns, t_pi_2));
	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_pulse_pi(P_list_full_pulse_pi_2);  P_list_full_pulse_pi.multiply(P_list_full_pulse_pi, true);

	P_list_three_rot<Spin_System_Doublet_Triplet> P_list_full_dummy(P_list_full_pulse_2ns);

	for (int q = 0; q < taus.size();++q) {
		double tau = taus[q];
		log_stream.save_x(tau);

		//show current tau on screen
		std::cout << tau*1e9 << "\n";

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

			rho_doublet = rho_doublets[q][i];

			for (int k = 0; k < phi_dipolar.size(); ++k) {

				for (int m = 0; m < phi_triplet.size(); ++m) {

					rho_T = spin_time_2.get_rho_T_list(m);

					double w= weights_doublet[i] * weights_dipolar[k] * weights_triplet[m];

					if (tau < t0) {  //laser before first pulse

						if (!first_part) {
							rho = Eigen::kroneckerProduct(rho_eq, rho_T);
							rho = simulate_P(spin_pulse_2, P_list_full_pulse_pi_2.P_list[i][m][k], t0, t1, w, rho, Log_trace_none{});
							rho = simulate_P(spin_time_2, P_list_full_time_fid1.P_list[i][m][k], t1, t2, w, rho, Log_trace_none{});
							rho = simulate_P(spin_pulse_2, P_list_full_pulse_pi.P_list[i][m][k], t2, t3, w, rho, Log_trace_none{});
							rho = simulate_P(spin_time_2, P_list_full_time_fid2.P_list[i][m][k], t3, t4, w, rho, Log_trace_none{});
							rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], t4, t5, w, rho, log_stream);

						}
						else {
							log_stream.copy_last_point();
							goto escape_point;
						}

					}
					else if (tau >= t0 && tau < t1) {  //laser during first pulse

						rho = Eigen::kroneckerProduct(rho_doublet, rho_T);
						rho = simulate_P(spin_pulse_2, P_list_full_dummy.P_list[i][m][k], tau, t1, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_fid1.P_list[i][m][k], t1, t2, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_pulse_2, P_list_full_pulse_pi.P_list[i][m][k], t2, t3, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_fid2.P_list[i][m][k], t3, t4, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], t4, t5, w, rho, log_stream);
					}
					else if (tau >= t1 && tau < t2) { //laser during first fid

						rho = Eigen::kroneckerProduct(rho_doublet, rho_T);
						rho = simulate_P(spin_time_2, P_list_full_dummy.P_list[i][m][k], tau, t2, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_pulse_2, P_list_full_pulse_pi.P_list[i][m][k], t2, t3, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_fid2.P_list[i][m][k], t3, t4, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], t4, t5, w, rho, log_stream);

					}
					else if (tau >= t2 && tau < t3) { //laser during refocussing pulse

						rho = Eigen::kroneckerProduct(rho_doublet, rho_T);
						rho = simulate_P(spin_pulse_2, P_list_full_dummy.P_list[i][m][k], tau, t3, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_fid2.P_list[i][m][k], t3, t4, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], t4, t5, w, rho, log_stream);

					}
					else if (tau >= t3 && tau < t4) { //laser during second fid(echo)

						rho = Eigen::kroneckerProduct(rho_doublet, rho_T);
						rho = simulate_P(spin_time_2, P_list_full_dummy.P_list[i][m][k], tau, t4, w, rho, Log_trace_none{  });
						rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], t4, t5, w, rho, log_stream);

					}
					else if (tau >= t4 && tau < t5) { //laser during echo-detection

						rho = Eigen::kroneckerProduct(rho_doublet, rho_T);
						rho = simulate_P(spin_time_2, P_list_full_time_2ns.P_list[i][m][k], tau, t5, w, rho, log_stream);
					}
					else if (tau >= t5) { //laser after echo

					}
					else {	}

				}//end T angle loop
			}//end dipolar angle loop

		}//end obs loop

		first_part = true;
		escape_point:;
		log_stream.next();

		if (tau < t0) {  //laser before first pulse
			if ((t0 - tau) < t_tau) {
				P_list_full_dummy.copy_P_list(reverse_P_list(P_list_full_pulse_pi_2, P_list_full_pulse_2ns, t1  - (tau + t_tau)));
			}
			else {

			}

		}
		else if (tau >= t0 && tau < t1) {  //laser during first pulse

			if ((t1 - tau) < t_tau) {
				P_list_full_dummy.copy_P_list(reverse_P_list(P_list_full_time_fid1, P_list_full_time_2ns, t2  - (tau + t_tau)));
			}
			else {
				P_list_full_dummy.multiply(P_list_full_pulse_8ns, false);
			}
		}
		else if (tau >= t1 && tau < t2) { //laser during first fid

			if ((t2 - tau) < t_tau) {
				P_list_full_dummy.copy_P_list(reverse_P_list(P_list_full_pulse_pi, P_list_full_pulse_2ns, t3 - (tau + t_tau)));
			}
			else {
				P_list_full_dummy.multiply(P_list_full_time_8ns, false);
			}

		}
		else if (tau >= t2 && tau < t3) { //laser during refocussing pulse

			if ((t3 - tau) < t_tau) {
				P_list_full_dummy.copy_P_list(reverse_P_list(P_list_full_time_fid2, P_list_full_time_2ns, t4  - (tau + t_tau)));
			}
			else {
				P_list_full_dummy.multiply(P_list_full_pulse_8ns, false);
			}

		}
		else if (tau >= t3 && tau < t4) { //laser during second fid(echo)

			if ((t4 - tau) < t_tau) {

			}
			else {
				P_list_full_dummy.multiply(P_list_full_time_8ns, false);
			}
		}
		else if (tau >= t4 && tau < t5) { //laser during echo-detection

		}
		else if (tau >= t5) { //laser after echo

		}
		else {}

	} //end tau loop
	log_stream.reset();

	log_stream.save_data();

}


//
// Created by Dmitri Bagaev on 2019-03-22.
//

#include "bayesian.h"

#define USE_NLOPT

#include <nlopt.hpp>

#include <boost/parameter/aux_/void.hpp>

#include <limbo/opt/nlopt_no_grad.hpp>
#include <Eigen/Core>
#include <limbo/kernel/exp.hpp>
#include <limbo/kernel/squared_exp_ard.hpp>
#include <limbo/mean/function_ard.hpp>
#include <limbo/model/gp.hpp>
#include <limbo/model/gp/kernel_lf_opt.hpp>
#include <limbo/tools.hpp>
#include <limbo/tools/macros.hpp>
#include <limbo/bayes_opt/bo_base.hpp>
#include <limbo/bayes_opt/boptimizer.hpp>
#include <limbo/model/gp/kernel_mean_lf_opt.hpp>
#include <limbo/model/multi_gp.hpp>
#include <limbo/acqui/gp_ucb.hpp>
#include <limbo/acqui/ei.hpp>

namespace INMOST {

    BayesianUniformDistribution::BayesianUniformDistribution() : distribution(0.0, 1.0) {
        int rank, size;

        MPI_Comm_rank(MPI_COMM_WORLD, &rank);
        MPI_Comm_size(MPI_COMM_WORLD, &size);

        unsigned int seed = static_cast<unsigned int>(time(NULL));

        if (rank == 0) {
            for (int i = 1; i < size; i++) {
                MPI_Send(&seed, 1, MPI_UNSIGNED, i, 0, MPI_COMM_WORLD);
            }
        } else {
            MPI_Status status;
            MPI_Recv(&seed, 1, MPI_UNSIGNED, 0, 0, MPI_COMM_WORLD, &status);
        }

        generator.seed(seed);
    }

    double BayesianUniformDistribution::next() {
        return distribution(generator);
    }

    unsigned int BayesianUniformDistribution::operator()(unsigned int n) {
        return static_cast<unsigned int>(next() * n);
    }

    unsigned int    BayesianOptimizer::DEFAULT_UNIQUE_POINTS_MAX_COUNT    = 7;
    unsigned int    BayesianOptimizer::DEFAULT_UNIQUE_POINTS_RANDOM_COUNT = 5;
    unsigned int    BayesianOptimizer::DEFAULT_INITIAL_ITERATIONS_COUNT   = 5;
    double          BayesianOptimizer::DEFAULT_INITIAL_ITERATIONS_RADIUS  = 0.2;
    double          BayesianOptimizer::DEFAULT_MAX_JUMP_BARRIER           = 0.1;

    BayesianOptimizer::BayesianOptimizer(const std::string &name, const OptimizationParameters &space, const OptimizerProperties &properties, std::size_t buffer_capacity) :
            OptimizerInterface(name, space, properties, buffer_capacity),
            unique_points_max_count(BayesianOptimizer::DEFAULT_UNIQUE_POINTS_MAX_COUNT),
            unique_points_random_count(BayesianOptimizer::DEFAULT_UNIQUE_POINTS_RANDOM_COUNT),
            initial_iterations_count(BayesianOptimizer::DEFAULT_INITIAL_ITERATIONS_COUNT),
            initial_iterations_radius(BayesianOptimizer::DEFAULT_INITIAL_ITERATIONS_RADIUS),
            max_jump_barrier(BayesianOptimizer::DEFAULT_MAX_JUMP_BARRIER) {

        if (this->HasProperty("unique_points_max_count")) {
            unique_points_max_count = static_cast<unsigned int>(std::atoi(this->GetProperty("unique_points_max_count").c_str()));
        }

        if (this->HasProperty("unique_points_random_count")) {
            unique_points_random_count = static_cast<unsigned int>(std::atoi(this->GetProperty("unique_points_random_count").c_str()));
        }

        if (this->HasProperty("initial_iterations_count")) {
            initial_iterations_count = static_cast<unsigned int>(std::atoi(this->GetProperty("initial_iterations_count").c_str()));
        }

        if (this->HasProperty("initial_iterations_radius")) {
            initial_iterations_radius = std::atof(this->GetProperty("initial_iterations_radius").c_str());
        }

        if (this->HasProperty("max_jump_barrier")) {
            max_jump_barrier = std::atof(this->GetProperty("max_jump_barrier").c_str());
        }

    }

    SuggestionChangedParameters BayesianOptimizer::AlgorithmMakeSuggestion() const {

        auto unique  = results.GetLastUniqueEntries(unique_points_max_count);
        auto entries = parameters.GetParameterEntries();

        std::vector<SuggestionChangedParameter> changed_parameters;

        if (unique.size() == 0) {

            std::size_t index = 0;
            std::for_each(entries.cbegin(), entries.cend(), [&index, &changed_parameters](const OptimizationParametersEntry &entry) {
                changed_parameters.emplace_back(SuggestionChangedParameter(index++, entry.first.GetDefaultValue()));
            });

            return changed_parameters;
        } else if (unique.size() < initial_iterations_count) {

            std::size_t index = 0;
            std::for_each(entries.cbegin(), entries.cend(), [&index, &changed_parameters, this, &unique](const OptimizationParametersEntry &entry) {
                auto parameter = entry.first;

                double min_bound = parameter.GetMinimalValue();
                double max_bound = parameter.GetMaximumValue();

                double r = random.next() * (max_bound - min_bound) * initial_iterations_radius;

                double next = entry.second + r * (2.0 * (unique.size() % 2) - 1.0);

                while (next < min_bound || next > max_bound) {
                    r    = random.next() * (max_bound - min_bound) * initial_iterations_radius;
                    next = entry.second + r * (2.0 * (unique.size() % 2) - 1.0);
                }

                changed_parameters.emplace_back(SuggestionChangedParameter(index++, next));
            });

            return changed_parameters;
        }

        std::random_shuffle(unique.begin() + 1, unique.end(), random);

        struct Params {
            struct kernel {
                // BO_PARAM(double, noise, 0.01); default
                // BO_PARAM(bool, optimize_noise, false); default
                BO_PARAM(double, noise, 0.01);

                BO_PARAM(bool, optimize_noise, false);
            };

            struct kernel_squared_exp_ard : public limbo::defaults::kernel_squared_exp_ard {
                // BO_PARAM(int, k, 0); default
                // BO_PARAM(double, sigma_sq, 1); default

                BO_PARAM(int, k, 4);

                BO_PARAM(double, sigma_sq, 0.2);
            };

            struct opt_rprop : public limbo::defaults::opt_rprop {
            };
            struct opt_nloptnograd : public limbo::defaults::opt_nloptnograd {
            };

            struct acqui_ucb {
                // BO_PARAM(double, alpha, 0.5); default

                BO_PARAM(double, alpha, 0.25);
            };

            struct acqui_gpucb : public limbo::defaults::acqui_gpucb {
            };
            struct acqui_ei : public limbo::defaults::acqui_ei {
            };
        };

        using Kernel2_t = limbo::kernel::SquaredExpARD<Params>;
        using Mean_t = limbo::mean::FunctionARD<Params, limbo::mean::Data<Params>>;
        using GP2_t = limbo::model::GP<Params, Kernel2_t, Mean_t, limbo::model::gp::KernelMeanLFOpt<Params>>;

        GP2_t gp_ard;

        std::vector<Eigen::VectorXd> samples;
        std::vector<Eigen::VectorXd> observations;

        std::for_each(unique.cbegin(), unique.cbegin() + std::min(static_cast<std::size_t>(unique_points_random_count), unique.size()), [this, &samples, &observations]
                (const OptimizationParameterResult &result) {
            Eigen::VectorXd sample(parameters.Size());

            int i = 0;
            std::for_each(result.GetPointsAfter().cbegin(), result.GetPointsAfter().cend(), [&i, &sample, this](const OptimizationParameterPoint &point) {
                auto parameter = parameters.GetParameter(static_cast<size_t>(i));

                double min_bound = parameter.GetMinimalValue();
                double max_bound = parameter.GetMaximumValue();

                double normalized = (parameter.ExtractValueFromPoint(point) - min_bound) / (max_bound - min_bound); // Normalize here to [0, 1]

                sample(i) = normalized;

                i += 1;
            });

            samples.push_back(sample);
            observations.push_back(limbo::tools::make_vector(-1.0 * result.GetMetricsAfter()));
        });

        double min_observation = (*std::min_element(observations.cbegin(), observations.cend(), [](const Eigen::VectorXd &l, const Eigen::VectorXd &r) {
            return l(0) < r(0);
        }))(0);

        double max_observation = (*std::max_element(observations.cbegin(), observations.cend(), [](const Eigen::VectorXd &l, const Eigen::VectorXd &r) {
            return l(0) < r(0);
        }))(0);

        std::transform(observations.cbegin(), observations.cend(), observations.begin(), [min_observation, max_observation](const Eigen::VectorXd &ob) {
            return limbo::tools::make_vector((ob(0) - min_observation) / ((10.0 / 6.0) * (max_observation - min_observation)) + 0.2); // Normalize here to [ 0.2, 0.8 ]
        });

        gp_ard.compute(samples, observations);
        gp_ard.optimize_hyperparams();

        using acquiopt_t = limbo::opt::NLOptNoGrad<Params, nlopt::GN_ISRES>;
        using acquisition_function_t = limbo::acqui::UCB<Params, GP2_t>;

        acquiopt_t             acquiopt;
        acquisition_function_t acqui(gp_ard, 0);

        auto afun               = limbo::FirstElem();
        auto acqui_optimization = [&](const Eigen::VectorXd &x, bool g) { return acqui(x, afun, g); };

        Eigen::VectorXd starting_point(parameters.Size());

        int i = 0;
        std::for_each(parameters.GetParameterEntries().cbegin(), parameters.GetParameterEntries().cend(), [&i, &starting_point](const OptimizationParametersEntry &entry) {

            auto parameter = entry.first;

            double min_bound = parameter.GetMinimalValue();
            double max_bound = parameter.GetMaximumValue();

            starting_point(i) = (entry.second - min_bound) / (max_bound - min_bound);

            i += 1;
        });

        Eigen::VectorXd new_sample = acquiopt(acqui_optimization, starting_point, true);

        for (int k = 0; k < parameters.Size(); ++k) {
            auto parameter = parameters.GetParameter(static_cast<size_t>(k));

            double min_bound = parameter.GetMinimalValue();
            double max_bound = parameter.GetMaximumValue();

            double barrier = max_jump_barrier;
            if (std::abs(new_sample(k) - starting_point(k)) > barrier) {
                new_sample(k) = starting_point(k) + barrier * (new_sample(k) - starting_point(k));
            }

            changed_parameters.emplace_back(SuggestionChangedParameter(k, (new_sample(k) * (max_bound - min_bound)) + min_bound));
        }

        return changed_parameters;
    }

    bool BayesianOptimizer::UpdateSpaceWithLatestResults() {
        const OptimizationParameterResult &last = results.at(0);

        if (last.IsGood() && (last.GetMetricsBefore() < 0.0 || (last.GetMetricsAfter() < last.GetMetricsBefore()))) {
            parameters.Update(last.GetChangedParameters(), last.GetMetricsAfter());
            return true;
        } else {
            return false;
        }
    }

    BayesianOptimizer::~BayesianOptimizer() {}

}


//// DO NOT USE. UNDER CONSTRUCTION.

#include <cstdio>
#include <string>
#include <limits>
#include <set>

#include <boost/algorithm/cxx11/all_of.hpp>
#include <boost/algorithm/cxx11/any_of.hpp>
#include <cctbx/error.h>
// iostream is included by this one
#include <cctbx/sgtbx/direct_space_asu/proto/asymmetric_unit.h>

#include "cctbx/maptbx/skeletons.h"

namespace cctbx { namespace maptbx
{

inline bool any_lt(const int3_t &a, const int3_t &b)
{
  for(short j=0; j<3; ++j)
    if( a[j]<b[j] )
      return true;
  return false;
}

inline bool any_lt(const int3_t &a, int b)
{
  for(short j=0; j<3; ++j)
    if( a[j]<b )
      return true;
  return false;
}

inline bool any_ge(const int3_t &a, const int3_t &b)
{
  for(short j=0; j<3; ++j)
    if( a[j]>=b[j] )
      return true;
  return false;
}

// faster version of small size std::set ?
template<typename T, unsigned short N=1000> class array_as_set :
  private scitbx::af::small<T,N>
{
public:
  typedef scitbx::af::small<T,N> buf_t;
  using buf_t::begin;
  using buf_t::end;
  using buf_t::empty;
  using buf_t::size;
  using typename buf_t::iterator;
  using typename buf_t::const_iterator;
  void insert(const T &v)
  {
    iterator l=std::lower_bound(this->begin(), this->end(),v);
    // *l >= v;
    if( l!=this->end() )
    {
      if( *l != v )
        this->buf_t::insert(l,v);
    }
    else
        this->buf_t::insert(l,v);
    //CCTBX_ASSERT( !this->empty() );
    //CCTBX_ASSERT( std::is_sorted(this->begin(), this->end()) );
    //CCTBX_ASSERT(std::adjacent_find(this->begin(),this->end())==this->end());
  }
};

skeleton swanson(const cctbx::maptbx::asymmetric_map &amap, double sigma)
{
  double mean=0., esd=0., mx=-9.E200;
  const auto &map_data = amap.data().const_ref();
  for(std::size_t ii=0; ii<map_data.size(); ++ii)
  {
    //! @todo discard points outside asu
    double m = map_data[ii];
    mean += m;
    esd += m*m;
    if( m > mx )
      mx = m;
  }
  mean /= map_data.size();
  esd = esd/map_data.size() - mean*mean;
  esd = std::sqrt(esd);
  double mapcutoff = mean + esd * sigma;

  std::vector<xyzm_t> xyzm;
  xyzm.reserve(10000);
  //! @todo code duplication: mmtbx::masks::atom_mask::mask_asu
  std::size_t inside=0, tot=0;
  const auto &opt_asu = amap.optimized_asu();
  const double *md = map_data.begin();
  for(auto i3=amap.grid_begin(); !i3.over(); i3.incr(), ++md)
  {
    int3_t p=i3();
    double m = *md;
    if( m>mapcutoff )
    {
      ++tot;
      if( opt_asu.where_is(p) != 0 ) // inside or on the face
      {
        ++inside;
        xyzm_t x(p,m);
        xyzm.push_back( x );
      }
    }
  }
  std::sort(xyzm.begin(), xyzm.end(),
    [](const xyzm_t &a, const xyzm_t &b) {return (get<1>(a)) > (get<1>(b));}
  );

  CCTBX_ASSERT( get<1>(xyzm.front()) == mx );
  marks_t marks(map_data.accessor(), 0UL); // 0 means no mark
  std::size_t nmarks=0, nbonds=0, min_count = 0, grows_count=0, join_count=0;
  const unsigned short cube_size = 26; // 3*3 + (3*3-1) + (3*3) == 3^3 - 1
  typedef int3_t i3t;
  int3_t cube[cube_size] = {
    i3t(-1,-1,-1),  i3t(0,-1,-1),  i3t(1,-1,-1),  i3t(-1,0,-1),  i3t(0,0,-1),
    i3t(1,0,-1),    i3t(-1,1,-1),  i3t(0,1,-1),   i3t(1,1,-1),   i3t(-1,-1,0),
    i3t(0,-1,0),    i3t(1,-1,0),   i3t(-1,0,0),   i3t(1,0,0),    i3t(-1,1,0),
    i3t(0,1,0),     i3t(1,1,0),    i3t(-1,-1,1),  i3t(0,-1,1),   i3t(1,-1,1),
    i3t(-1,0,1),    i3t(0,0,1),    i3t(1,0,1),    i3t(-1,1,1),   i3t(0,1,1),
    i3t(1,1,1)
  };

  auto ibox_min = amap.box_begin();
  auto ibox_max = amap.box_end();
  skeleton result;
  result.maximums.reserve(1000);
  for(std::size_t jj=0; jj<xyzm.size(); ++jj)
  {
    // typedef std::set<std::size_t> feature_set_t;
    typedef array_as_set<std::size_t> feature_set_t;
    feature_set_t featureset;
    int3_t x = get<0>(xyzm[jj]);
    for(unsigned short ic=0; ic<cube_size; ++ic)
    {
      int3_t neighbor = x + cube[ic]; //! @todo check if in cell ?
      if( any_lt(neighbor,ibox_min) || any_ge(neighbor,ibox_max) )
        continue;
      unsigned mark = marks(neighbor);
      if( mark != 0U ) // 0 is not a mark
        featureset.insert(mark);
    }

    const unsigned fssize = featureset.size();
    if( fssize==0U ) // all_eq(featureset, 0) )
    {
      // 2a: new maximum
      ++nmarks;
      marks(x) = nmarks;
      result.maximums.push_back(x);
    }
    else if( fssize == 1 )
    {
      // 2b: part of the growing nodule
      std::size_t mark = *featureset.begin();
      CCTBX_ASSERT( mark != 0 );
      marks(x) = mark;
      ++grows_count;
    }
    else if( fssize == cube_size )
    {
      // 2d: local minimum will not be in the neighborhood of any point
      ++min_count;
      marks(x) = *featureset.begin(); // highest nodule
    }
    else if( fssize>1 )
    {
      // 2c: two or more nodules merging
      ++join_count;
      marks(x) = *featureset.begin(); // highest nodule
      for(feature_set_t::const_iterator i=featureset.begin();
          i!=featureset.end(); ++i)
      {
        std::size_t fi = *i;
        CCTBX_ASSERT( fi>0U && fi<=nmarks );
        feature_set_t::const_iterator j=i;
        ++j;
        for( ; j!=featureset.end(); ++j)
        {
          std::size_t fj = *j;
          CCTBX_ASSERT( fj>0U && fj<=nmarks );
          CCTBX_ASSERT( fi<fj );
          join_t join;
          join.ilt = fi;
          join.igt = fj;
          result.joins.insert(join); // inserts only new joins
        }
      }
    }
  }
  CCTBX_ASSERT( nmarks + grows_count + join_count + min_count == xyzm.size() );
  result.min_count = min_count;
  result.grows_count = grows_count;
  result.join_count = join_count;
  result.marks = std::move(marks);
  result.mapcutoff = mapcutoff;
  return result;
}

std::vector<std::size_t> find_clusters(const skeleton &skelet)
{
  std::size_t nmaxs = skelet.maximums.size(), mol_id=0;
  std::vector<std::size_t> atoms(nmaxs,0);
  for(joins_t::const_iterator i=skelet.joins.begin(); i!=skelet.joins.end();
    ++i)
  {
    const join_t &join = *i;
    long i_cat = join.ilt;
    long i_an = join.igt;
    CCTBX_ASSERT( i_cat < i_an && i_cat>0 && i_an>0 && i_cat<=nmaxs
      && i_an<=nmaxs );
    if( atoms[i_cat-1U]==0 && atoms[i_an-1U]==0 )
    {
      ++mol_id;
      atoms[i_cat] = mol_id;
      atoms[i_an] = mol_id;
    }
    else
    {
      if(atoms[i_cat]!=0 && atoms[i_an]!=0 && atoms[i_cat]!=atoms[i_an])
      {
        std::size_t imn = std::min(atoms[i_cat],atoms[i_an]);
        atoms[i_cat] = imn;
        atoms[i_an] = imn;
      }
      else
      {
        if( atoms[i_cat]!=0 )
          atoms[i_an] = atoms[i_cat];
        if( atoms[i_an]!=0 )
          atoms[i_cat] = atoms[i_an];
      }
    }
  }
  std::size_t imn = 0, imx=0;
  do
  {
    imx = *std::max_element(atoms.begin(), atoms.end());
    for(joins_t::const_iterator i=skelet.joins.begin(); i!=skelet.joins.end();
      ++i)
    {
      const join_t &join = *i;
      long i_cat = join.ilt;
      long i_an = join.igt;
      if( atoms[i_cat]==0 || atoms[i_an]==0 )
        throw std::logic_error("disaster");
      if( atoms[i_cat] != atoms[i_an] )
      {
        imn = std::min(atoms[i_cat],atoms[i_an]);
        atoms[i_cat] = imn;
        atoms[i_an] = imn;
      }
    }
  }while( imn!=0 );
  imx = *std::max_element(atoms.begin(), atoms.end());

  std::vector< array<std::size_t,2> > molecules;
  molecules.reserve(imx);
  std::size_t imxsz = 0;
  for(std::size_t i=0; i<imx; ++i)
  {
    imn = std::count(atoms.begin(), atoms.end(), i);
    array< std::size_t,2 > m = {imn,0};
    molecules.push_back(m);
    if( imn!=0 )
      ++mol_id;
    if( imn>imxsz )
    {
      imx = i;
      imxsz = imn;
    }
  }
  if( imx!=0 )
  {
    mol_id = 1;
    for(std::size_t i=0; i<molecules.size(); ++i)
    {
      get<1>(molecules[i]) = mol_id;
      ++mol_id;
    }
  }
  std::vector<std::size_t> molids;
  for(std::size_t i=0; i<atoms.size(); ++i)
  {
    if( atoms[i] == 0 )
      continue; // ?
    molids.push_back( get<1>(molecules[atoms[i]]) );
  }
  return molids;
}

}} // cctbx::maptbx


#include "detail/physics.hpp"
#include "detail/fun.hpp"
#include "tools/debug.hpp"

#include "kalman_filter.hpp"
#include "model.hpp"
#include "observation.hpp"
#include "track.hpp"

#include <cmath>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <iostream>
#include <limits>
#include <utility>

#include <Eigen/Dense>

namespace biggles
{

void track::update_boundary_cache()
{
    // if there are no observations in the track, set the bounding box to an extremal value.
    if(observations_.empty())
    {
        boundary_min_location_ = point_2d(std::numeric_limits<float>::max(), std::numeric_limits<float>::max());
        boundary_max_location_ = point_2d(std::numeric_limits<float>::min(), std::numeric_limits<float>::min());
        return;
    }

    // initialise the bounding box to the first location
    const observation& first_obs(*begin());
    boundary_min_location_ = point_2d(x(first_obs), y(first_obs));
    boundary_max_location_ = point_2d(x(first_obs), y(first_obs));

    BOOST_FOREACH(const observation& o, std::make_pair(begin(), end()))
    {
        // check observation fits into birth and death time
        if (not (t(o) < first_time_stamp() + duration())) {
            std::cout << "t(o) =               " << t(o) << std::endl;
            std::cout << "first_time_stamp() = " << first_time_stamp() << std::endl;
            std::cout << "duration() =         " << duration() << std::endl;
        }
        BOOST_ASSERT(t(o) < first_time_stamp() + duration());
        BOOST_ASSERT(t(o) >= first_time_stamp());

        // update minimum bound
        x(boundary_min_location_) = std::min(x(boundary_min_location_), x(o));
        y(boundary_min_location_) = std::min(y(boundary_min_location_), y(o));

        // update maximum bound
        x(boundary_max_location_) = std::max(x(boundary_max_location_), x(o));
        y(boundary_max_location_) = std::max(y(boundary_max_location_), y(o));
    }
}

void track::check_common_timestamps()
{
    if(size() == 0)
        return;

    time_stamp last_ts = t(*begin());

    BOOST_FOREACH(const observation &o, std::make_pair(++begin(), end()))
    {
        if(t(o) == last_ts)
            throw std::runtime_error("track::check_common_timestamps: at least one pair of observations share a common time stamp.");
        last_ts = t(o);
    }
}

float track::log_posterior(const model::parameters& parameters) const
{
    const Eigen::Matrix2f& R(model::observation_error_covariance(parameters));

    if (not is_symmetric(R)) {
        std::cout << measure_asymmetry(R) << std::endl;
        throw std::runtime_error("R is not symmetric");
    }

    if((cached_posterior_.first == R) && (cached_posterior_.second < 0.f)) {
        return cached_posterior_.second;
    }

    float log_pdf = 0.f;

    // create a kalman filter for the track
    boost::shared_ptr<const kalman_filter> p_kf(make_kalman_filter(parameters));
    //kalman_filter::states_and_cov_deque smoothed_states_and_covariances;
    //rts_smooth(*p_kf, std::front_inserter(smoothed_states_and_covariances));

    // early out if no states
    if(p_kf->predictions().empty()) {
        return log_pdf;
    }

    // the observation matrix
    Eigen::Matrix<float, 2, 4> B;
    B << 1, 0, 0, 0,
         0, 0, 1, 0;

    // iterate over all state predictions
    time_stamp time_stamp(first_time_stamp());
    track::const_iterator track_obs_it(begin());
    const float normalisation(-logf(2.f * M_PI));
    BOOST_FOREACH(const kalman_filter::state_covariance_pair& state_and_cov, p_kf->predictions())
    {
        BOOST_ASSERT(time_stamp < last_time_stamp());

        // do we have an observation at this time stamp?
        if((track_obs_it != end()) && (t(*track_obs_it) == time_stamp))
        {
            // ... yes

            // find associated observation
            const observation& o(*track_obs_it);
            ++track_obs_it;

            /*
            if (state_and_cov.second.determinant()<=0) {
                std::cerr << "................" << std::endl;
                OK(size());
                OK(duration());
            }
            */

            // estimate covariance of pdf
            Eigen::Matrix2f covariance = B * state_and_cov.second * B.transpose() + R;

            // calculate predicted observation
            Eigen::Vector2f predicted_obs = B * state_and_cov.first;

            // actual observation
            Eigen::Vector2f obs;
            obs[0] = x(o);
            obs[1] = y(o);

            // this is just a multivariate Gaussian log pdf
            Eigen::Vector2f delta = obs - predicted_obs;
            float a = delta.transpose() * covariance.inverse() * delta;
            BOOST_ASSERT(std::isfinite(a));
            log_pdf += -0.5f * a;
            if (not (covariance.determinant()>0)) {
                std::cerr << "====================" << std::endl;
                OK(covariance);
                OK(R);
                OK(state_and_cov.second);
                OK(state_and_cov.second.determinant());
            }
            //BOOST_ASSERT(covariance.determinant()>0);
            log_pdf += -0.5f * logf(covariance.determinant());
            log_pdf += normalisation;
        }

        ++time_stamp;
        //std::cout << boost::format("  ** %.7f") % log_pdf << std::endl;
    }
    BOOST_ASSERT(track_obs_it == end());

    // exponential distribution over maximum offset
    //float lambda = 1.f / model::constraint_radius(parameters);
    //BOOST_ASSERT(std::isfinite(lambda));
    BOOST_ASSERT(std::isfinite(radius()));
    //log_pdf += logf(lambda) - lambda * radius();

    cached_posterior_ = std::make_pair(R, log_pdf);
    //std::cout << boost::format("  ** %.7f") % log_pdf << std::endl;
    return log_pdf;
}

bool verify_track(const track& track)
{
    observation_collection::const_iterator previous(track.observations().begin());

    if(track.observations().end() == previous)
    {
        std::cerr << "track has no observations.\n";
        return false;
    }

    BOOST_FOREACH(const observation& obs,
                  std::make_pair(++(track.observations().begin()), track.observations().end()))
    {
        if(t(obs) <= t(*previous))
        {
            std::cerr << "track has non monotonic time stamps.\n";
            return false;
        }

        time_stamp delta_t = t(obs) - t(*previous);
        BOOST_ASSERT(delta_t > 0);
        /*
        if(delta_t >= detail::last_delta_t_)
        {
            std::cerr << "track has too large a gap in time (" << delta_t << ").\n";
            return false;
        }
        */

        if(not observations_see_each_other(obs, *previous, detail::speed_of_light_))
        {
            float dx(x(*previous) - x(obs));
            float dy(y(*previous) - y(obs));
            float ds_sq = dx*dx + dy*dy;
            std::cerr << "track's displacement is too large (" << sqrt(ds_sq) << ") for time lag " << delta_t << ".\n";
            std::cerr << "Time points: " << t(*previous) << ", " << t(obs) << std::endl;
            time_stamp ts=track.first_time_stamp();
            std::cerr << boost::format("duration: [%d, %d]") % ts % track.last_time_stamp() << std::endl;
            std::cerr << "num obs =" << track.size() << std::endl;
            std::cerr << boost::format("obs range: [%d, %d]")
                % track.observations().first_time_stamp()
                % track.observations().last_time_stamp()
                << std::endl;
            for (; ts < track.last_time_stamp(); ++ts) {
                observation_collection::const_range obs_at_t(track.observations().at_time_stamp(ts));
                if (obs_at_t.first != obs_at_t.second) {
                    while (obs_at_t.first != obs_at_t.second) {
                        observation obs = *obs_at_t.first++;
                        std::cout << boost::format("[%f, %f, %d]") % x(obs) % y(obs) % t(obs) << std::endl;
                    }
                }
                else {
                    std::cout << boost::format("[-, -, %d]") % ts << std::endl;
                }
            }
            return false;
        }

        ++previous;
    }

    return true;
}

float track::radius() const
{
    if(empty())
        return 0.f;

    const point_2d& min(min_location());
    const point_2d& max(max_location());
    float dx = x(min) - x(max);
    float dy = y(min) - y(max);
    return sqrtf(dx*dx + dy*dy);
}

float dist(const track& t1, const track& t2) {
    float result = 0.f;
    if ( x(t1.min_location()) > x(t2.max_location()) ) result += square(x(t1.min_location()) - x(t2.max_location()));
    if ( x(t2.min_location()) > x(t1.max_location()) ) result += square(x(t2.min_location()) - x(t1.max_location()));
    if ( y(t1.min_location()) > y(t2.max_location()) ) result += square(y(t1.min_location()) - y(t2.max_location()));
    if ( y(t2.min_location()) > y(t1.max_location()) ) result += square(y(t2.min_location()) - y(t1.max_location()));
    return sqrtf(result);
}

boost::shared_ptr<const kalman_filter> track::make_kalman_filter(const model::parameters& p) const
{
    return boost::shared_ptr<kalman_filter>(new kalman_filter(*this,
        model::observation_error_covariance(p), model::process_noise_covariance(p)) );
}

}


#include "terms/Tensor.hpp"
#include "terms/IndexSpaceMeta.hpp"
#include "terms/IndexSubstitution.hpp"
#include "utils/IndexSpaceResolver.hpp"

#include <algorithm>
#include <cassert>

#include <boost/range/join.hpp>

namespace Contractor::Terms {

void Tensor::transferSymmetry(const Tensor &source, Tensor &destination) {
	// If these tensors don't refer to the same element, transferring the symmetry does not make
	// a whole lot of sense
	assert(source.refersToSameElement(destination, false));
	assert(source.getIndices().size() == destination.getIndices().size());

	const IndexSubstitution mapping = source.getIndexMapping(destination);

	PermutationGroup symmetry(destination.getIndices());
	for (const IndexSubstitution &currentSymOp : source.getSymmetry().getGenerators()) {
		IndexSubstitution copy = currentSymOp;
		mapping.apply(copy);

		symmetry.addGenerator(std::move(copy), false);
	}

	symmetry.regenerateGroup();

	destination.setSymmetry(std::move(symmetry));
}

Tensor::Tensor(const std::string_view name, const Tensor::index_list_t &indices, const PermutationGroup &symmetry)
	: m_indices(indices), m_name(name), m_symmetry(symmetry) {
	m_symmetry.setRootSequence(getIndices());
	sortIndices();
}

Tensor::Tensor(const std::string_view name, Tensor::index_list_t &&indices, PermutationGroup &&symmetry)
	: m_indices(indices), m_name(name), m_symmetry(std::move(symmetry)) {
	m_symmetry.setRootSequence(getIndices());
	sortIndices();
}

bool operator==(const Tensor &lhs, const Tensor &rhs) {
	return lhs.m_name == rhs.m_name && lhs.m_symmetry == rhs.m_symmetry
		   && lhs.m_symmetry.getCanonicalRepresentation() == rhs.m_symmetry.getCanonicalRepresentation()
		   && lhs.m_symmetry.getCanonicalRepresentationFactor() == rhs.m_symmetry.getCanonicalRepresentationFactor();
}

bool operator!=(const Tensor &lhs, const Tensor &rhs) {
	return !(lhs == rhs);
}

bool operator<(const Tensor &lhs, const Tensor &rhs) {
	if (lhs.getName() != rhs.getName()) {
		return lhs.getName() < rhs.getName();
	}

	if (lhs.getIndices().size() != rhs.getIndices().size()) {
		return lhs.getIndices().size() < rhs.getIndices().size();
	}

	if (lhs.getIndices() != rhs.getIndices()) {
		return lhs.getIndices() < rhs.getIndices();
	}

	return lhs.getSymmetry().size() < rhs.getSymmetry().size();
}

std::ostream &operator<<(std::ostream &out, const Tensor &element) {
	out << element.m_name << "[";
	for (std::size_t i = 0; i < element.m_indices.size(); i++) {
		out << element.m_indices[i];
		if (i + 1 < element.m_indices.size()) {
			out << ",";
		}
	}

	out << "] " << element.m_symmetry;

	return out;
}

const Tensor::index_list_t &Tensor::getIndices() const {
	return m_indices;
}

Tensor::index_list_t &Tensor::getIndices() {
	return m_indices;
}

const std::string_view Tensor::getName() const {
	return m_name;
}

void Tensor::setName(const std::string_view &name) {
	m_name = name;
}

const PermutationGroup &Tensor::getSymmetry() const {
	return m_symmetry;
}

PermutationGroup &Tensor::accessSymmetry() {
	return m_symmetry;
}

void Tensor::setSymmetry(const PermutationGroup &symmetry) {
	assert(symmetry.contains(m_indices));

	m_symmetry = symmetry;
}

int Tensor::getS() const {
	return m_S;
}

void Tensor::setS(int S) {
	m_S = S;
}

bool Tensor::hasS() const {
	return m_S != std::numeric_limits< int >::max();
}

int Tensor::getDoubleMs() const {
	return m_doubleMs;
}

void Tensor::setDoubleMs(int doubleMs) {
	m_doubleMs = doubleMs;
}

std::vector< IndexSubstitution > createAntisymmetricExchanges(const Tensor::index_list_t &indices,
															  Index::Type indexType) {
	std::vector< IndexSubstitution > substitutions;

	// Assert that the indices are sorted into the groups creator, annihilator, other
	auto begin = std::find_if(indices.begin(), indices.end(),
							  [indexType](const Index &current) { return current.getType() == indexType; });
	auto end   = std::find_if(begin, indices.end(),
                            [indexType](const Index &current) { return current.getType() != indexType; });

	// Create all pairings
	for (auto i = begin; i != end; ++i) {
		for (auto j = i + 1; j != end; ++j) {
			substitutions.push_back(IndexSubstitution::createPermutation({ { *i, *j } }, -1));
		}
	}

	return substitutions;
}

bool Tensor::isAntisymmetrized() const {
	std::vector< IndexSubstitution > creatorExchanges = createAntisymmetricExchanges(m_indices, Index::Type::Creator);
	std::vector< IndexSubstitution > annihilatorExchanges =
		createAntisymmetricExchanges(m_indices, Index::Type::Annihilator);

	auto joined = boost::join(creatorExchanges, annihilatorExchanges);
	for (const IndexSubstitution &current : joined) {
		if (!m_symmetry.contains(current)) {
			return false;
		}
	}

	return true;
}

bool Tensor::isPartiallyAntisymmetrized() const {
	std::vector< IndexSubstitution > creatorExchanges = createAntisymmetricExchanges(m_indices, Index::Type::Creator);

	bool isPartiallyAntisymmetric = !creatorExchanges.empty();

	for (const IndexSubstitution &current : creatorExchanges) {
		if (!m_symmetry.contains(current)) {
			isPartiallyAntisymmetric = false;
			break;
		}
	}

	if (isPartiallyAntisymmetric) {
		return true;
	}

	std::vector< IndexSubstitution > annihilatorExchanges =
		createAntisymmetricExchanges(m_indices, Index::Type::Annihilator);

	for (const IndexSubstitution &current : annihilatorExchanges) {
		if (!m_symmetry.contains(current)) {
			return false;
		}
	}

	return !annihilatorExchanges.empty() || (annihilatorExchanges.empty() && creatorExchanges.empty());
}

std::vector< IndexSubstitution > createSymmetricExchanges(const Tensor::index_list_t &indices) {
	std::vector< IndexSubstitution > substitutions;

	// Assert that the indices are sorted into the groups creator, annihilator, other
	auto creatorBegin     = std::find_if(indices.begin(), indices.end(),
                                     [](const Index &current) { return current.getType() == Index::Type::Creator; });
	auto creatorEnd       = std::find_if(creatorBegin, indices.end(),
                                   [](const Index &current) { return current.getType() != Index::Type::Creator; });
	auto annihilatorBegin = std::find_if(
		creatorEnd, indices.end(), [](const Index &current) { return current.getType() == Index::Type::Annihilator; });
	auto annihilatorEnd = std::find_if(annihilatorBegin, indices.end(), [](const Index &current) {
		return current.getType() != Index::Type::Annihilator;
	});

	assert(std::distance(creatorBegin, creatorEnd) == std::distance(annihilatorBegin, annihilatorEnd));

	// Create all column-wise substitutions
	std::size_t amount = std::distance(creatorBegin, creatorEnd);
	for (std::size_t i = 0; i < amount; ++i) {
		for (std::size_t j = i + 1; j < amount; ++j) {
			substitutions.push_back(
				IndexSubstitution::createPermutation({ { *(creatorBegin + i), *(creatorBegin + j) },
													   { *(annihilatorBegin + i), *(annihilatorBegin + j) } }));
		}
	}

	return substitutions;
}

bool Tensor::hasColumnSymmetry() const {
	std::vector< IndexSubstitution > columnWiseExchanges = createSymmetricExchanges(m_indices);

	for (const IndexSubstitution &current : columnWiseExchanges) {
		if (!getSymmetry().contains(current)) {
			return false;
		}
	}

	return true;
}

bool Tensor::hasPartialColumnSymmetry() const {
	std::vector< IndexSubstitution > columnWiseExchanges = createSymmetricExchanges(m_indices);

	for (const IndexSubstitution &current : columnWiseExchanges) {
		if (getSymmetry().contains(current)) {
			return true;
		}
	}

	return false;
}

bool Tensor::refersToSameElement(const Tensor &other, bool accountForSymmetry) const {
	if (m_indices.size() != other.m_indices.size() || getName() != other.getName()) {
		return false;
	}
	if (accountForSymmetry && (m_symmetry.size() != other.getSymmetry().size())) {
		// We don't compare the symmetry itself, since as of now the symmetry compares indices "name-sensitively" thus i
		// and j are always considered to be different, regardless of context
		return false;
	}

	const Tensor::index_list_t &otherIndices =
		accountForSymmetry ? other.getSymmetry().getCanonicalRepresentation() : other.getIndices();

	return refersToSameIndexSequence(otherIndices, accountForSymmetry);
}

bool Tensor::refersToSameIndexSequence(const Tensor::index_list_t &otherIndices, bool accountForSymmetry) const {
	const Tensor::index_list_t &ownIndices = accountForSymmetry ? m_symmetry.getCanonicalRepresentation() : m_indices;

	// Assume index order in both tensors to be compatible
	for (std::size_t i = 0; i < ownIndices.size(); i++) {
		if (ownIndices[i].getType() != otherIndices[i].getType()) {
			return false;
		}
		if (ownIndices[i].getSpace() != otherIndices[i].getSpace()) {
			return false;
		}
		if (ownIndices[i].getSpin() != otherIndices[i].getSpin()) {
			return false;
		}
		// The exact ID does not matter here as that is only a matter of naming convention
		// However if we end up having equal indices in a single tensor, then there must
		// also be equal indices in the other tensor
		auto it             = std::find(ownIndices.begin() + i + 1, ownIndices.end(), ownIndices[i]);
		bool foundDuplicate = it != ownIndices.end();
		while (it != ownIndices.end()) {
			std::size_t duplicateIndex = std::distance(ownIndices.begin(), it);

			if (otherIndices[i] != otherIndices[duplicateIndex]) {
				return false;
			}

			it = std::find(ownIndices.begin() + duplicateIndex + 1, ownIndices.end(), ownIndices[i]);
		}

		if (!foundDuplicate) {
			// Check if the otherIndices has a duplicate of the current index. If it does, the
			// index structure of the two tensors is incompatible since this tensor does not have
			// a duplicate at this position
			auto otherIt = std::find(otherIndices.begin() + i + 1, otherIndices.end(), otherIndices[i]);

			if (otherIt != otherIndices.end()) {
				return false;
			}
		}
	}

	return true;
}

IndexSubstitution Tensor::getIndexMapping(const Tensor &other) const {
	IndexSubstitution::substitution_list mapping;

	// If both Tensors refer to the same element, then the index mapping is a simple positional one.
	// Thus the only thing that needs to be taken care of is to not duplicate an index mapping.
	for (std::size_t i = 0; i < m_indices.size(); i++) {
		IndexSubstitution::index_pair_t currentPair(m_indices[i], other.m_indices[i]);

		if (std::find(mapping.begin(), mapping.end(), currentPair) == mapping.end()) {
			mapping.push_back(std::move(currentPair));
		}
	}

	return IndexSubstitution(std::move(mapping), 1);
}

ContractionResult Tensor::contract(const Tensor &other, const Utils::IndexSpaceResolver &resolver) const {
	ContractionResult result;
	result.cost = 1;

	std::vector< Index > contractedIndices;
	Tensor::index_list_t resultIndices;

	for (const Index &currentIndex : m_indices) {
		auto it = std::find_if(other.m_indices.begin(), other.m_indices.end(),
							   [currentIndex](const Index &other) { return Index::isSame(currentIndex, other); });

		if (it != other.m_indices.end()) {
			// The tensors share an index
			unsigned int currentCost = resolver.getMeta(currentIndex.getSpace()).getSize();

			result.spaceExponents[currentIndex.getSpace()] += 1;

			// Costs are multiplicative
			result.cost *= currentCost;

			contractedIndices.push_back(currentIndex);
		} else {
			resultIndices.push_back(currentIndex);
		}
	}

	// Gather result indices from the other Tensor
	if (contractedIndices.size() != other.m_indices.size()) {
		for (const Index &currentIndex : other.m_indices) {
			auto it = std::find_if(contractedIndices.begin(), contractedIndices.end(),
								   [currentIndex](const Index &other) { return Index::isSame(currentIndex, other); });

			if (it == contractedIndices.end()) {
				// This index has not been contracted
				resultIndices.push_back(currentIndex);
			}
		}
	}

	// Auto-generate the name in such a way that it will result in the same name regardless of the order
	// of the contraction. In order to achieve that, we sort the name parts alphabetically.
	std::string resultName;
	if (getName().compare(other.getName()) > 0) {
		resultName.append(other.getName());
		resultName.append("_");
		resultName.append(getName());
	} else {
		resultName.append(getName());
		resultName.append("_");
		resultName.append(other.getName());
	}

	// Until here we have computed the cost of evaluating a single element in the
	// result Tensor. However for the total operation cost we now also have to figure
	// out how expensive it is to compute all entries in the result tensor
	for (const Index &currentResultIndex : resultIndices) {
		result.spaceExponents[currentResultIndex.getSpace()] += 1;

		result.cost *= resolver.getMeta(currentResultIndex.getSpace()).getSize();
	}

	result.resultTensor = Tensor(resultName, std::move(resultIndices));

	// As a final step we want to figure out whether any of the index symmetries from the original Tensors still apply
	// to the result Tensor.
	// Note that the indices in the result are the same as in the Tensors it consists of. There is also no ambiguity for
	// where each index came from since indices that occur in both Tensors are being contracted and thus no longer
	// show in the result Tensor.
	PermutationGroup resultSymmetry(result.resultTensor.getIndices());
	for (const IndexSubstitution &currentSymmetry :
		 boost::join(m_symmetry.getGenerators(), other.getSymmetry().getGenerators())) {
		if (currentSymmetry.appliesTo(result.resultTensor)) {
			resultSymmetry.addGenerator(currentSymmetry, false);
		}
	}

	resultSymmetry.regenerateGroup();

	result.resultTensor.setSymmetry(std::move(resultSymmetry));

	return result;
}

bool canonical_index_less(const Index &lhs, const Index &rhs) {
	return lhs.getType() < rhs.getType();
}

void Tensor::sortIndices() {
	std::stable_sort(m_indices.begin(), m_indices.end(), canonical_index_less);

	m_symmetry.setRootSequence(m_indices);
}

bool Tensor::hasCanonicalIndexSequence() const {
	return m_indices == m_symmetry.getCanonicalRepresentation();
}

int Tensor::canonicalizeIndices() {
	if (hasCanonicalIndexSequence()) {
		return 1;
	}

	m_indices = m_symmetry.getCanonicalRepresentation();

	int factor = m_symmetry.getCanonicalRepresentationFactor();

	m_symmetry.setRootSequence(m_indices);

	return factor;
}

}; // namespace Contractor::Terms


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

#include <adobe/algorithm/median.hpp>

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(median_test) {
    using adobe::median;

    {
        int a = 1, b = 2, c = 3;
        BOOST_CHECK_EQUAL(&median(a, b, c), &b);
        BOOST_CHECK_EQUAL(&median(a, c, b), &b);
        BOOST_CHECK_EQUAL(&median(b, a, c), &b);
        BOOST_CHECK_EQUAL(&median(b, c, a), &b);
        BOOST_CHECK_EQUAL(&median(c, a, b), &b);
        BOOST_CHECK_EQUAL(&median(c, b, a), &b);
    }
    {
        int a = 1, b = 1, c = 3;
        BOOST_CHECK_EQUAL(&median(a, b, c), &b);
        BOOST_CHECK_EQUAL(&median(a, c, b), &b);
        BOOST_CHECK_EQUAL(&median(c, a, b), &b);
    }
    {
        int a = 1, b = 3, c = 3;
        BOOST_CHECK_EQUAL(&median(a, b, c), &b);
        BOOST_CHECK_EQUAL(&median(b, a, c), &b);
        BOOST_CHECK_EQUAL(&median(b, c, a), &b);
    }
    {
        int a = 1, b = 1, c = 1;
        BOOST_CHECK_EQUAL(&median(a, b, c), &b);
    }

    {
        int a = 1, c = 3;
        const int b = 2;
        BOOST_CHECK_EQUAL(&median(a, b, c), &b);
        BOOST_CHECK_EQUAL(&median(a, c, b), &b);
        BOOST_CHECK_EQUAL(&median(b, a, c), &b);
        BOOST_CHECK_EQUAL(&median(b, c, a), &b);
        BOOST_CHECK_EQUAL(&median(c, a, b), &b);
        BOOST_CHECK_EQUAL(&median(c, b, a), &b);
    }
}


#ifndef MESH_STORAGE_HPP__
#define MESH_STORAGE_HPP__

#include <vector>
#include <Eigen/Dense>
using namespace Eigen;

namespace polyfem
{
	struct Vertex
	{
		int id;
		std::vector<double> v;
		std::vector<uint32_t> neighbor_vs;
		std::vector<uint32_t> neighbor_es;
		std::vector<uint32_t> neighbor_fs;
		std::vector<uint32_t> neighbor_hs;

		bool boundary;
		bool boundary_hex;
	};
	struct Edge
	{
		int id;
		std::vector<uint32_t> vs;
		std::vector<uint32_t> neighbor_fs;
		std::vector<uint32_t> neighbor_hs;

		bool boundary;
		bool boundary_hex;
	};
	struct Face
	{
		int id;
		std::vector<uint32_t> vs;
		std::vector<uint32_t> es;
		std::vector<uint32_t> neighbor_hs;
		bool boundary;
		bool boundary_hex;
	};

	struct Element
	{
		int id;
		std::vector<uint32_t> vs;
		std::vector<uint32_t> es;
		std::vector<uint32_t> fs;
		std::vector<bool> fs_flag;
		bool hex = false;
		std::vector<double> v_in_Kernel;
	};

	enum MeshType
	{
		Tri = 0,
		Qua,
		HSur,
		Tet,
		Hyb,
		Hex
	};

	struct Mesh3DStorage
	{
		MeshType type;
		Eigen::MatrixXd points;
		std::vector<Vertex> vertices;
		std::vector<Edge> edges;
		std::vector<Face> faces;
		std::vector<Element> elements;

		Eigen::MatrixXi EV;//EV(2, ne)
		Eigen::MatrixXi FV, FE, FH, FHi;//FV (3, nf), FE(3, nf), FH (2, nf), FHi(2, nf)
		Eigen::MatrixXi HV, HF;//HV(4, nh), HE(6, nh), HF(4, nh)
	};

	struct Mesh_Quality
	{
		std::string Name;
		double min_Jacobian;
		double ave_Jacobian;
		double deviation_Jacobian;
		VectorXd V_Js;
		VectorXd H_Js;
		VectorXd Num_Js;
		int32_t V_num, H_num;
	};
}

#endif


//
// Created by wei on 10/10/18.
//

#include <Open3D/Open3D.h>
#include <Cuda/Integration/UniformTSDFVolumeCuda.h>
#include <Cuda/Integration/UniformMeshVolumeCuda.h>
#include <Cuda/Common/LinearAlgebraCuda.h>

#include <Eigen/Eigen>

#include <opencv2/opencv.hpp>
#include <vector>
#include <gtest/gtest.h>

using namespace open3d;
using namespace open3d::cuda;
using namespace open3d::utility;
using namespace open3d::geometry;

TEST(UniformTSDFVolumeCuda, UploadAndDownload) {
    //const size_t N = 512;
    const size_t N = 16;
    const size_t NNN = N * N * N;
    std::vector<float> tsdf;
    std::vector<uchar> weight;
    std::vector<Vector3b> color;
    tsdf.resize(NNN);
    weight.resize(NNN);
    color.resize(NNN);

    UniformTSDFVolumeCuda volume;
    volume.Create(N);

    int cnt = 0;
    for (int i = 0; i < N; ++i) {
        for (int j = 0; j < N; ++j) {
            for (int k = 0; k < N; ++k) {
                tsdf[cnt] = i * j * k;
                weight[cnt] = uchar(fminf(i, 255));
                color[cnt] = Vector3b(i, j, k);
                ++cnt;
            }
        }
    }

    volume.UploadVolume(tsdf, weight, color);
    tsdf.clear();
    weight.clear();
    color.clear();

    auto downloaded = volume.DownloadVolume();
    tsdf = std::get<0>(downloaded);
    weight = std::get<1>(downloaded);
    color = std::get<2>(downloaded);

    cnt = 0;
    for (int i = 0; i < N; ++i) {
        for (int j = 0; j < N; ++j) {
            for (int k = 0; k < N; ++k) {
                EXPECT_NEAR(tsdf[cnt], i * j * k, NNN * 1e-6);
                EXPECT_EQ(weight[cnt], uchar(fminf(i, 255)));
                EXPECT_EQ(color[cnt], Vector3b(i, j, k));
                ++cnt;
            }
        }
    }
}

TEST(UniformTSDFVolumeCuda, RayCasting) {
    using namespace open3d;
    Image depth, color;
    io::ReadImage("../../../examples/TestData/RGBD/depth/00000.png", depth);
    io::ReadImage("../../../examples/TestData/RGBD/color/00000.jpg", color);
    RGBDImageCuda rgbd;
    rgbd.Upload(depth, color);

    PinholeCameraIntrinsicCuda intrinsics(
        camera::PinholeCameraIntrinsicParameters::PrimeSenseDefault);

    TransformCuda transform = TransformCuda::Identity();

    const float voxel_length = 0.01f;
    transform.SetTranslation(Vector3f(-voxel_length * 256));
    UniformTSDFVolumeCuda volume(512, voxel_length, voxel_length * 3, transform);

    TransformCuda extrinsics = TransformCuda::Identity();
    for (int i = 0; i < 10; ++i) {
        volume.Integrate(rgbd, intrinsics, extrinsics);
    }

    ImageCuda<float, 3> raycaster;
    raycaster.Create(depth.width_, depth.height_);

    Timer timer;
    int iteration_times = 100;
    timer.Start();
    for (int i = 0; i < iteration_times; ++i) {
        volume.RayCasting(raycaster, intrinsics, extrinsics);
    }
    timer.Stop();
    LogInfo("Average raycasting time: %f milliseconds\n",
              timer.GetDuration() / iteration_times);

    cv::Mat imraycaster = raycaster.DownloadMat();
    cv::imshow("im", imraycaster);
    cv::waitKey(-1);
}

int main(int argc, char **argv) {
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}


/*
Copyright (c) 2020 Hirokazu Ishida
This software is released under the MIT License, see LICENSE.
tinyfk: https://github.com/HiroIshida/tinyfk
*/

#ifndef tinyfk_hpp
#define tinyfk_hpp

#include "urdf_parser/urdf_parser.h"
#include "urdf_model/pose.h"
#include "urdf_model/joint.h"
#include <iostream>
#include <fstream>
#include <stdexcept>
#include <Eigen/Core> // slow compile...
#include <array>
#include <stack>
#include <unordered_map>
#include <assert.h>

namespace tinyfk
{

  using MatrixXdC = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
  using MatrixXdR = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

  // TODO templatize
  // an util data structure to handle matrix.
  struct TinyMatrix // coll major (same as eigen)
  {
    double* _data; // beginning of the block matrix
    int _i_begin;
    int _j_begin;

    int _n_block;
    int _m_block;

    int _n_whole;
    int _m_whole;

    TinyMatrix(Eigen::MatrixXd& mat, int i_begin, int j_begin, int n, int m) :
      _data(mat.data()),
      _i_begin(i_begin), _j_begin(j_begin),
      _n_block(n), _m_block(m),
      _n_whole(mat.rows()), _m_whole(mat.cols()) {}

    TinyMatrix(Eigen::MatrixXd& mat) :
      _data(mat.data()),
      _i_begin(0), _j_begin(0),
      _n_block(mat.rows()), _m_block(mat.cols()),
      _n_whole(_n_block), _m_whole(_m_block) {}

    TinyMatrix(double* data, int i_begin, int j_begin, int n, int m, int n_whole, int m_whole) :
      _data(data), _i_begin(i_begin), _j_begin(j_begin), _n_block(n), _m_block(m),
      _n_whole(n_whole), _m_whole(m_whole) {}

    inline int rows(){
      return _n_block;
    }

    inline int cols(){
      return _m_block;
    }

    inline int get_idx(int i, int j){
      assert(i<_n_whole && "out of index");
      assert(j<_m_whole && "out of index");

      int idx = _n_whole * (j+_j_begin) + (i+_i_begin);
      return idx;
    }

    TinyMatrix block(int i, int j, int n, int m){
      TinyMatrix mat = {_data, _i_begin+i, _j_begin+j, n, m, _n_whole, _m_whole};
      return mat;
    }

    TinyMatrix slice(int i){ // we consider matrix is coll major.
      return this->block(0, i, _n_block, 1);
    }

    double& operator() (int i, int j){
      return _data[this->get_idx(i, j)];
    }

    double& operator[] (int i){ // access to sliced matrix
      return _data[this->get_idx(i, 0)];
    }
  };

  struct TransformCache
  {
    int _N_link;
    std::vector<urdf::Pose> _data;
    std::vector<bool> _isCachedVec;

    TransformCache(){}
    TransformCache(unsigned int N_link) :
      _N_link(N_link), _data(std::vector<urdf::Pose>(N_link)),
      _isCachedVec(std::vector<bool>(N_link, false)) {}

    void set_cache(unsigned int link_id, const urdf::Pose& tf){
      assert(!_isCachedVec[link_id] && "attempt to break an existing cache");
      _isCachedVec[link_id] = true;
      _data[link_id] = tf;
    }

    urdf::Pose* get_cache(unsigned int link_id){
      bool isAlreadyCached = (_isCachedVec[link_id] == true);
      if(!isAlreadyCached){return nullptr;} //the cache does not exists
      return &_data[link_id];
    }

    void extend(){
      _N_link++;
      _data.push_back(urdf::Pose());
      _isCachedVec.push_back(false);
      this->clear();
    }

    void clear(){// performance critical
      // bool's default value is false.
      _isCachedVec = std::vector<bool>(_N_link);
    }
  };

  struct NastyStack
  {
    std::vector<urdf::Pose> _tf_stack;
    std::vector<unsigned int> _hid_stack; // here id stack
    NastyStack(){};
    NastyStack(unsigned int N_link) : _tf_stack(std::vector<urdf::Pose>(N_link)),
    _hid_stack(std::vector<unsigned int>(N_link)) {}
  };

  struct RelevancePredicateTable
  {
    std::vector<std::vector<bool>> _table;
    RelevancePredicateTable(){};
    RelevancePredicateTable(int N_link, int N_joint){
      for(int i=0; i<N_joint; i++){
        _table.push_back(std::vector<bool>(N_link));
      }
    }
    bool isRelevant(int joint_id, int link_id) const{
      return _table[joint_id][link_id];
    }
  };

  struct BasePose
  {
    std::array<double, 3> _pose3d;
    urdf::Pose _pose;
    void set(double x, double y, double theta){
      _pose.position = urdf::Vector3(x, y, 0.0);
      _pose.rotation = urdf::Rotation(0.0, 0.0, 1.0 * sin(0.5 * theta), cos(0.5 * theta));
      _pose3d[0] = x; _pose3d[1] = y; _pose3d[2] = theta;
    }
  };

  class RobotModel
  {
    public: //members
      // change them all to private later
      urdf::ModelInterfaceSharedPtr _robot_urdf_interface;

      urdf::LinkSharedPtr _root_link;
      std::vector<urdf::LinkSharedPtr> _links;
      std::unordered_map<std::string, int> _link_ids;

      std::vector<urdf::JointSharedPtr> _joints;
      std::unordered_map<std::string, int> _joint_ids;
      std::vector<double> _joint_angles;
      RelevancePredicateTable _rptable;
      BasePose _base_pose;
      int _num_dof;

      mutable NastyStack _nasty_stack; // TODO add constructor??
      mutable TransformCache _tf_cache;

    public: //functions
      RobotModel(const std::string& xml_string);
      RobotModel(){}

      void get_link_point(
          unsigned int link_id, urdf::Pose& out_tf_root_to_ef, bool basealso) const;

      // naive jacobian computation with finite differentiation (just for testing)
      // as in the finite differentiatoin, set_joint_angle is called, this function cannot be
      // const-nized
      Eigen::MatrixXd get_jacobian_naive(
          unsigned int elink_id, const std::vector<unsigned int>& joint_ids,
          bool rotalso = false, bool basealso = false
          );

      void set_joint_angles(// this will clear all the cache stored
          const std::vector<unsigned int>& joint_ids, const std::vector<double>& joint_angles);
      void _set_joint_angles(// lower version of the set_joint_angle which does not clear cache
          const std::vector<unsigned int>& joint_ids, const std::vector<double>& joint_angles);

      void set_base_pose(double x, double y, double theta);
      void _set_base_pose(double x, double y, double theta);

      void clear_cache();

      void set_init_angles();

      std::vector<double> get_joint_angles(const std::vector<unsigned int>& joint_ids) const;
      std::vector<unsigned int> get_joint_ids(std::vector<std::string> joint_names) const;
      std::vector<unsigned int> get_link_ids(std::vector<std::string> link_names) const;

      // private (I wanna make these function private, but
      // don't know who to do unit test after that
      // anyway, don't use it
      void set_joint_angle(unsigned int joint_id, double angle){
        _joint_angles[joint_id] = angle;
      }

      // perfromance of returning array of eigen is actually almost same as pass by reference
      // thanks to the compiler's optimization
      std::array<Eigen::MatrixXd, 2> get_jacobians_withcache(
          const std::vector<unsigned int>& elink_ids,
          const std::vector<unsigned int>& joint_ids,
          bool rpyalso = false, // only point jacobian is computed by default
          bool basealso = false
          ) const;

      void _solve_forward_kinematics(
          int elink_id, const std::vector<unsigned int>& joint_ids,
          bool with_rot, bool with_base, TinyMatrix& pose_arr, TinyMatrix& jacobian) const;

      void _solve_batch_forward_kinematics(
          std::vector<unsigned int> elink_ids, const std::vector<unsigned int>& joint_ids,
          bool with_rot, bool with_base, TinyMatrix& pose_arr, TinyMatrix& jacobian_arr) const;

      void get_link_point_withcache(
          unsigned int link_id, urdf::Pose& out_tf_root_to_ef,
          bool usebase) const;

      void _get_link_point_creating_cache(
          unsigned int link_id, urdf::Pose& out_tf_root_to_ef,
          bool usebase) const;

      void add_new_link(
          std::string link_name,
          unsigned int parent_id,
          std::array<double, 3> position){

        bool link_name_exists = (_link_ids.find(link_name) != _link_ids.end());
        if(link_name_exists){
          std::string message = "link name " + link_name + " already exists";
          throw std::invalid_argument("link name : " + link_name + " already exists");
        }

        auto fixed_joint = std::make_shared<urdf::Joint>();
        auto&& vec = urdf::Vector3(position[0], position[1], position[2]);
        // only these two infomation is used in kinematics computation
        fixed_joint->parent_to_joint_origin_transform.position = vec;
        fixed_joint->type = urdf::Joint::FIXED;

        int link_id = _links.size();
        auto new_link = std::make_shared<urdf::Link>();
        new_link->parent_joint = fixed_joint;
        new_link->setParent(_links[parent_id]);
        new_link->name = link_name;
        new_link->id = link_id;

        _link_ids[link_name] = link_id;
        _links.push_back(new_link);
        _links[parent_id]->child_links.push_back(new_link);
        _tf_cache.extend();

        this->_update_rptable(); // set _rptable
      }

    private:
      void _update_rptable(){
        // this function usually must come in the end of a function

        // we must recreate from scratch
        int n_link = _link_ids.size();
        int n_dof = _joint_ids.size();
        auto rptable = RelevancePredicateTable(n_link, n_dof);

        for(urdf::JointSharedPtr joint : _joints){
          int joint_id = _joint_ids.at(joint->name);
          urdf::LinkSharedPtr clink = joint->getChildLink();
          std::stack<urdf::LinkSharedPtr> link_stack;
          link_stack.push(clink);
          while(!link_stack.empty()){
            auto here_link = link_stack.top();
            link_stack.pop();
            rptable._table[joint_id][here_link->id] = true;
            for(auto& link : here_link->child_links){
              link_stack.push(link);
            }
          }
        }
        _rptable = rptable;
      }
  };

  RobotModel construct_from_urdfpath(const std::string& urdf_path);
};//end namespace

#endif // include guard


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/functional.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto plus = [](auto x, auto y) {
        return x + y;
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto to_int = [](char c) {
        return static_cast<int>(c) - 48;
    };

    BOOST_HANA_CONSTEXPR_LAMBDA auto inc = [](auto x) {
        return x + 1;
    };

    BOOST_HANA_CONSTEXPR_CHECK(lockstep(plus)(to_int, inc)('3', 4) == 3 + 5);
    //! [main]
}


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

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>

using namespace boost;
using namespace unit_test;
using namespace accumulators;

template<typename T>
void assert_is_double(T const &)
{
    BOOST_MPL_ASSERT((is_same<T, double>));
}

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    accumulator_set<int, stats<droppable<tag::mean> > > acc, test_acc(sample = 0);

    acc(1);
    BOOST_CHECK_CLOSE(1., mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(1u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    acc(0);
    BOOST_CHECK_CLOSE(0.5, mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    acc.drop<tag::mean>();

    acc(2);
    BOOST_CHECK_CLOSE(0.5, mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    assert_is_double(mean(acc));

    accumulator_set<int, stats<droppable<tag::mean(immediate)> > > acc2, test_acc2(sample = 0);

    acc2(1);
    BOOST_CHECK_CLOSE(1., mean(acc2), 1e-5);
    BOOST_CHECK_EQUAL(1u, count(acc2));

    acc2(0);
    BOOST_CHECK_CLOSE(0.5, mean(acc2), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc2));

    acc2.drop<tag::mean>();

    acc2(2);
    BOOST_CHECK_CLOSE(0.5, mean(acc2), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc2));

    assert_is_double(mean(acc2));
}

///////////////////////////////////////////////////////////////////////////////
// test_stat2
//
void test_stat2()
{
    accumulator_set<int, stats<droppable<tag::sum>, droppable<tag::mean> > > acc, test_acc(sample = 0);

    acc(1);
    BOOST_CHECK_CLOSE(1., mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(1u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    acc(0);
    BOOST_CHECK_CLOSE(0.5, mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    acc.drop<tag::mean>();
    acc.drop<tag::sum>();

    acc(2);
    BOOST_CHECK_CLOSE(0.5, mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    assert_is_double(mean(acc));
}

///////////////////////////////////////////////////////////////////////////////
// test_stat3
//
void test_stat3()
{
    accumulator_set<int, stats<droppable<tag::sum>, droppable<tag::count>, droppable<tag::mean> > > acc, test_acc(sample = 0);

    acc(1);
    BOOST_CHECK_CLOSE(1., mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(1u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    acc(0);
    BOOST_CHECK_CLOSE(0.5, mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    acc.drop<tag::mean>();
    acc.drop<tag::sum>();

    acc(2);
    BOOST_CHECK_CLOSE(1./3., mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(3u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    acc.drop<tag::count>();
    acc(3);
    BOOST_CHECK_CLOSE(1./3., mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(3u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));

    assert_is_double(mean(acc));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("droppable test");

    test->add(BOOST_TEST_CASE(&test_stat));
    test->add(BOOST_TEST_CASE(&test_stat2));
    test->add(BOOST_TEST_CASE(&test_stat3));

    return test;
}


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

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/constant/register.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_oneo_180 oneo_180
 *
 * \par Description
 * Constant oneo_180 = 1/180
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/oneo_180.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::oneo_180_(A0)>::type
 *     oneo_180();
 * }
 * \endcode
 *
 *
 * \param T template parameter of oneo_180
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag oneo_180 of functor oneo_180
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    BOOST_SIMD_CONSTANT_REGISTER( Oneo_180, int, 0
                                 , 0x3bb60b61, 0x3f76c16c16c16c17ll
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Oneo_180, Oneo_180)
} }

#include <boost/simd/sdk/constant/common.hpp>

#endif


#ifndef INCLUDED_COMMON_TYPES_HH
#define INCLUDED_COMMON_TYPES_HH

#include <Eigen/Core>

#include <functional>
#include <random>

namespace common {
  using index_type    = Eigen::Index;
  using vector_type   = Eigen::VectorXd;
  using function_type = std::function<double(vector_type)>;
  using rng_type      = std::mt19937_64;
}

#endif


#define BOOST_TEST_MODULE unit_length_test

#include <boost/test/unit_test.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/cgs/length.hpp>
#include <boost/astronomy/units/length.hpp>

namespace bu = boost::units;
namespace bau = boost::astronomy::units;

BOOST_AUTO_TEST_SUITE(length_unit)

BOOST_AUTO_TEST_CASE(units_astronomical_unit)
{
    BOOST_TEST((std::is_same<
        bu::quantity<bu::astronomical::astronomical_unit_base_unit::unit_type>,
        bu::quantity<bau::length_astronomical_unit>>::value
        ));

    bu::quantity<
        bu::astronomical::astronomical_unit_base_unit::unit_type,
        int
    > q1 = 10 * bau::astronomical_unit;
    bu::quantity<bau::length_astronomical_unit, int> q2 = 10 * bau::astronomical_units;

    BOOST_TEST((q1.value() == 10));
    BOOST_TEST((q1 == q2));
}

BOOST_AUTO_TEST_CASE(units_light_day)
{
    BOOST_TEST((std::is_same<
        bu::quantity<bu::astronomical::light_day_base_unit::unit_type>,
        bu::quantity<bau::length_light_day>>::value
        ));

    bu::quantity<bu::astronomical::light_day_base_unit::unit_type, int> q1 = 10 * bau::light_day;
    bu::quantity<bau::length_light_day, int> q2 = 10 * bau::light_days;

    BOOST_TEST((q1.value() == 10));
    BOOST_TEST((q1 == q2));
}

BOOST_AUTO_TEST_CASE(units_light_hour)
{
    BOOST_TEST((std::is_same<
        bu::quantity<bu::astronomical::light_hour_base_unit::unit_type>,
        bu::quantity<bau::length_light_hour>>::value
        ));

    bu::quantity<bu::astronomical::light_hour_base_unit::unit_type, int> q1 = 10 * bau::light_hour;
    bu::quantity<bau::length_light_hour, int> q2 = 10 * bau::light_hour;

    BOOST_TEST((q1.value() == 10));
    BOOST_TEST((q1 == q2));
}

BOOST_AUTO_TEST_CASE(units_light_minute)
{
    BOOST_TEST((std::is_same<
        bu::quantity<bu::astronomical::light_minute_base_unit::unit_type>,
        bu::quantity<bau::length_light_minute>>::value
        ));

    bu::quantity<bu::astronomical::light_minute_base_unit::unit_type> q1 = 10 * bau::light_minute;
    bu::quantity<bau::length_light_minute, int> q2 = 10 * bau::light_minutes;

    BOOST_TEST((q1.value() == 10));
    BOOST_TEST((q1 == q2));
}

BOOST_AUTO_TEST_CASE(units_light_second)
{
    BOOST_TEST((std::is_same<
        bu::quantity<bu::astronomical::light_second_base_unit::unit_type>,
        bu::quantity<bau::length_light_second>>::value
        ));

    bu::quantity<bu::astronomical::light_second_base_unit::unit_type> q1 = 10 * bau::light_second;
    bu::quantity<bau::length_light_second, int> q2 = 10 * bau::light_seconds;

    BOOST_TEST((q1.value() == 10));
    BOOST_TEST((q1 == q2));
}

BOOST_AUTO_TEST_CASE(units_light_year)
{
    BOOST_TEST((std::is_same<
        bu::quantity<bu::astronomical::light_year_base_unit::unit_type>,
        bu::quantity<bau::length_light_year>>::value
        ));

    bu::quantity<bu::astronomical::light_year_base_unit::unit_type, int> q1 = 10 * bau::light_year;
    bu::quantity<bau::length_light_year, int> q2 = 10 * bau::light_years;

    BOOST_TEST((q1.value() == 10));
    BOOST_TEST((q1 == q2));
}

BOOST_AUTO_TEST_CASE(units_parsec)
{
    BOOST_TEST((std::is_same<
        bu::quantity<bu::astronomical::parsec_base_unit::unit_type>,
        bu::quantity<bau::length_parsec>>::value
        ));

    bu::quantity<bu::astronomical::parsec_base_unit::unit_type, int> q1 = 10 * bau::parsec;
    bu::quantity<bau::length_parsec, int> q2 = 10 * bau::parsecs;

    BOOST_TEST((q1.value() == 10));
    BOOST_TEST((q1 == q2));
}

BOOST_AUTO_TEST_CASE(units_meter)
{
    BOOST_TEST((std::is_same<
        bu::quantity<bu::si::length>,
        bu::quantity<bau::length_meter>>::value
        ));
    BOOST_TEST((std::is_same<
        bu::quantity<bu::si::length>,
        bu::quantity<bau::length_metre>>::value
        ));

    bu::quantity<bu::si::length, int> q1 = 10 * bau::meter;
    bu::quantity<bau::length_meter, int> q2 = 10 * bau::meters;

    BOOST_TEST((q1.value() == 10));
    BOOST_TEST((q1 == q2));

    q1 = 10 * bau::metre;
    q2 = 10 * bau::metres;

    BOOST_TEST((q1.value() == 10));
    BOOST_TEST((q1 == q2));
}

BOOST_AUTO_TEST_CASE(units_centimeter)
{
    BOOST_TEST((std::is_same<
        bu::quantity<bu::cgs::length>,
        bu::quantity<bau::length_centimeter>>::value
        ));
    BOOST_TEST((std::is_same<
        bu::quantity<bu::cgs::length>,
        bu::quantity<bau::length_centimetre>>::value
        ));

    bu::quantity<bu::cgs::length, int> q1 = 10 * bau::centimeter;
    bu::quantity<bau::length_centimeter, int> q2 = 10 * bau::centimeters;

    BOOST_TEST((q1.value() == 10));
    BOOST_TEST((q1 == q2));

    q1 = 10 * bau::centimetre;
    q2 = 10 * bau::centimetres;

    BOOST_TEST((q1.value() == 10));
    BOOST_TEST((q1 == q2));
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/test/unit_test.hpp>

#include <stdlib.h>

#include <rpc/blockchain.cpp>
#include <test/test_bitcoin.h>

/* Equality between doubles is imprecise. Comparison should be done
 * with a small threshold of tolerance, rather than exact equality.
 */
static bool DoubleEquals(double a, double b, double epsilon)
{
    return std::abs(a - b) < epsilon;
}

static CBlockIndex* CreateBlockIndexWithNbits(uint32_t nbits)
{
    CBlockIndex* block_index = new CBlockIndex();
    block_index->nHeight = 46367;
    block_index->nTime = 1269211443;
    block_index->nBits = nbits;
    return block_index;
}

static CChain CreateChainWithNbits(uint32_t nbits)
{
    CBlockIndex* block_index = CreateBlockIndexWithNbits(nbits);
    CChain chain;
    chain.SetTip(block_index);
    return chain;
}

static void RejectDifficultyMismatch(double difficulty, double expected_difficulty) {
     BOOST_CHECK_MESSAGE(
        DoubleEquals(difficulty, expected_difficulty, 0.00001),
        "Difficulty was " + std::to_string(difficulty)
            + " but was expected to be " + std::to_string(expected_difficulty));
}

/* Given a BlockIndex with the provided nbits,
 * verify that the expected difficulty results.
 */
static void TestDifficulty(uint32_t nbits, double expected_difficulty)
{
    CBlockIndex* block_index = CreateBlockIndexWithNbits(nbits);
    /* Since we are passing in block index explicitly,
     * there is no need to set up anything within the chain itself.
     */
    CChain chain;

    double difficulty = GetDifficulty(chain, block_index);
    delete block_index;

    RejectDifficultyMismatch(difficulty, expected_difficulty);
}

BOOST_FIXTURE_TEST_SUITE(blockchain_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(get_difficulty_for_very_low_target)
{
    TestDifficulty(0x1f111111, 0.000001);
}

BOOST_AUTO_TEST_CASE(get_difficulty_for_low_target)
{
    TestDifficulty(0x1ef88f6f, 0.000016);
}

BOOST_AUTO_TEST_CASE(get_difficulty_for_mid_target)
{
    TestDifficulty(0x1df88f6f, 0.004023);
}

BOOST_AUTO_TEST_CASE(get_difficulty_for_high_target)
{
    TestDifficulty(0x1cf88f6f, 1.029916);
}

BOOST_AUTO_TEST_CASE(get_difficulty_for_very_high_target)
{
    TestDifficulty(0x12345678, 5913134931067755359633408.0);
}

// Verify that difficulty is 1.0 for an empty chain.
BOOST_AUTO_TEST_CASE(get_difficulty_for_null_tip)
{
    CChain chain;
    double difficulty = GetDifficulty(chain, nullptr);
    RejectDifficultyMismatch(difficulty, 1.0);
}

/* Verify that if difficulty is based upon the block index
 * in the chain, if no block index is explicitly specified.
 */
BOOST_AUTO_TEST_CASE(get_difficulty_for_null_block_index)
{
    CChain chain = CreateChainWithNbits(0x1df88f6f);

    double difficulty = GetDifficulty(chain, nullptr);
    delete chain.Tip();

    double expected_difficulty = 0.004023;

    RejectDifficultyMismatch(difficulty, expected_difficulty);
}

/* Verify that difficulty is based upon the explicitly specified
 * block index rather than being taken from the provided chain,
 * when both are present.
 */
BOOST_AUTO_TEST_CASE(get_difficulty_for_block_index_overrides_tip)
{
    CChain chain = CreateChainWithNbits(0x1df88f6f);
    /* This block index's nbits should be used
     * instead of the chain's when calculating difficulty.
     */
    CBlockIndex* override_block_index = CreateBlockIndexWithNbits(0x12345678);

    double difficulty = GetDifficulty(chain, override_block_index);
    delete chain.Tip();
    delete override_block_index;

    RejectDifficultyMismatch(difficulty, 5913134931067755359633408.0);
}

BOOST_AUTO_TEST_SUITE_END()