//
// Copyright 2019-2020 Mateusz Loskot <mateusz at loskot dot net>
//
// 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 <boost/gil.hpp>
#include <boost/gil/extension/numeric/channel_numeric_operations.hpp>

#include <boost/core/lightweight_test.hpp>

#include <tuple>
#include <type_traits>

#include "test_utility_output_stream.hpp"
#include "core/channel/test_fixture.hpp"

namespace gil = boost::gil;
namespace fixture = boost::gil::test::fixture;

struct test_plus_integer_same_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        gil::channel_plus_t<channel_t, channel_t, channel_t> f;
        BOOST_TEST_EQ(f(0, 0), channel_t(0));
        BOOST_TEST_EQ(f(100, 27), channel_t(127));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_plus_integer_same_types{});
    }
};

struct test_plus_integer_mixed_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        {
            using channel1_t = channel_t;
            using channel2_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
            gil::channel_plus_t<channel1_t, channel2_t, channel1_t> f;
            BOOST_TEST_EQ(f(0, 0), channel1_t(0));
            BOOST_TEST_EQ(f(100, 27), channel_t(127));
        }
        {
            using channel1_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
            using channel2_t = channel_t;
            gil::channel_plus_t<channel1_t, channel2_t, channel2_t> f;
            BOOST_TEST_EQ(f(0, 0), channel2_t(0));
            BOOST_TEST_EQ(f(100, 27), channel_t(127));
        }
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_plus_integer_mixed_types{});
    }
};

struct test_plus_integer_signed_types_with_overflow
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        // Signed integer overflow is UB, so just check addition does not yield mathematically
        // expected value but is constrained by the range of representable values for given type.

        auto const max_value = gil::channel_traits<channel_t>::max_value();
        gil::channel_plus_t<channel_t, channel_t, channel_t> f;
        BOOST_TEST_NE(f(max_value, 1), std::int64_t(max_value) + 1);
        BOOST_TEST_NE(f(max_value, max_value), std::int64_t(max_value) + max_value);
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_signed_types>(test_plus_integer_signed_types_with_overflow{});
    }
};

struct test_plus_integer_unsigned_types_with_wraparound
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        // The C Standard, 6.2.5, paragraph 9 [ISO/IEC 9899:2011], states:
        // A computation involving unsigned operands can never overflow, because a result that
        // cannot be represented by the resulting unsigned integer type is reduced modulo the number
        // that is one greater than the largest value that can be represented by the resulting type.

        auto const max_value = gil::channel_traits<channel_t>::max_value();
        auto const min_value = gil::channel_traits<channel_t>::min_value();
        gil::channel_plus_t<channel_t, channel_t, channel_t> f;
        BOOST_TEST_EQ(f(max_value, 1), min_value);
        BOOST_TEST_EQ(f(max_value, max_value), max_value - 1);
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_unsigned_types>(test_plus_integer_unsigned_types_with_wraparound{});
    }
};

struct test_minus_integer_same_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        gil::channel_minus_t<channel_t, channel_t, channel_t> f;
        BOOST_TEST_EQ(f(0, 0), channel_t(0));
        BOOST_TEST_EQ(f(100, 27), channel_t(73));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_minus_integer_same_types{});
    }
};

struct test_minus_integer_mixed_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        {
            using channel1_t = channel_t;
            using channel2_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
            gil::channel_minus_t<channel1_t, channel2_t, channel1_t> f;
            BOOST_TEST_EQ(f(0, 0), channel1_t(0));
            BOOST_TEST_EQ(f(100, 27), channel_t(73));
        }
        {
            using channel1_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
            using channel2_t = channel_t;
            gil::channel_minus_t<channel1_t, channel2_t, channel2_t> f;
            BOOST_TEST_EQ(f(0, 0), channel2_t(0));
            BOOST_TEST_EQ(f(100, 27), channel_t(73));
        }
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_minus_integer_mixed_types{});
    }
};

struct test_multiplies_integer_same_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        gil::channel_multiplies_t<channel_t, channel_t, channel_t> f;
        BOOST_TEST_EQ(f(0, 0), channel_t(0));
        BOOST_TEST_EQ(f(1, 1), channel_t(1));
        BOOST_TEST_EQ(f(4, 2), channel_t(8));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_multiplies_integer_same_types{});
    }
};

struct test_multiplies_integer_mixed_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        {
            using channel1_t = channel_t;
            using channel2_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
            gil::channel_multiplies_t<channel1_t, channel2_t, channel1_t> f;
            BOOST_TEST_EQ(f(0, 0), channel1_t(0));
            BOOST_TEST_EQ(f(4, 2), channel_t(8));
        }
        {
            using channel1_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
            using channel2_t = channel_t;
            gil::channel_multiplies_t<channel1_t, channel2_t, channel2_t> f;
            BOOST_TEST_EQ(f(0, 0), channel2_t(0));
            BOOST_TEST_EQ(f(4, 2), channel_t(8));
        }
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_multiplies_integer_mixed_types{});
    }
};

struct test_divides_integer_same_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        gil::channel_divides_t<channel_t, channel_t, channel_t> f;
        BOOST_TEST_EQ(f(0, 1), channel_t(0));
        BOOST_TEST_EQ(f(1, 1), channel_t(1));
        BOOST_TEST_EQ(f(4, 2), channel_t(2));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_divides_integer_same_types{});
    }
};

struct test_divides_integer_mixed_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        {
            using channel1_t = channel_t;
            using channel2_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
            gil::channel_divides_t<channel1_t, channel2_t, channel1_t> f;
            BOOST_TEST_EQ(f(0, 1), channel1_t(0));
            BOOST_TEST_EQ(f(4, 2), channel_t(2));
        }
        {
            using channel1_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
            using channel2_t = channel_t;
            gil::channel_divides_t<channel1_t, channel2_t, channel2_t> f;
            BOOST_TEST_EQ(f(0, 1), channel2_t(0));
            BOOST_TEST_EQ(f(4, 2), channel_t(2));
        }
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_divides_integer_mixed_types{});
    }
};

struct test_plus_scalar_integer_same_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        gil::channel_plus_scalar_t<channel_t, int, channel_t> f;
        BOOST_TEST_EQ(f(0, 0), channel_t(0));
        BOOST_TEST_EQ(f(100, 27), channel_t(127));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_plus_scalar_integer_same_types{});
    }
};

struct test_plus_scalar_integer_mixed_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        using channel_result_t = std::uint8_t;
        gil::channel_plus_scalar_t<channel_t, int, channel_result_t> f;
        BOOST_TEST_EQ(f(0, 0), channel_result_t(0));
        BOOST_TEST_EQ(f(100, 27), channel_result_t(127));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_plus_scalar_integer_mixed_types{});
    }
};

struct test_minus_scalar_integer_same_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        gil::channel_minus_scalar_t<channel_t, int, channel_t> f;
        BOOST_TEST_EQ(f(0, 0), channel_t(0));
        BOOST_TEST_EQ(f(100, 27), channel_t(73));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_minus_scalar_integer_same_types{});
    }
};

struct test_minus_scalar_integer_mixed_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        using channel_result_t = std::uint8_t;
        gil::channel_minus_scalar_t<channel_t, int, std::uint8_t> f;
        BOOST_TEST_EQ(f(0, 0), channel_result_t(0));
        BOOST_TEST_EQ(f(100, 27), channel_result_t(73));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_minus_scalar_integer_mixed_types{});
    }
};

struct test_multiplies_scalar_integer_same_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        gil::channel_multiplies_scalar_t<channel_t, channel_t, channel_t> f;
        BOOST_TEST_EQ(f(0, 0), channel_t(0));
        BOOST_TEST_EQ(f(1, 1), channel_t(1));
        BOOST_TEST_EQ(f(4, 2), channel_t(8));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_multiplies_scalar_integer_same_types{});
    }
};

struct test_multiplies_scalar_integer_mixed_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        using channel_result_t = std::uint8_t;
        gil::channel_multiplies_scalar_t<channel_t, int, channel_result_t> f;
        BOOST_TEST_EQ(f(0, 0), channel_result_t(0));
        BOOST_TEST_EQ(f(4, 2), channel_result_t(8));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_multiplies_scalar_integer_mixed_types{});
    }
};

struct test_divides_scalar_integer_same_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        gil::channel_divides_scalar_t<channel_t, channel_t, channel_t> f;
        BOOST_TEST_EQ(f(0, 1), channel_t(0));
        BOOST_TEST_EQ(f(1, 1), channel_t(1));
        BOOST_TEST_EQ(f(4, 2), channel_t(2));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_divides_scalar_integer_same_types{});
    }
};

struct test_divides_scalar_integer_mixed_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        using channel_result_t = std::uint8_t; // duplicates only one of fixture::channel_integer_types
        gil::channel_divides_scalar_t<channel_t, int, channel_result_t> f;
        BOOST_TEST_EQ(f(0, 1), channel_t(0));
        BOOST_TEST_EQ(f(4, 2), channel_t(2));
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_divides_scalar_integer_mixed_types{});
    }
};

struct test_halves_integer_same_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        gil::channel_halves_t<channel_t> f;
        {
            channel_t c(0);
            f(c);
            BOOST_TEST_EQ(c, channel_t(0));
        }
        {
            channel_t c(2);
            f(c);
            BOOST_TEST_EQ(c, channel_t(1));
        }
        {
            channel_t c(4);
            f(c);
            BOOST_TEST_EQ(c, channel_t(2));
        }
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_halves_integer_same_types{});
    }
};

struct test_zeros_integer_same_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        gil::channel_zeros_t<channel_t> f;
        {
            channel_t c(0);
            f(c);
            BOOST_TEST_EQ(c, channel_t(0));
        }
        {
            channel_t c(2);
            f(c);
            BOOST_TEST_EQ(c, channel_t(0));
        }
        {
            channel_t c(4);
            f(c);
            BOOST_TEST_EQ(c, channel_t(0));
        }
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_zeros_integer_same_types{});
    }
};

struct test_assigns_integer_same_types
{
    template <typename Channel>
    void operator()(Channel const&)
    {
        using channel_t = Channel;
        gil::channel_assigns_t<channel_t, channel_t> f;
        {
            channel_t c1(10);
            channel_t c2(20);
            f(c1, c2);
            BOOST_TEST_EQ(c2, c1);
        }
    }
    static void run()
    {
        boost::mp11::mp_for_each<fixture::channel_integer_types>(test_assigns_integer_same_types{});
    }
};

int main()
{
    test_plus_integer_same_types::run();
    test_plus_integer_mixed_types::run();
    test_plus_integer_signed_types_with_overflow::run();
    test_plus_integer_unsigned_types_with_wraparound::run();

    test_minus_integer_same_types::run();
    test_minus_integer_mixed_types::run();

    test_multiplies_integer_same_types::run();
    test_multiplies_integer_mixed_types::run();

    test_divides_integer_same_types::run();
    test_divides_integer_mixed_types::run();

    test_plus_scalar_integer_same_types::run();
    test_plus_scalar_integer_mixed_types::run();

    test_minus_scalar_integer_same_types::run();
    test_minus_scalar_integer_mixed_types::run();

    test_multiplies_scalar_integer_same_types::run();
    test_multiplies_scalar_integer_mixed_types::run();

    test_divides_scalar_integer_same_types::run();
    test_divides_scalar_integer_mixed_types::run();

    test_halves_integer_same_types::run();
    test_zeros_integer_same_types::run();
    test_assigns_integer_same_types::run();

    return ::boost::report_errors();
}


//#include <ros/ros.h>
#include <fstream>
#include <iostream>
#include <map>
#include <vector>
#include <string>
#include <sstream>
#include <list>
#include <Eigen/Dense>
#include <sys/time.h>
#include <cmath>
#include <random>

#include <dynmeans/dynmeans.hpp>
#include "maxmatching.hpp"

using namespace std;

typedef Eigen::Vector2d V2d;

//function declarations
double computeAccuracy(vector<int> labels1, vector<int> labels2, map<int, int> matchings);
void birthDeathMotionProcesses(vector<V2d>& clusterCenters, vector<bool>& aliveClusters, double birthProbability, double deathProbability, double motionStdDev);
void generateData(vector<V2d> clusterCenters, vector<bool> aliveClusters, int nDataPerClusterPerStep, double likelihoodstd, vector<V2d>& clusterData, vector<int>& trueLabels);

//random number generator
mt19937 rng;//uses the same seed every time, 5489u

int main(int argc, char** argv){
	//generates clusters that jump around on the domain R^2
	//they move stochastically with a normal distribution w/ std deviation 0.05
	//they die stochastically with probability 0.05 at each step
	//a cluster is created stochastically with probability 0.10 at each step in the area [0,1]x[0,1]

	//note: when computing accuracy, ***proper tracking is taken into account***
	//i.e. if true label 1 is matched to learned label 3, then that matching is fixed from that point forward
	//later pairings that attempt to match true label 1 to something else
	//or learned label 3 to something else are discarded

	//constants
	//play with these to change the data generation process
	double birthProbability = 0.10;
	double deathProbability = 0.05;
	double motionStdDev = 0.05;
	double clusterStdDev = 0.05;
	int nDataPerClusterPerStep = 15; // datapoints generated for each cluster per timestep
	int nSteps = 100;//run the experiment for nSteps steps
	int initialClusters = 4; //the number of clusters to start with

	//data structures for the cluster centers
	vector<V2d> clusterCenters;
	vector<bool> aliveClusters;

	//start with some number of initial clusters
	uniform_real_distribution<double> uniformDist01(0, 1);
	for (int i = 0; i < initialClusters; i++){
			V2d newCenter;
			newCenter(0) = uniformDist01(rng);
			newCenter(1) = uniformDist01(rng);
			clusterCenters.push_back(newCenter);
			aliveClusters.push_back(true);
	}

	//the Dynamic Means object
	//play with lambda/Q/tau to change Dynamic Means' performance
	double lambda = 0.05;
	double T_Q = 6.8;
	double K_tau = 1.01;
	double Q = lambda/T_Q;
	double tau = (T_Q*(K_tau-1.0)+1.0)/(T_Q-1.0);
	int nRestarts = 10;
	DynMeans<V2d> dynm(lambda, Q, tau);

	//run the experiment
	double cumulativeAccuracy = 0;//stores the accuracy accumulated for each step
	map<int, int> matchings;//stores the saved matchings from previous timesteps
							//enables proper label tracking (see note at line 27)
	for (int i = 0; i < nSteps; i++){
		//****************************
		//birth/death/motion processes
		//****************************
		cout << "Step " << i << ": Clusters undergoing birth/death/motion..." << endl;
		birthDeathMotionProcesses(clusterCenters, aliveClusters, birthProbability, deathProbability, motionStdDev);
		//******************************************
		//generate the data for the current timestep
		//******************************************
		cout << "Step " << i << ": Generating data from the clusters..." << endl;
		vector<V2d> clusterData;
		vector<int> trueLabels;
		generateData(clusterCenters, aliveClusters, nDataPerClusterPerStep, clusterStdDev, clusterData, trueLabels);

		//***************************
		//cluster using Dynamic Means
		//***************************
		vector<V2d> learnedParams;
		vector<int> learnedLabels;
		double tTaken, obj;
		cout << "Step " << i << ": Clustering..." << endl;
		dynm.cluster(clusterData, nRestarts, learnedLabels, learnedParams, obj, tTaken);

		//***************************************************
		//calculate the accuracy via linear programming
		//including proper cluster label tracking (see above)
		//***************************************************
		matchings = getMaxMatchingConsistentWithOldMatching(learnedLabels, trueLabels, matchings);
		double acc = computeAccuracy(learnedLabels, trueLabels, matchings);
		cout << "Step " << i << ": Accuracy = " << acc <<  "\%" << endl;
		cumulativeAccuracy += acc;
	}
	cout << "Average Accuracy: " << cumulativeAccuracy/(double)nSteps << "\%" << endl;
	cout << "Done!" << endl;

	return 0;
}

//this function takes two label sets and a matching and outputs
//the accuracy of the labelling (assuming labels1 = learned, labels2 = true)
double computeAccuracy(vector<int> labels1, vector<int> labels2, map<int, int> matchings){
	if (labels1.size() != labels2.size()){
		cout << "Error: computeAccuracy requires labels1/labels2 to have the same size" << endl;
		return -1;
	}

	double acc = 0;
	for (int i = 0; i < labels1.size(); i++){
		if (matchings[labels1[i]] == labels2[i]){
			acc += 1.0;
		}
	}
	//compute the accuracy
	return 100.0*acc/(double)labels1.size();
}

//this function takes a set of cluster centers and runs them through a birth/death/motion model
void birthDeathMotionProcesses(vector<V2d>& clusterCenters, vector<bool>& aliveClusters, double birthProbability, double deathProbability, double motionStdDev){

	//distributions
	uniform_real_distribution<double> uniformDistAng(0, 2*M_PI);
	uniform_real_distribution<double> uniformDist01(0, 1);
	normal_distribution<double> transitionDistRadial(0, motionStdDev);

	for (int j = 0; j < clusterCenters.size(); j++){
		//for each cluster center, decide whether it dies
		if (aliveClusters[j] && uniformDist01(rng) < deathProbability){
			cout << "Cluster " << j << " died." << endl;
			aliveClusters[j] = false;
		} else if (aliveClusters[j]) {
			//if it survived, move it stochastically
			//radius sampled from normal
			double steplen = transitionDistRadial(rng);
			//angle sampled from uniform
			double stepang = uniformDistAng(rng);
			clusterCenters[j](0) += steplen*cos(stepang);
			clusterCenters[j](1) += steplen*sin(stepang);
			cout << "Cluster " << j << " moved to " << clusterCenters[j].transpose() << endl;
		}
	}
	//decide whether to create a new cluster
	if (uniformDist01(rng) < birthProbability || all_of(aliveClusters.begin(), aliveClusters.end(), [](bool b){return !b;}) ) {
		V2d newCenter;
		newCenter(0) = uniformDist01(rng);
		newCenter(1) = uniformDist01(rng);
		clusterCenters.push_back(newCenter);
		aliveClusters.push_back(true);
		cout << "Cluster " << clusterCenters.size()-1 << " was created at " << clusterCenters.back().transpose() << endl;
	}
}

//this function takes a set of cluster centers and generates data from them
void generateData(vector<V2d> clusterCenters, vector<bool> aliveClusters, int nDataPerClusterPerStep, double clusterStdDev, vector<V2d>& clusterData, vector<int>& trueLabels){

	//distributions
	uniform_real_distribution<double> uniformDistAng(0, 2*M_PI);
	normal_distribution<double> likelihoodDistRadial(0, clusterStdDev);
	//loop through alive centers, generate nDataPerClusterPerStep datapoints for each
	for (int j = 0; j < clusterCenters.size(); j++){
		if (aliveClusters[j]){
			for (int k = 0; k < nDataPerClusterPerStep; k++){
				V2d newData = clusterCenters[j];
				double len = likelihoodDistRadial(rng);
				double ang = uniformDistAng(rng);
				newData(0) += len*cos(ang);
				newData(1) += len*sin(ang);
				clusterData.push_back(newData);
				trueLabels.push_back(j);
			}
		}
	}
}


#ifndef USE_CUDA

#include <kernel.hpp>
#include <Eigen/Core>

// C++ Version
namespace Kernel
{
    double dot(const std::vector<Eigen::Vector3d> & v1, const std::vector<Eigen::Vector3d> & v2)
    {
        double x=0;
        for (int i=0; i<v1.size(); ++i)
        {
            x += v1[i].dot(v2[i]);
        }
        return x;
    }
}

#endif


#include "benchmark.h"

#include <Eigen/Core>
#include <chrono>
#include <iostream>
#include <cstdlib>

double dontOptimizeAway = 0;

void benchmark_Eigen(
    const std::vector<std::string>& parameterNames,
    const Json::Array& parameters,
    const std::vector<std::string>& returnNames,
    Json::Array& returnValues)
{
    //number of runs for time measures
    const int runs = 10;
	const int subruns = 1;

    //test if the config is valid
    assert(parameterNames.size() == 1);
    assert(parameterNames[0] == "Vector-Size");
    assert(returnNames.size() == 1);
    assert(returnNames[0] == "Time");

    int numConfigs = parameters.Size();
    for (int config = 0; config < numConfigs; ++config)
    {
        //Input
        int vectorSize = parameters[config][0].AsInt32();
        double totalTime = 0;
        std::cout << "  VectorSize: " << vectorSize << std::flush;

        //Create matrices
		Eigen::VectorXf a(vectorSize); a.setRandom();
		Eigen::VectorXf b(vectorSize); b.setRandom();

        //Run it multiple times
        for (int run = 0; run < runs; ++run)
        {
            //Main logic
            auto start = std::chrono::steady_clock::now();

			for (int i = 0; i < subruns; ++i) {
				float result = a.dot(b);
				dontOptimizeAway += result;
			}

            auto finish = std::chrono::steady_clock::now();
            double elapsed = std::chrono::duration_cast<
                std::chrono::duration<double> >(finish - start).count() * 1000 / subruns;
            totalTime += elapsed;
        }

        //Result
        Json::Array result;
        double finalTime = totalTime / runs;
        result.PushBack(finalTime);
        returnValues.PushBack(result);
        std::cout << " -> " << finalTime << "ms" << std::endl;
    }
}


/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "vision-precomp.h"   // Precompiled headers

#include <mrpt/config.h>
#include <mrpt/vision/utils.h>
#include <mrpt/vision/pnp_algos.h>

// Opencv 2.3 had a broken <opencv/eigen.h> in Ubuntu 14.04 Trusty => Disable PNP classes
#include <mrpt/config.h>

#if MRPT_HAS_OPENCV && MRPT_OPENCV_VERSION_NUM<0x240
#	undef MRPT_HAS_OPENCV
#	define MRPT_HAS_OPENCV 0
#endif

#include <iostream>

#include <mrpt/utils/types_math.h> // Eigen must be included first via MRPT to enable the plugin system
#include <Eigen/Core>
#include <Eigen/Dense>

#include <mrpt/otherlibs/do_opencv_includes.h>
#if MRPT_HAS_OPENCV
#	include <opencv2/core/eigen.hpp>
#endif

#include "dls.h"
#include "epnp.h"
#include "upnp.h"
#include "p3p.h"
#include "ppnp.h"
#include "posit.h"
#include "lhm.h"
#include "rpnp.h"
#include "so3.h"

bool mrpt::vision::pnp::CPnP::dls(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
    try{
        #if MRPT_HAS_OPENCV==1

        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R_eig;
        Eigen::MatrixXd t_eig;

        // Compute pose
        cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv(3,3,CV_32F), t_cv(3,1,CV_32F);

        cv::eigen2cv(cam_in_eig, cam_in_cv);
        cv::eigen2cv(img_pts_eig, img_pts_cv);
        cv::eigen2cv(obj_pts_eig, obj_pts_cv);

        mrpt::vision::pnp::dls d(obj_pts_cv, img_pts_cv);
        bool ret = d.compute_pose(R_cv,t_cv);

        cv::cv2eigen(R_cv, R_eig);
        cv::cv2eigen(t_cv, t_eig);

        Eigen::Quaterniond q(R_eig);

        pose_mat << t_eig,q.vec();

        return ret;

        #else
		throw(-1);
        #endif
    }
    catch(int e)
    {
        switch(e)
        {
            case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::epnp(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
    try{
        #if MRPT_HAS_OPENCV==1

        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R_eig;
        Eigen::MatrixXd t_eig;

        // Compute pose
        cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv, t_cv;

        cv::eigen2cv(cam_in_eig, cam_in_cv);
        cv::eigen2cv(img_pts_eig, img_pts_cv);
        cv::eigen2cv(obj_pts_eig, obj_pts_cv);

        mrpt::vision::pnp::epnp e(cam_in_cv, obj_pts_cv, img_pts_cv);
        e.compute_pose(R_cv,t_cv);

        cv::cv2eigen(R_cv, R_eig);
        cv::cv2eigen(t_cv, t_eig);

        Eigen::Quaterniond q(R_eig);

        pose_mat << t_eig,q.vec();

        return true;

        #else
        throw(-1);
        #endif
    }
    catch(int e){
        switch(e)
        {
            case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::upnp(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
    try{
        #if MRPT_HAS_OPENCV==1

        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R_eig;
        Eigen::MatrixXd t_eig;

        // Compute pose
        cv::Mat cam_in_cv(3,3,CV_32F), img_pts_cv(2,n,CV_32F), obj_pts_cv(3,n,CV_32F), R_cv, t_cv;

        cv::eigen2cv(cam_in_eig, cam_in_cv);
        cv::eigen2cv(img_pts_eig, img_pts_cv);
        cv::eigen2cv(obj_pts_eig, obj_pts_cv);

        mrpt::vision::pnp::upnp u(cam_in_cv, obj_pts_cv, img_pts_cv);
        u.compute_pose(R_cv,t_cv);

        cv::cv2eigen(R_cv, R_eig);
        cv::cv2eigen(t_cv, t_eig);

        Eigen::Quaterniond q(R_eig);

        pose_mat << t_eig,q.vec();

        return true;
        #else
        throw(-1);
        #endif
    }
    catch(int e)
    {
        switch(e)
        {
            case -1: std::cout << "Please install OpenCV for DLS-PnP" << std::endl;
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::p3p(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){

    try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::p3p p(cam_in_eig);
        bool ret = p.solve(R,t, obj_pts_eig, img_pts_eig);

        Eigen::Quaterniond q(R);

        pose_mat << t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::rpnp(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat){
    try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose();
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts;
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::rpnp r(obj_pts_eig, img_pts_eig, cam_in_eig, n);
        bool ret = r.compute_pose(R,t);

        Eigen::Quaterniond q(R);

        pose_mat << t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::ppnp(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat)
{
	try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose();
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts;
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::ppnp p(obj_pts_eig,img_pts_eig, cam_in_eig);

        bool ret = p.compute_pose(R,t,n);

        Eigen::Quaterniond q(R);

        pose_mat << t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::posit(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat)
{
	try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::posit p(obj_pts_eig,img_pts_eig, cam_in_eig, n);

        bool ret = p.compute_pose(R,t);

        Eigen::Quaterniond q(R);

        pose_mat << t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }

}

bool mrpt::vision::pnp::CPnP::lhm(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat)
{
    try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose();
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts;
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::lhm l(obj_pts_eig, img_pts_eig, cam_intrinsic, n);

        bool ret = l.compute_pose(R,t);

        Eigen::Quaterniond q(R);

        pose_mat<<t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}

bool mrpt::vision::pnp::CPnP::so3(const Eigen::Ref<Eigen::MatrixXd> obj_pts, const Eigen::Ref<Eigen::MatrixXd> img_pts, int n, const Eigen::Ref<Eigen::MatrixXd> cam_intrinsic, Eigen::Ref<Eigen::MatrixXd> pose_mat)
{
    try{
        // Input 2d/3d correspondences and camera intrinsic matrix
        Eigen::MatrixXd cam_in_eig,img_pts_eig, obj_pts_eig;

        // Check for consistency of input matrix dimensions
        if (img_pts.rows() != obj_pts.rows() || img_pts.cols() !=obj_pts.cols())
            throw(2);
        else if (cam_intrinsic.rows()!=3 || cam_intrinsic.cols()!=3)
            throw(3);

        if(obj_pts.rows() < obj_pts.cols())
        {
            cam_in_eig=cam_intrinsic.transpose();
            img_pts_eig=img_pts.transpose().block(0,0,n,2);
            obj_pts_eig=obj_pts.transpose();
        }
        else
        {
            cam_in_eig=cam_intrinsic;
            img_pts_eig=img_pts.block(0,0,n,2);
            obj_pts_eig=obj_pts;
        }

        // Output pose
        Eigen::Matrix3d R;
        Eigen::Vector3d t;

        // Compute pose
        mrpt::vision::pnp::p3p p(cam_in_eig);
        p.solve(R,t, obj_pts_eig, img_pts_eig);

        mrpt::vision::pnp::so3 s(obj_pts_eig, img_pts_eig, cam_in_eig, n);
        bool ret = s.compute_pose(R,t);

        Eigen::Quaterniond q(R);

        pose_mat<<t,q.vec();

        return ret;
    }
    catch(int e)
    {
        switch(e)
        {
            case  2: std::cout << "2d/3d correspondences mismatch\n Check dimension of obj_pts and img_pts" << std::endl;
            case  3: std::cout << "Camera intrinsic matrix does not have 3x3 dimensions " << std::endl;
        }
        return false;
    }
}


/**
 * @author     : n-jing (siliuhe@sina.com)
 * @date       : 2020-06-30
 * @description: demo of embedded mass-spring and embedded fem methods
 * @version    : 1.0
 *
 * @author     : Zhu Fei (feizhu@pku.edu.cn)
 * @date       : 2021-07-19
 * @description: poslish code
 * @version    : 1.1
 */
#include <memory>

#include <boost/property_tree/json_parser.hpp>

#include "Framework/Framework/SceneGraph.h"
#include "Framework/Framework/Log.h"
#include "Dynamics/ParticleSystem/ParticleElasticBody.h"
#include "Dynamics/ParticleSystem/ElasticityModule.h"
#include "Dynamics/ParticleSystem/StaticBoundary.h"
#include "Dynamics/ParticleSystem/SolidFluidInteraction.h"
#include "Framework/Mapping/PointSetToPointSet.h"
#include "Dynamics/EmbeddedMethod/EmbeddedFiniteElement.h"
#include "Dynamics/EmbeddedMethod/EmbeddedMassSpring.h"
#include "Rendering/SurfaceMeshRender.h"
#include "GUI/GlutGUI/GLApp.h"

using namespace std;
using namespace PhysIKA;

/**
 * setup properties for the object to be simulated
 *
 * @param[in] bunny  the scene node representing the elastic object
 * @param[in] i      index of the elastic object
 */
template <typename T>
void setupModel(T& bunny, int i)
{
    auto sRender = std::make_shared<SurfaceMeshRender>();
    bunny->getSurfaceNode()->addVisualModule(sRender);

    if (i == 0)
        sRender->setColor(Vector3f(1, 1, 0));
    else if (i == 1)
        sRender->setColor(Vector3f(1, 0, 1));
    else if (i == 2)
        sRender->setColor(Vector3f(0, 1, 1));
    else if (i == 3)
        sRender->setColor(Vector3f(0, 0, 1));
    else if (i == 4)
        sRender->setColor(Vector3f(0, 1, 0));
    else
        sRender->setColor(Vector3f(1, 0, 0));

    bunny->setMass(1.0);
    bunny->loadParticles("../../Media/bunny/sparse_bunny_points.obj");
    bunny->loadSurface("../../Media/bunny/bunny_mesh.obj");
    bunny->translate(Vector3f(0.2 + 0.3 * i, 0.2, 0.8));
    bunny->setVisible(true);
    bunny->getElasticitySolver()->setIterationNumber(10);
    bunny->getElasticitySolver()->inHorizon()->setValue(0.03);
    bunny->getTopologyMapping()->setSearchingRadius(0.05);
}

/**
 * add elastic body in scene
 *
 * @param[in] root  root node of current scene graph
 * @param[in] sfi   sfi node in scene, elastic bodies are registered to the node for collision handling
 * @param[in] i     index of the elastic body, setup model color and position according to this index
 * @param[in] model name of the simulation method backend
 */
void addSimulationModel(std::shared_ptr<StaticBoundary<DataType3f>>& root, std::shared_ptr<SolidFluidInteraction<DataType3f>>& sfi, int i, std::string model = "")
{
    if (model == "mass_spring")
    {
        std::shared_ptr<EmbeddedMassSpring<DataType3f>> bunny = std::make_shared<EmbeddedMassSpring<DataType3f>>();
        root->addParticleSystem(bunny);
        setupModel(bunny, i);

        boost::property_tree::ptree pt;
        read_json("../../Media/bunny/collision_hybrid.json", pt);
        bunny->init_problem_and_solver(pt);
        sfi->addParticleSystem(bunny);
    }
    else if (model == "fem")
    {
        std::shared_ptr<EmbeddedFiniteElement<DataType3f>> bunny = std::make_shared<EmbeddedFiniteElement<DataType3f>>();
        root->addParticleSystem(bunny);
        setupModel(bunny, i);

        boost::property_tree::ptree pt;
        read_json("../../Media/bunny/collision_hybrid.json", pt);
        bunny->init_problem_and_solver(pt);
        sfi->addParticleSystem(bunny);
    }
    else
    {
        std::shared_ptr<ParticleElasticBody<DataType3f>> bunny = std::make_shared<ParticleElasticBody<DataType3f>>();
        root->addParticleSystem(bunny);
        setupModel(bunny, i);
        sfi->addParticleSystem(bunny);
    }
}

/**
 * setup scene: 3 elastic bunnies spacing horizontally fall to the ground, using different algorithm backend
 */
void createScene()
{

    SceneGraph& scene = SceneGraph::getInstance();
    scene.setUpperBound(Vector3f(1, 2.0, 1));
    scene.setLowerBound(Vector3f(0, 0.0, 0));

    std::shared_ptr<StaticBoundary<DataType3f>> root = scene.createNewScene<StaticBoundary<DataType3f>>();
    root->loadCube(Vector3f(0, 0.0, 0), Vector3f(1, 2.0, 1), 0.015f, true);  //scene boundary

    //use SFI node to handle particle collisions between elastic bodies
    std::shared_ptr<SolidFluidInteraction<DataType3f>> sfi = std::make_shared<SolidFluidInteraction<DataType3f>>();
    root->addChild(sfi);
    sfi->setInteractionDistance(0.03);  // 0.03 is an very important parameter

    //3 elastic bodies, one with embedded-mass-spring backend, one with embedded-fem backend, one with projective-peridynamics backend
    for (int i = 0; i < 3; i++)
    {
        string model = (i % 3 == 0) ? "mass_spring" : ((i % 3 == 1) ? "fem" : "");
        addSimulationModel(root, sfi, i, model);
    }
}

int main()
{
    createScene();

    Log::setOutput("console_log.txt");
    Log::setLevel(Log::Info);
    Log::sendMessage(Log::Info, "Simulation begin");

    GLApp window;
    window.createWindow(1024, 768);
    window.mainLoop();

    Log::sendMessage(Log::Info, "Simulation end!");
    return 0;
}


//  Copyright (c) 2018 Robert Ramey
//
// 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 <iostream>

#include <boost/safe_numerics/safe_integer.hpp>

int main(int, const char *[]){
    std::cout << "example 3:";
    std::cout << "undetected underflow in data type" << std::endl;
    std::cout << "Not using safe numerics" << std::endl;
    // problem: decrement can yield incorrect result
    try{
        unsigned int x = 0;
        // the following silently produces an incorrect result
        --x;
        std::cout << x << " != " << -1 << std::endl;

        // when comparing int and unsigned int, C++ converts
        // the int to unsigned int so the following assertion
        // fails to detect the above error!
        assert(x == -1);

        std::cout << "error NOT detected!" << std::endl;
    }
    catch(const std::exception &){
        // never arrive here
        std::cout << "error detected!" << std::endl;
    }
    // solution: replace unsigned int with safe<unsigned int>
    std::cout << "Using safe numerics" << std::endl;
    try{
        using namespace boost::safe_numerics;
        safe<unsigned int> x = 0;
        // decrement unsigned to less than zero throws exception
        --x;
        assert(false); // never arrive here
    }
    catch(const std::exception & e){
        std::cout << e.what() << std::endl;
        std::cout << "error detected!" << std::endl;
    }
    return 0;
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  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/simd/function/scalar/secpi.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/sqrt_2.hpp>

STF_CASE_TPL (" secpi",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::secpi;

  using r_t = decltype(secpi(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(secpi(bs::Half<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(secpi(bs::Inf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(secpi(bs::Mhalf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(secpi(bs::Minf<T>()), bs::Nan<r_t>(), 0.5);
  STF_ULP_EQUAL(secpi(bs::Nan<T>()), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(secpi(-bs::Quarter<T>()), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(secpi(bs::One<T>()), bs::Mone<r_t>(), 0.5);
  STF_ULP_EQUAL(secpi(bs::Quarter<T>()), bs::Sqrt_2<r_t>(), 0.5);
  STF_ULP_EQUAL(secpi(bs::Zero<T>()), bs::One<r_t>(), 0.5);
}


//  (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/time_series/ordered_inserter.hpp>
#include <boost/time_series/sparse_series.hpp>
#include <boost/time_series/constant_series.hpp>
#include <boost/time_series/piecewise_constant_series.hpp>
#include <boost/time_series/numeric/numeric.hpp>

using namespace boost::unit_test;
using namespace boost::time_series;

namespace seq = boost::sequence;
namespace rrs = boost::range_run_storage;

std::ptrdiff_t min_ = -inf;
std::ptrdiff_t max_ = inf;

boost::time_series::piecewise_constant_series<int> tmp;
#define BOOST_CHECK_RANGE_RUN_EQUAL(x,y)\
    BOOST_CHECK_EQUAL(x, (boost::time_series::make_ordered_inserter(::tmp) y .commit(), ::tmp))

///////////////////////////////////////////////////////////////////////////////
// test_constant_series
//
void test_constant_series()
{
    // check addition of two constant series yields a constant series
    constant_series<int> c1(4);
    constant_series<int> c2(5);

    BOOST_CHECK_EQUAL(0, std::distance(seq::begin(c1), seq::end(c1)));
    BOOST_CHECK_EQUAL(min_, rrs::offset(rrs::pre_run(c1)));
    BOOST_CHECK_EQUAL(max_, rrs::end_offset(rrs::pre_run(c1)));
    BOOST_CHECK_EQUAL(4, rrs::pre_value(c1));

    BOOST_CHECK_EQUAL(0, std::distance(seq::begin(c2), seq::end(c2)));
    BOOST_CHECK_EQUAL(min_, rrs::offset(rrs::pre_run(c2)));
    BOOST_CHECK_EQUAL(max_, rrs::end_offset(rrs::pre_run(c2)));
    BOOST_CHECK_EQUAL(5, rrs::pre_value(c2));

    constant_series<int> c3 = c1 + c2;

    BOOST_CHECK_EQUAL(0, std::distance(seq::begin(c3), seq::end(c3)));
    BOOST_CHECK_EQUAL(min_, rrs::offset(rrs::pre_run(c3)));
    BOOST_CHECK_EQUAL(max_, rrs::end_offset(rrs::pre_run(c3)));
    BOOST_CHECK_EQUAL(9, rrs::pre_value(c3));

    c3 += c1;

    BOOST_CHECK_EQUAL(0, std::distance(seq::begin(c3), seq::end(c3)));
    BOOST_CHECK_EQUAL(min_, rrs::offset(rrs::pre_run(c3)));
    BOOST_CHECK_EQUAL(max_, rrs::end_offset(rrs::pre_run(c3)));
    BOOST_CHECK_EQUAL(13, rrs::pre_value(c3));
}

///////////////////////////////////////////////////////////////////////////////
// test_constant_series_2
//
void test_constant_series_2()
{
    typedef rrs::unit_run<std::ptrdiff_t> run; // sparse run

    // check addition of two constant series yields a constant series
    constant_series<int> c(42);
    sparse_series<int> s;

    BOOST_CHECK_EQUAL(0, std::distance(seq::begin(c), seq::end(c)));

    make_ordered_inserter(s)
        (8, 2)
        (6, 4)
        (4, 6)
        (2, 8)
    .commit();

    BOOST_CHECK_EQUAL(4, std::distance(seq::begin(s), seq::end(s)));

    piecewise_constant_series<int> pwc = c + s;
    boost::result_of<seq::op::begin(piecewise_constant_series<int> &)>::type where;

    BOOST_CHECK_RANGE_RUN_EQUAL(
        pwc
      , (42, -inf, 2)(50, 2, 3)(42, 3, 4)(48, 4, 5)(42, 5, 6)(46, 6, 7)(42, 7, 8)(44, 8, 9)(42, 9, inf)
    );
}

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

    test->add(BOOST_TEST_CASE(&test_constant_series));
    test->add(BOOST_TEST_CASE(&test_constant_series_2));

    return test;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  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)

//! \file
//!\brief Tests for the nextafter function of (fixed_point).

#include <cmath>

#define BOOST_TEST_MODULE test_negatable_func_nextafter
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(test_negatable_func_nextafter)
{
  typedef boost::fixed_point::negatable<7, -8> fixed_point_type;

  const fixed_point_type increment = ldexp(fixed_point_type(1), -8);

  // Check positive argument.
  fixed_point_type x(0);
  const fixed_point_type y = fixed_point_type(1) / 10;

  x = nextafter(x, y);

  BOOST_CHECK_EQUAL(x, increment);

  // Check negative argument.
  x = nextafter(fixed_point_type(0), -y);

  BOOST_CHECK_EQUAL(x, -increment);

  // Check zero argument.
  x = nextafter(fixed_point_type(0), fixed_point_type(0));

  BOOST_CHECK_EQUAL(x, fixed_point_type(0));

  // Check identical arguments.
  x = nextafter(fixed_point_type(0.25F), fixed_point_type(0.25F));

  BOOST_CHECK_EQUAL(x, fixed_point_type(0.25F));

  x = nextafter(fixed_point_type(-0.25F), fixed_point_type(-0.25F));

  BOOST_CHECK_EQUAL(x, fixed_point_type(-0.25F));

  // Check the prior to maximum argument.
  x = nextafter((std::numeric_limits<fixed_point_type>::max)() - fixed_point_type(increment),
                (std::numeric_limits<fixed_point_type>::max)());

  // Check the maximum argument, returns max.
  BOOST_CHECK_EQUAL(x, (std::numeric_limits<fixed_point_type>::max)());
}


/*
@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 = _ + _;
    BOOST_HANA_CONSTEXPR_CHECK(plus(1, 2) == 1 + 2);

    BOOST_HANA_CONSTEXPR_LAMBDA auto increment = _ + 1;
    BOOST_HANA_CONSTEXPR_CHECK(increment(1) == 2);

    BOOST_HANA_CONSTEXPR_LAMBDA auto double_ = 2 * _;
    BOOST_HANA_CONSTEXPR_CHECK(double_(1) == 2);

    // Extra arguments are ignored.
    BOOST_HANA_CONSTEXPR_CHECK(double_(1, "ignored") == 2);
    //! [main]
}


#include <boost/multiprecision/cpp_int.hpp>
#include <boost/lexical_cast.hpp>
#include <cmath>
#include <iostream>

// Requires Boost 1.53 or higher (for multiprecision)
// How long does it take one core to count to a certain number of bits?
// To build: g++ 128.cpp -o count -Wall -Wextra
// To use: time ./count number

int main(int argc, char * argv[])
{
	const std::string usage = "usage: time ./count number\n";

	if( argc != 2 )
	{
		std::cerr << usage;
		return 1;
	}

	namespace mp = boost::multiprecision;

	int bits = boost::lexical_cast<int>(argv[1]);

	mp::uint512_t c(pow(2, bits));
	mp::uint512_t d(0);

	std::cout << "Counting to: " << c << "\n";

        while ( d != c )
	{
		++d;
	}

	return 0;
}


#include "CALPHADFreeEnergyFunctionsBinary.h"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

namespace pt = boost::property_tree;

int main(int argc, char* argv[])
{
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::LINEAR;

    double temperature = 1450.;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadAuNi.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinary cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    const int npts = 100;
    std::ofstream os("FvsC.dat", std::ios::out);

    std::cout << " Compute energies..." << std::endl;
    cafe.printEnergyVsComposition(temperature, os, npts);
}


//  (C) Copyright Eric Niebler 2008.
//  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/mpl/assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_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<
            tag::rolling_mean
        >
    > acc(tag::rolling_mean::window_size = 5), test_acc(tag::rolling_mean::window_size = 5, sample = 0);

    acc(1);
    BOOST_CHECK_CLOSE(1., rolling_mean(acc), 1e-5);

    acc(2);
    BOOST_CHECK_CLOSE(1.5, rolling_mean(acc), 1e-5);

    acc(3);
    BOOST_CHECK_CLOSE(2., rolling_mean(acc), 1e-5);

    acc(4);
    BOOST_CHECK_CLOSE(2.5, rolling_mean(acc), 1e-5);

    acc(5);
    BOOST_CHECK_CLOSE(3., rolling_mean(acc), 1e-5);

    acc(6);
    BOOST_CHECK_CLOSE(4., rolling_mean(acc), 1e-5);

    acc(7);
    BOOST_CHECK_CLOSE(5., rolling_mean(acc), 1e-5);

    assert_is_double(rolling_mean(acc));
}

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

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

    return test;
}


/*
 * testCSP.cpp
 * @brief develop code for CSP solver
 * @date Feb 5, 2012
 * @author Frank Dellaert
 */

#include <gtsam_unstable/discrete/CSP.h>
#include <gtsam_unstable/discrete/Domain.h>

#include <boost/assign/std/map.hpp>
using boost::assign::insert;
#include <CppUnitLite/TestHarness.h>

#include <fstream>
#include <iostream>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST_UNSAFE(BinaryAllDif, allInOne) {
  // Create keys and ordering
  size_t nrColors = 2;
  //  DiscreteKey ID("Idaho", nrColors), UT("Utah", nrColors), AZ("Arizona",
  //  nrColors);
  DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors);

  // Check construction and conversion
  BinaryAllDiff c1(ID, UT);
  DecisionTreeFactor f1(ID & UT, "0 1 1 0");
  EXPECT(assert_equal(f1, c1.toDecisionTreeFactor()));

  // Check construction and conversion
  BinaryAllDiff c2(UT, AZ);
  DecisionTreeFactor f2(UT & AZ, "0 1 1 0");
  EXPECT(assert_equal(f2, c2.toDecisionTreeFactor()));

  DecisionTreeFactor f3 = f1 * f2;
  EXPECT(assert_equal(f3, c1 * f2));
  EXPECT(assert_equal(f3, c2 * f1));
}

/* ************************************************************************* */
TEST_UNSAFE(CSP, allInOne) {
  // Create keys and ordering
  size_t nrColors = 2;
  DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors);

  // Create the CSP
  CSP csp;
  csp.addAllDiff(ID, UT);
  csp.addAllDiff(UT, AZ);

  // Check an invalid combination, with ID==UT==AZ all same color
  DiscreteFactor::Values invalid;
  invalid[ID.first] = 0;
  invalid[UT.first] = 0;
  invalid[AZ.first] = 0;
  EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9);

  // Check a valid combination
  DiscreteFactor::Values valid;
  valid[ID.first] = 0;
  valid[UT.first] = 1;
  valid[AZ.first] = 0;
  EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9);

  // Just for fun, create the product and check it
  DecisionTreeFactor product = csp.product();
  // product.dot("product");
  DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0");
  EXPECT(assert_equal(expectedProduct, product));

  // Solve
  CSP::sharedValues mpe = csp.optimalAssignment();
  CSP::Values expected;
  insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1);
  EXPECT(assert_equal(expected, *mpe));
  EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);
}

/* ************************************************************************* */
TEST_UNSAFE(CSP, WesternUS) {
  // Create keys
  size_t nrColors = 4;
  DiscreteKey
      // Create ordering according to example in ND-CSP.lyx
      WA(0, nrColors),
      OR(3, nrColors), CA(1, nrColors), NV(2, nrColors), ID(8, nrColors),
      UT(9, nrColors), AZ(10, nrColors), MT(4, nrColors), WY(5, nrColors),
      CO(7, nrColors), NM(6, nrColors);

  // Create the CSP
  CSP csp;
  csp.addAllDiff(WA, ID);
  csp.addAllDiff(WA, OR);
  csp.addAllDiff(OR, ID);
  csp.addAllDiff(OR, CA);
  csp.addAllDiff(OR, NV);
  csp.addAllDiff(CA, NV);
  csp.addAllDiff(CA, AZ);
  csp.addAllDiff(ID, MT);
  csp.addAllDiff(ID, WY);
  csp.addAllDiff(ID, UT);
  csp.addAllDiff(ID, NV);
  csp.addAllDiff(NV, UT);
  csp.addAllDiff(NV, AZ);
  csp.addAllDiff(UT, WY);
  csp.addAllDiff(UT, CO);
  csp.addAllDiff(UT, NM);
  csp.addAllDiff(UT, AZ);
  csp.addAllDiff(AZ, CO);
  csp.addAllDiff(AZ, NM);
  csp.addAllDiff(MT, WY);
  csp.addAllDiff(WY, CO);
  csp.addAllDiff(CO, NM);

  // Solve
  Ordering ordering;
  ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7),
      Key(8), Key(9), Key(10);
  CSP::sharedValues mpe = csp.optimalAssignment(ordering);
  // GTSAM_PRINT(*mpe);
  CSP::Values expected;
  insert(expected)(WA.first, 1)(CA.first, 1)(NV.first, 3)(OR.first, 0)(
      MT.first, 1)(WY.first, 0)(NM.first, 3)(CO.first, 2)(ID.first, 2)(
      UT.first, 1)(AZ.first, 0);

  // TODO: Fix me! mpe result seems to be right. (See the printing)
  // It has the same prob as the expected solution.
  // Is mpe another solution, or the expected solution is unique???
  EXPECT(assert_equal(expected, *mpe));
  EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);

  // Write out the dual graph for hmetis
#ifdef DUAL
  VariableIndexOrdered index(csp);
  index.print("index");
  ofstream os("/Users/dellaert/src/hmetis-1.5-osx-i686/US-West-dual.txt");
  index.outputMetisFormat(os);
#endif
}

/* ************************************************************************* */
TEST_UNSAFE(CSP, AllDiff) {
  // Create keys and ordering
  size_t nrColors = 3;
  DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors);

  // Create the CSP
  CSP csp;
  vector<DiscreteKey> dkeys;
  dkeys += ID, UT, AZ;
  csp.addAllDiff(dkeys);
  csp.addSingleValue(AZ, 2);
  //  GTSAM_PRINT(csp);

  // Check construction and conversion
  SingleValue s(AZ, 2);
  DecisionTreeFactor f1(AZ, "0 0 1");
  EXPECT(assert_equal(f1, s.toDecisionTreeFactor()));

  // Check construction and conversion
  AllDiff alldiff(dkeys);
  DecisionTreeFactor actual = alldiff.toDecisionTreeFactor();
  //  GTSAM_PRINT(actual);
  //  actual.dot("actual");
  DecisionTreeFactor f2(
      ID & AZ & UT,
      "0 0 0  0 0 1  0 1 0   0 0 1  0 0 0  1 0 0   0 1 0  1 0 0  0 0 0");
  EXPECT(assert_equal(f2, actual));

  // Check an invalid combination, with ID==UT==AZ all same color
  DiscreteFactor::Values invalid;
  invalid[ID.first] = 0;
  invalid[UT.first] = 1;
  invalid[AZ.first] = 0;
  EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9);

  // Check a valid combination
  DiscreteFactor::Values valid;
  valid[ID.first] = 0;
  valid[UT.first] = 1;
  valid[AZ.first] = 2;
  EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9);

  // Solve
  CSP::sharedValues mpe = csp.optimalAssignment();
  CSP::Values expected;
  insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2);
  EXPECT(assert_equal(expected, *mpe));
  EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9);

  // Arc-consistency
  vector<Domain> domains;
  domains += Domain(ID), Domain(AZ), Domain(UT);
  SingleValue singleValue(AZ, 2);
  EXPECT(singleValue.ensureArcConsistency(1, domains));
  EXPECT(alldiff.ensureArcConsistency(0, domains));
  EXPECT(!alldiff.ensureArcConsistency(1, domains));
  EXPECT(alldiff.ensureArcConsistency(2, domains));
  LONGS_EQUAL(2, domains[0].nrValues());
  LONGS_EQUAL(1, domains[1].nrValues());
  LONGS_EQUAL(2, domains[2].nrValues());

  // Parial application, version 1
  DiscreteFactor::Values known;
  known[AZ.first] = 2;
  DiscreteFactor::shared_ptr reduced1 = alldiff.partiallyApply(known);
  DecisionTreeFactor f3(ID & UT, "0 1 1  1 0 1  1 1 0");
  EXPECT(assert_equal(f3, reduced1->toDecisionTreeFactor()));
  DiscreteFactor::shared_ptr reduced2 = singleValue.partiallyApply(known);
  DecisionTreeFactor f4(AZ, "0 0 1");
  EXPECT(assert_equal(f4, reduced2->toDecisionTreeFactor()));

  // Parial application, version 2
  DiscreteFactor::shared_ptr reduced3 = alldiff.partiallyApply(domains);
  EXPECT(assert_equal(f3, reduced3->toDecisionTreeFactor()));
  DiscreteFactor::shared_ptr reduced4 = singleValue.partiallyApply(domains);
  EXPECT(assert_equal(f4, reduced4->toDecisionTreeFactor()));

  // full arc-consistency test
  csp.runArcConsistency(nrColors);
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#include <boost/math/tools/roots.hpp>
#include <ibs>
#include <ste>
//#include <boost/math/tools/roots.hpp>
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <map>
#include <math.h>
#include <string>

template <class T> struct synchronousPhaseFunctor {
  synchronousPhaseFunctor(T const &target, std::vector<double> &voltages,
                          std::vector<double> &harmonicNumbers, float charge)
      : U0(target), volts(voltages), hs(harmonicNumbers), ch(charge) {}
  std::tuple<double, double> operator()(T const &phi) {
    T volt1, volt2, volt3;
    T dvolt1, dvolt2, dvolt3;
    volt1 = ch * volts[0] * sin(phi);
    volt2 = ch * volts[1] * sin((hs[1] / hs[0]) * phi);
    volt3 = ch * volts[2] * sin((hs[2] / hs[0]) * phi);

    dvolt1 = ch * volts[0] * cos(phi);
    dvolt2 = ch * volts[1] * (hs[1] / hs[0]) * cos((hs[1] / hs[0]) * phi);
    dvolt3 = ch * volts[2] * (hs[2] / hs[0]) * cos((hs[2] / hs[0]) * phi);
    std::tuple<double, double> out = {volt1 + volt2 + volt3 - U0,
                                      dvolt1 + dvolt2 + dvolt3};
    return out;
  }

private:
  T U0;
  std::vector<double> volts;
  std::vector<double> hs;
  double ch;
};

template <class T>
T synchronousPhaseFunctorDeriv(T x, std::vector<double> &voltages,
                               std::vector<double> &harmnumbers, double charge,
                               T guess, T min, T max) {
  // return cube root of x using 1st derivative and Newton_Raphson.
  using namespace boost::math::tools;

  const int digits =
      std::numeric_limits<T>::digits; // Maximum possible binary digits accuracy
                                      // for type T.
  int get_digits = static_cast<int>(
      digits * 0.6); // Accuracy doubles with each step, so stop when we have
                     // just over half the digits correct.
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = newton_raphson_iterate(
      synchronousPhaseFunctor<T>(x, voltages, harmnumbers, charge), guess, min,
      max, get_digits, it);
  return result;
}
double pcoeff(double voltage, double angularFreq, double p0,
              double betaRelativistic, double charge) {
  return (angularFreq * voltage * charge) / (2.0 * pi * p0 * betaRelativistic);
}

double HamiltonianTripleRf(double tcoeff, std::vector<double> &voltages,
                           std::vector<double> &harmonicNumbers,
                           double phiSynchronous, double t, double delta,
                           double omega0, double p0, double betaRelativistic,
                           double charge) {
  double kinetic, potential1, potential2, potential3;
  kinetic = 0.5 * tcoeff * pow(delta, 2);

  std::printf("%-30s %16.8e\n", "kinetic", kinetic);
  potential1 = pcoeff(voltages[0], omega0, p0, betaRelativistic, charge) *
               (cos(harmonicNumbers[0] * omega0 * t) - cos(phiSynchronous) +
                (harmonicNumbers[0] * omega0 * t - phiSynchronous) *
                    sin(phiSynchronous));
  std::printf("%-30s %16.8e\n", "pcoeff0",
              pcoeff(voltages[0], omega0, p0, betaRelativistic, charge));
  std::printf("%-30s %16.8e\n", "phase0", harmonicNumbers[0] * omega0 * t);
  std::printf("%-30s %16.8e\n", "potential1", potential1);

  potential2 =
      pcoeff(voltages[1], omega0, p0, betaRelativistic, charge) *
      (harmonicNumbers[0] / harmonicNumbers[1]) *
      (cos(harmonicNumbers[1] * omega0 * t) -
       cos(harmonicNumbers[1] * phiSynchronous / harmonicNumbers[0]) +
       (harmonicNumbers[1] * omega0 * t -
        harmonicNumbers[1] * phiSynchronous / harmonicNumbers[0]) *
           sin(harmonicNumbers[1] * phiSynchronous / harmonicNumbers[0]));

  potential3 =
      pcoeff(voltages[2], omega0, p0, betaRelativistic, charge) *
      (harmonicNumbers[0] / harmonicNumbers[2]) *
      (cos(harmonicNumbers[2] * omega0 * t) -
       cos(harmonicNumbers[2] * phiSynchronous / harmonicNumbers[0]) +
       (harmonicNumbers[2] * omega0 * t -
        harmonicNumbers[2] * phiSynchronous / harmonicNumbers[0]) *
           sin(harmonicNumbers[2] * phiSynchronous / harmonicNumbers[0]));

  return kinetic + potential1 + potential2 + potential3;
};

double synchrotronTune(std::map<string, double> &twissheadermap,
                       std::vector<double> h, std::vector<double> v) {
  double p0 = twissheadermap["PC"] * 1.0e9;
  double phis = twissheadermap["phis"];
  double charge = twissheadermap["CHARGE"];
  double Omega2 =
      (h[0] * twissheadermap["eta"] * charge) / (2.0 * pi * p0) *
      (v[0] * cos(phis) + v[1] * (h[1] / h[0]) * cos((h[1] / h[0]) * phis) +
       v[2] * (h[2] / h[0]) * cos((h[2] / h[0]) * phis));
  return sqrt(abs(Omega2));
};

int main() {
  string twissfilename = "../src/b2_design_lattice_1996.twiss";
  map<string, double> twissheadermap;
  twissheadermap = GetTwissHeader(twissfilename);

  // rf settings
  std::vector<double> h, v;
  h.push_back(400.0);
  v.push_back(-1.5e6);

  // bunch length
  double sigs = 0.005;
  // aatom
  double aatom = emass / pmass;
  // set energy loss per turn manually
  // TODO: implement radiation update of twiss
  twissheadermap["U0"] = 174e3;
  // update twiss header with long parameters
  ste_longitudinal::updateTwissHeaderLong(twissheadermap, h, v, aatom, sigs);

  double energyLostPerTurn = 174000;    // radation losses per turn per particle
  double acceleratorLength = 240.00839; // length in meter
  double gammar = twissheadermap["GAMMA"]; // relativistic gamma
  // double eta = 0.0007038773471 -
  //          1 / pow(gammar, 2); // slip factor approx alpha - 1/ gammar**2
  double betar = twissheadermap["betar"]; // relativistic beta
  double trev = twissheadermap["trev"];
  double h0 = 400.0;
  double h1 = 400; // 1200.0;
  double h2 = 400; // 1400.0;
  double v0 = -1.5e6;
  double v1 = 0.0; // 20.0e6;
  double v2 = 0.0; // 17.14e6;
  double omega0 = (2 * pi) / trev;
  double p0 = 1.7e9;
  double charge = -1.0;
  std::vector<double> hnumbers = {h0, h1, h2};
  std::vector<double> voltages = {v0, v1, v2};

  double search1 =
      trev * h0 * omega0 /
      (8 * max(max(h0, h1), h2)); // give positive offset to find upstream
                                  // root and not downstream root
  double search2 = trev * h0 * omega0 / (8 * max(max(h0, h1), h2)) -
                   trev * h0 * omega0 / min(min(h0, h1), h2);
  double searchWidth = trev * h0 * omega0 / (2 * max(max(h0, h1), h2));
  ste_output::cyan();
  std::printf("%-30s : %16.6f\n", "Search1", search1 / pi * 180);
  std::printf("%-30s : %16.6f\n", "Search2", search2 / pi * 180);
  std::printf("%-30s : %16.6f\n", "Width", searchWidth / pi * 180);
  ste_output::reset();

  double synchronousPhase0 = synchronousPhaseFunctorDeriv(
      energyLostPerTurn, voltages, hnumbers, charge, search1,
      search1 - searchWidth, search1 + searchWidth);
  double synchronousPhase1 = synchronousPhaseFunctorDeriv(
      energyLostPerTurn, voltages, hnumbers, charge, search2,
      search2 - searchWidth, search2 + searchWidth);

  ste_output::blue();
  std::printf("%-30s : %16.6f %16.6f %16.6e \n", "synchronous phase", search1,
              synchronousPhase0 / pi * 180, synchronousPhase0 / (h0 * omega0));
  std::printf("%-30s : %16.6f %16.6f %16.6e \n", "synchronous phase", search2,
              synchronousPhase1 / pi * 180, synchronousPhase1 / (h0 * omega0));
  ste_output::yellow();
  std::printf("%-30s : %16.6f %16.6f %16.6e\n", "synchronous phase", 173.0,
              twissheadermap["phis"],
              twissheadermap["phis"] / 180.0 * pi / (h0 * omega0));
  ste_output::reset();

  std::cout << "Find next extremum of Hamiltonian" << std::endl;

  double synchronousPhase0Next = synchronousPhaseFunctorDeriv(
      energyLostPerTurn, voltages, hnumbers, charge, search1 + searchWidth,
      search1 + searchWidth / 2, search1 + 2 * searchWidth);
  double synchronousPhase1Next = synchronousPhaseFunctorDeriv(
      energyLostPerTurn, voltages, hnumbers, charge, search2 + searchWidth,
      search2 + searchWidth / 2, search2 + 2 * searchWidth);

  ste_output::blue();
  std::printf("%-30s : %16.6f %16.6f %16.6e \n", "synchronous phase next",
              search1 + searchWidth, synchronousPhase0Next / pi * 180,
              synchronousPhase0Next / (h0 * omega0));
  std::printf("%-30s : %16.6f %16.6f %16.6e \n", "synchronous phase next ",
              search2 + searchWidth, synchronousPhase1Next / pi * 180,
              synchronousPhase1Next / (h0 * omega0));
  ste_output::yellow();
  std::printf("%-30s : %16.6f\n", "synchronous phase", twissheadermap["phis"]);
  ste_output::reset();

  std::printf("%s", "\nHamiltonians\n");
  ste_output::blue();
  double tc = ste_longitudinal::tcoeff(twissheadermap, h0);
  double tcval = tc / (h0 * omega0);
  double ohammax = HamiltonianTripleRf(
      tcval, voltages, hnumbers, synchronousPhase1,
      synchronousPhase1Next / (h0 * omega0), 0.0, omega0, p0, betar, charge);
  double hammax =
      ste_longitudinal::Hamiltonian(twissheadermap, hnumbers, voltages, tc,
                                    synchronousPhase1Next / (h0 * omega0), 0.0);
  std::printf("%-30s : %16.6f \n", "tcoeff", tcval);
  std::printf("%-30s : %16.6f \n", "ohammax", ohammax);
  ste_output::yellow();
  std::printf("%-30s : %16.6f \n", "hammax", hammax);
  ste_output::reset();

  std::cout << "Synchrotron tune : "
            << synchrotronTune(twissheadermap, hnumbers, voltages) << std::endl;
  std::cout << "Synchrotron tune (Hz): "
            << synchrotronTune(twissheadermap, hnumbers, voltages) * omega0 /
                   (2.0 * pi)
            << std::endl;
  std::printf("%-30s : %16.6f \n", "syncTune",
              synchrotronTune(twissheadermap, hnumbers, voltages));
  std::printf("%-30s : %16.6f \n", "syncTune Hz",
              synchrotronTune(twissheadermap, hnumbers, voltages) * omega0 /
                  (2.0 * pi));
  ste_output::yellow();
  std::printf("%-30s : %16.6f \n", "syncTune", twissheadermap["qs"]);
  std::printf("%-30s : %16.6f \n", "syncTune Hz",
              twissheadermap["qs"] * omega0 / (2.0 * pi));
  ste_output::reset();

  std::printf("%16.6e  %16.6e %16.6e %16.6e %16.6e\n", synchronousPhase0Next,
              synchronousPhase0,
              abs((synchronousPhase0Next - synchronousPhase0) / pi * 180.0), h0,
              omega0);
  std::printf("%-30s : %16.6e \n", "tauhat",
              abs((synchronousPhase0Next - synchronousPhase0) / (h0 * omega0)));
  ste_output::yellow();
  std::printf("%-30s : %16.6e \n", "tauhat", twissheadermap["tauhat"]);
  std::printf("%-30s : %16.6e \n", "tauhat",
              (twissheadermap["phis"] - (twissheadermap["phis"] - 180.0)) /
                  180.0 * pi / (h0 * omega0));
  ste_output::reset();
  return 0;
}


// File: vector2.cpp

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

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

    // Define dense vector of complex with 7 elements.
    dense_vector<std::complex<float>, mtl::vec::parameters<tag::row_major> >  v(7);

    // Set all elements to 3+2i
    v= std::complex<float>(3.0, 2.0);
    std::cout << "v is " << v << "\n";

    // Set all elements to 5+0i
    v= 5.0;
    std::cout << "v is " << v << "\n";

    v= 6;
    std::cout << "v is " << v << "\n";

    return 0;
}


#include <eigen3/Eigen/Dense>
#include <iostream>
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/weighted_median.hpp>
using boost::bind;
using boost::ref;
using namespace boost::accumulators;
using namespace Eigen;

void f(int i) { std::cout << i << std::endl; }

int main(){
    accumulator_set<float, stats<tag::median > > acc1;
    accumulator_set<float, stats<tag::median >,int> acc2;

    int i = 0;
    VectorXi rw=VectorXi::Random(100);
    VectorXf rn=VectorXf::Random(100);

    std::for_each(rn.data(), rn.data()+rn.rows(), bind<void>( ref(acc1), _1 ) );
    //std::for_each(i, 5, f); // no clue what was meant here...
#if 0
    rw=rw.cwiseAbs();
    rw(0)=1000;
    for(int i=0;i<100;i++){
        acc1(rn(i));
        acc2(rn(i),weight=rw(i));
    }
#endif

  std::cout << "         Median: " << median(acc1) << std::endl;
  std::cout << "Weighted Median: " << median(acc2) << std::endl;

  return 0;
}