//***********************************************************
// 07/03/2019	1.0.0	Rémi Saint-Amant   Creation
//***********************************************************
#include "LaricobiusNigrinusModel.h"
#include "ModelBase/EntryPoint.h"
#include "Basic\DegreeDays.h"
#include <boost/math/distributions/weibull.hpp>
#include <boost/math/distributions/beta.hpp>
#include <boost/math/distributions/Rayleigh.hpp>
#include <boost/math/distributions/logistic.hpp>
#include <boost/math/distributions/exponential.hpp>

using namespace WBSF::HOURLY_DATA;
using namespace WBSF::LNF;
using namespace std;

//static const bool BEGIN_NOVEMBER = false;
//static const size_t FIRST_Y = BEGIN_NOVEMBER ? 1 : 0;

namespace WBSF
{
	static const CDegreeDays::TDailyMethod DD_METHOD = CDegreeDays::MODIFIED_ALLEN_WAVE;
	enum { ACTUAL_CDD, DATE_DD717, DIFF_DAY, NB_OUTPUTS };

	//this line link this model with the EntryPoint of the DLL
	static const bool bRegistred =
		CModelFactory::RegisterModel(CLaricobiusNigrinusModel::CreateObject);

	CLaricobiusNigrinusModel::CLaricobiusNigrinusModel()
	{
		//NB_INPUT_PARAMETER is used to determine if the dll
		//uses the same number of parameters than the model interface
		NB_INPUT_PARAMETER = -1;
		VERSION = "1.0.2 (2020)";

		//		m_start = CTRef(YEAR_NOT_INIT, JANUARY, DAY_01);
			//	m_threshold = 5.6;
				//m_sumDD = 540;

		m_bCumul = false;
		for (size_t s = 0; s < NB_STAGES; s++)
		{
			for (size_t p = 0; p < NB_RDR_PARAMS; p++)
			{
				m_RDR[s][p] = CLaricobiusNigrinusEquations::RDR[s][p];
			}
		}

		for (size_t p = 0; p < NB_OVP_PARAMS; p++)
			m_OVP[p] = CLaricobiusNigrinusEquations::OVP[p];

		for (size_t p = 0; p < NB_ADE_PARAMS; p++)
			m_ADE[p] = CLaricobiusNigrinusEquations::ADE[p];

		for (size_t p = 0; p < NB_EAS_PARAMS; p++)
			m_EAS[p] = CLaricobiusNigrinusEquations::EAS[p];
	}

	CLaricobiusNigrinusModel::~CLaricobiusNigrinusModel()
	{
	}

	//this method is call to load your parameter in your variable
	ERMsg CLaricobiusNigrinusModel::ProcessParameters(const CParameterVector& parameters)
	{
		ERMsg msg;

		size_t c = 0;

		m_bCumul = parameters[c++].GetBool();

		if (parameters.size() == 1 + NB_STAGES * NB_RDR_PARAMS + NB_OVP_PARAMS + NB_ADE_PARAMS + NB_EAS_PARAMS)
		{
			for (size_t s = 0; s < NB_STAGES; s++)
			{
				for (size_t p = 0; p < NB_RDR_PARAMS; p++)
				{
					m_RDR[s][p] = parameters[c++].GetFloat();
				}
			}

			for (size_t p = 0; p < NB_OVP_PARAMS; p++)
				m_OVP[p] = parameters[c++].GetFloat();

			for (size_t p = 0; p < NB_ADE_PARAMS; p++)
				m_ADE[p] = parameters[c++].GetFloat();

			for (size_t p = 0; p < NB_EAS_PARAMS; p++)
				m_EAS[p] = parameters[c++].GetFloat();

		}

		return msg;
	}

	/*ERMsg CLaricobiusNigrinusModel::OnExecuteAnnual()
	{
		_ASSERTE(m_weather.size() > 1);

		ERMsg msg;
		CTRef today = CTRef::GetCurrentTRef();

		CTPeriod outputPeriod = m_weather.GetEntireTPeriod(CTM::ANNUAL);
		m_output.Init(outputPeriod, NB_OUTPUTS);

		for (size_t y = 0; y < m_weather.GetNbYears(); y++)
		{
			int year = m_weather[y].GetTRef().GetYear();
			CTRef begin = CTRef(year, m_start.GetMonth(), m_start.GetDay());
			CTRef end = CTRef(year, DECEMBER, DAY_31);

			double CDD = 0;

			CTRef day717;
			double actualCDD = -999;
			CDegreeDays DD(DD_METHOD, m_threshold);

			for (CTRef d = begin; d < end; d++)
			{
				CDD += DD.GetDD(m_weather.GetDay(d));
				if (CDD >= m_sumDD && !day717.IsInit())
					day717 = d;

				if (d.as(CTM(CTM::DAILY, CTM::OVERALL_YEARS)) == today.as(CTM(CTM::DAILY, CTM::OVERALL_YEARS)))
					actualCDD = CDD;
			}

			m_output[y][ACTUAL_CDD] = actualCDD;
			if (day717.IsInit())
			{
				m_output[y][DATE_DD717] = day717.GetRef();
				m_output[y][DIFF_DAY] = (int)day717.GetJDay() - (int)today.GetJDay();
			}
		}

		return msg;
	}*/

	//This method is called to compute the solution
	ERMsg CLaricobiusNigrinusModel::OnExecuteDaily()
	{
		ERMsg msg;

		/*	if (m_weather.GetNbYears() < 2)
			{
				msg.ajoute("Laricobius nigrinus model need at least 2 years of data");
				return msg;
			}*/

		if (!m_weather.IsHourly())
			m_weather.ComputeHourlyVariables();

		//This is where the model is actually executed
		CTPeriod p = m_weather.GetEntireTPeriod(CTM(CTM::DAILY));
		m_output.Init(p, NB_STATS, 0);

		//we simulate 2 years at a time.
		//we also manager the possibility to have only one year
		for (size_t y = 0; y < m_weather.size(); y++)
		{
			ExecuteDaily(m_weather[y].GetTRef().GetYear(), m_weather, m_output);
		}

		return msg;
	}

	void CLaricobiusNigrinusModel::ExecuteDaily(int year, const CWeatherYears& weather, CModelStatVector& output)
	{
		//Create stand
		CLNFStand stand(this, m_OVP[Τᴴ¹], m_OVP[Τᴴ²]);

		//Set parameters to equation
		for (size_t s = 0; s < NB_STAGES; s++)
		{
			for (size_t p = 0; p < NB_RDR_PARAMS; p++)
			{
				stand.m_equations.m_RDR[s][p] = m_RDR[s][p];
			}
		}

		for (size_t p = 0; p < NB_OVP_PARAMS; p++)
			stand.m_equations.m_OVP[p] = m_OVP[p];

		for (size_t p = 0; p < NB_ADE_PARAMS; p++)
			stand.m_equations.m_ADE[p] = m_ADE[p];

		for (size_t p = 0; p < NB_EAS_PARAMS; p++)
			stand.m_equations.m_EAS[p] = m_EAS[p];

		stand.init(year, weather);

		//compute 30 days avg
		//stand.ComputeTavg30(year, weather);

		//Create host
		CLNFHostPtr pHost(new CLNFHost(&stand));

		pHost->m_nbMinObjects = 10;
		pHost->m_nbMaxObjects = 1000;

		pHost->Initialize<CLaricobiusNigrinus>(CInitialPopulation(CTRef(year, JANUARY, DAY_01), 0, 400, 100, -1));

		//add host to stand
		stand.m_host.push_front(pHost);

		CTPeriod p = weather[year].GetEntireTPeriod(CTM(CTM::DAILY));

		//if have other year extend period to February
		//ASSERT(weather[year].HavePrevious());
		//if (BEGIN_NOVEMBER)
		//{
		//	p.Begin() = CTRef(year - 1, NOVEMBER, DAY_01);
		//}

		//if have other year extend period to February
		//if (weather[year].HavePrevious())
			//p.Begin() = CTRef(year - 1, JULY, DAY_01);

		//if have other year extend period to February
		if (weather[year].HaveNext())
			p.End() = CTRef(year + 1, JUNE, DAY_30);

		for (CTRef d = p.Begin(); d <= p.End(); d++)
		{
			/*if (d == CTRef(year + 1, JANUARY, DAY_01))
			{
				int gg;
					gg=0;
			}*/

			stand.Live(weather.GetDay(d));
			if (output.IsInside(d))
				stand.GetStat(d, output[d]);

			stand.AdjustPopulation();
			HxGridTestConnection();
		}

		if (m_bCumul)
		{
			//cumulative result
			for (size_t s = S_EGG; s < S_ACTIVE_ADULT; s++)
			{
				CTPeriod p = weather[year].GetEntireTPeriod(CTM(CTM::DAILY));

				CStatistic stat = output.GetStat(s, p);
				if (stat.IsInit() && stat[SUM] > 0)
				{
					output[0][s] = output[0][s] * 100 / stat[SUM];//when first day is not 0
					for (CTRef d = p.Begin() + 1; d <= p.End(); d++)
					{
						output[d][s] = output[d - 1][s] + output[d][s] * 100 / stat[SUM];
						_ASSERTE(!_isnan(output[d][s]));
					}
				}
			}

			/*for (size_t s = S_M_EGG; s <= S_M_DEAD_ADULT; s++)
			{
				CTPeriod p = weather[year].GetEntireTPeriod(CTM(CTM::DAILY));
				if (s >= S_M_ACTIVE_ADULT)
				{
					p.Begin() = CTRef(year, JULY, DAY_01);
					if (weather[year].HaveNext())
						p.End() = CTRef(year + 1, JUNE, DAY_30);
				}

				CStatistic stat = output.GetStat(s, p);
				if (stat.IsInit() && stat[SUM] > 0)
				{
					for (CTRef d = p.Begin() + 1; d <= p.End(); d++)
					{
						if(output.IsInside(d))
							output[d][s] = output[d - 1][s] + output[d][s] * 100 / stat[SUM];
						_ASSERTE(!_isnan(output[d][s]));
					}
				}

			}*/

		}
	}

	void CLaricobiusNigrinusModel::AddDailyResult(const StringVector& header, const StringVector& data)
	{
		ASSERT(data.size() == 5);

		CSAResult obs;
		//if (data[0] != "BlacksburgLab" && data[0] != "VictoriaLab")
		//if (data[0] != "VictoriaLab")
		//{
		CStatistic egg_creation_date;

		obs.m_ref.FromFormatedString(data[1]);
		obs.m_obs.resize(NB_INPUTS);
		for (size_t i = 0; i < NB_INPUTS; i++)
		{
			obs.m_obs[i] = stod(data[i + 2]);

			if (i == 0 && obs.m_obs[i] > -999)
				m_egg_creation_date[data[0] + "_" + to_string(obs.m_ref.GetYear())] += obs.m_ref.GetJDay();

			//if (i == 1 && obs.m_obs[i] <= -999 && stod(data[i + 5]) > -999)
				//obs.m_obs[i] = stod(data[i + 5]);//second method

			if (obs.m_obs[i] > -999)
			{
				m_nb_days[i] += obs.m_ref.GetJDay();
				m_years[i].insert(obs.m_ref.GetYear());
			}
		}

		m_SAResult.push_back(obs);

		//}

	}

	double GetSimX(size_t s, CTRef TRefO, double obs, const CModelStatVector& output)
	{
		double x = -999;

		if (obs > -999)
		{
			//if (obs > 0.01 && obs < 99.99)
			if (obs >= 100)
				obs = 99.99;//to avoid some problem of truncation

			long index = output.GetFirstIndex(s, ">=", obs, 1, CTPeriod(TRefO.GetYear(), FIRST_MONTH, FIRST_DAY, TRefO.GetYear(), LAST_MONTH, LAST_DAY));
			if (index >= 1)
			{
				double obsX1 = output.GetFirstTRef().GetJDay() + index;
				double obsX2 = output.GetFirstTRef().GetJDay() + index + 1;

				double obsY1 = output[index][s];
				double obsY2 = output[index + 1][s];
				if (obsY2 != obsY1)
				{
					double slope = (obsX2 - obsX1) / (obsY2 - obsY1);
					double obsX = obsX1 + (obs - obsY1)*slope;
					ASSERT(!_isnan(obsX) && _finite(obsX));

					x = obsX;
				}
			}
		}

		return x;
	}

	bool CLaricobiusNigrinusModel::IsParamValid()const
	{
		if (m_OVP[Τᴴ¹] >= m_OVP[Τᴴ²])
			return false;

		bool bValid = true;
		for (size_t s = 0; s <= NB_STAGES && bValid; s++)
		{
			if (s == EGG || s == LARVAE /*|| s == AESTIVAL_DIAPAUSE_ADULT*/)
			{
				CStatistic rL;
				for (double Э = 0.01; Э < 0.5; Э += 0.01)
				{
					double r = 1.0 - log((pow(Э, m_RDR[s][Ϙ]) - 1.0) / (pow(0.5, m_RDR[s][Ϙ]) - 1.0)) / m_RDR[s][к];
					if (r >= 0.4 && r <= 2.5)
						rL += 1.0 / r;//reverse for comparison
				}

				CStatistic rH;
				for (double Э = 0.51; Э < 1.0; Э += 0.01)
				{
					double r = 1.0 - log((pow(Э, m_RDR[s][Ϙ]) - 1.0) / (pow(0.5, m_RDR[s][Ϙ]) - 1.0)) / m_RDR[s][к];
					if (r >= 0.4 && r <= 2.5)
						rH += r;
				}

				if (rL.IsInit() && rH.IsInit())
					bValid = fabs(rL[SUM] - rH[SUM]) < 5.3; //in Régnière (2012) obtain a max of 5.3
				else
					bValid = false;
			}
		}

		return bValid;
	}

	void CLaricobiusNigrinusModel::CalibrateDiapauseEndTh(CStatisticXY& stat)
	{
		static const double DiapauseDuration[3][3] =
		{
			{128.1,127.9,134.0},
			{156.7,162.2,166.2},
			{194.8,203.7,-999}
		};

		static const double DiapauseDurationSD[3][3] =
		{
			{2.2,3,	3.8},
			{4.1,4.4,5.4},
			{4.2,3.4,10.8}
		};

		if (m_SAResult.size() != 8)
			return;

		for (size_t t = 0; t < 3; t++)
		{
			for (size_t dl = 0; dl < 3; dl++)
			{
				if (DiapauseDuration[t][dl] > -999)
				{
					//NbVal = 8	Bias = 0.00263	MAE = 0.95222	RMSE = 1.25691	CD = 0.99785	R² = 0.99786
					//lam0 = 15.81011 {  15.80907, 15.81142}	VM = { 0.00021,   0.00060 }
					//lam1 = 2.50857 {   2.50779, 2.50943}	VM = { 0.00021,   0.00073 }
					//lam2 = 6.64395 {   6.63745, 6.64922}	VM = { 0.00113,   0.00379 }
					//lam3 = 7.81911 {   7.80857, 7.82666}	VM = { 0.00183,   0.00492 }
					//lam_a = 0.16346 {   0.16328, 0.16369}	VM = { 0.00006,   0.00019 }
					//lam_b = 0.26484 {   0.26458, 0.26499}	VM = { 0.00007,   0.00020 }

					double T = 10 + 5 * t;
					double DL = 8 + dl * 4;
					double DD = 120.0 + (215.0 - 120.0) * 1 / (1 + exp(-(T - m_ADE[ʎ0]) / m_ADE[ʎ1]));
					double f = exp(-m_ADE[ʎa] + m_ADE[ʎb] * 1 / (1 + exp(-(DL - m_ADE[ʎ2]) / m_ADE[ʎ3])));

					stat.Add(DiapauseDuration[t][dl], DD * f);
				}
			}
		}
	}

	static const int ROUND_VAL = 4;
	CTRef CLaricobiusNigrinusModel::GetDiapauseEnd(const CWeatherYear& weather)
	{
		CTPeriod p = weather.GetEntireTPeriod(CTM(CTM::DAILY));
		//return  p.Begin() + 253;

		double sumDD = 0;
		//for (CTRef TRef = p.Begin()+172; TRef <= p.End()&& TRef<= p.Begin() + int(m_ADE[ʎ0]); TRef++)
		for (CTRef TRef = p.Begin() + int(m_ADE[ʎ0]-1); TRef <= p.End() && TRef <= p.Begin() + int(m_ADE[ʎ1]-1); TRef++)
		{
			//size_t ii = TRef - p.Begin();
			const CWeatherDay& wday = m_weather.GetDay(TRef);
			double T = wday[H_TNTX][MEAN];

			T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);
			//T = Round(max(m_ADE[ʎa], T), ROUND_VAL);

			double DD = min(0.0, T - m_ADE[ʎb]);//DD is negative

			sumDD += DD;
		}

		//boost::math::weibull_distribution<double> begin_dist(-m_ADE[ʎ2], m_ADE[ʎ3]);
//		int begin = (int)Round(m_ADE[ʎ0] + m_ADE[ʎa] * cdf(begin_dist, -sumDD), 0);

		boost::math::logistic_distribution<double> begin_dist(m_ADE[ʎ2], m_ADE[ʎ3]);
		int begin = (int)Round(m_ADE[ʎ1] + m_ADE[ʎa] * cdf(begin_dist, sumDD), 0);
		return  p.Begin() + begin;

	}

	void CLaricobiusNigrinusModel::CalibrateDiapauseEnd(const bitset<3>& test, CStatisticXY& stat)
	{
		for (size_t EVALUATE_STAGE = 0; EVALUATE_STAGE < NB_INPUTS; EVALUATE_STAGE++)
			//for (size_t j = 0; j < NB_INPUTS; j++)
		{
			if (test[EVALUATE_STAGE])
			{

				if (m_OVP[Τᴴ¹] >= m_OVP[Τᴴ²])
					return;

				if (m_SAResult.empty())
					return;

				if (!m_weather.IsHourly())
					m_weather.ComputeHourlyVariables();

				for (size_t y = 0; y < m_weather.GetNbYears(); y++)
				{
					int year = m_weather[y].GetTRef().GetYear();
					if (m_years[EVALUATE_STAGE].find(year) == m_years[EVALUATE_STAGE].end())
						continue;

					double sumDD = 0;
					vector<double> CDD;
					CTPeriod p;

					if (EVALUATE_STAGE == I_EMERGED_ADULT)
					{

						p = m_weather[year].GetEntireTPeriod(CTM(CTM::DAILY));
						CDD.resize(p.size(), 0);

						CTRef diapauseEnd = GetDiapauseEnd(m_weather[year]);
						for (CTRef TRef = diapauseEnd; TRef <= p.End(); TRef++)
						{
							const CWeatherDay& wday = m_weather.GetDay(TRef);
							double T = wday[H_TNTX][MEAN];
							T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);

							//double DD = max(0.0, T - 4.0);//DD is negative
							//T = min(T, m_OVP[Τᴴ²]);
							double DD = max(0.0, T - m_EAS[Τᴴ]);//DD is positive

							sumDD += DD;

							size_t ii = TRef - p.Begin();
							CDD[ii] = sumDD;
						}

					}
					else
					{
						p = m_weather[year].GetEntireTPeriod(CTM(CTM::DAILY));
						//p.Begin() = GetDiapauseEnd(m_weather[year - 1]);

						CDD.resize(p.size(), 0);

						CDegreeDays DDModel(CDegreeDays::MODIFIED_ALLEN_WAVE, m_OVP[Τᴴ¹], m_OVP[Τᴴ²]);

						for (CTRef TRef = p.Begin(); TRef <= p.End(); TRef++)
						{
							const CWeatherDay& wday = m_weather.GetDay(TRef);
							size_t ii = TRef - p.Begin();
							sumDD += DDModel.GetDD(wday);
							CDD[ii] = sumDD;
						}
					}

					for (size_t i = 0; i < m_SAResult.size(); i++)
					{
						size_t ii = m_SAResult[i].m_ref - p.Begin();
						if (m_SAResult[i].m_ref.GetYear() == year && ii < CDD.size())
						{
							double obs_y = m_SAResult[i].m_obs[EVALUATE_STAGE];

							if (obs_y > -999)
							{

								double sim_y = 0;

								if (EVALUATE_STAGE == I_EMERGED_ADULT)
								{
									boost::math::logistic_distribution<double> emerged_dist(m_EAS[μ], m_EAS[ѕ]);
									//boost::math::weibull_distribution<double> emerged_dist(m_EAS[μ], m_EAS[ѕ]);
									sim_y = Round(cdf(emerged_dist, CDD[ii]) * 100, ROUND_VAL);

								}
								else
								{
									boost::math::logistic_distribution<double> create_dist(m_OVP[μ], m_OVP[ѕ]);
									//boost::math::weibull_distribution<double> create_dist(m_OVP[μ], m_OVP[ѕ]);
									sim_y = Round(cdf(create_dist, CDD[ii]) * 100, ROUND_VAL);

								}

								if (sim_y < 0.1)
									sim_y = 0;
								if (sim_y > 99.9)
									sim_y = 100;

								stat.Add(obs_y, sim_y);
							}
						}
					}
				}//for all years
			}//if
		}//for
		return;

	}

	void CLaricobiusNigrinusModel::CalibrateOviposition(CStatisticXY& stat)
	{
		if (m_SAResult.empty())
			return;

		for (size_t y = 0; y < m_weather.GetNbYears(); y++)
		{
			int year = m_weather[y].GetTRef().GetYear();
			string key = m_info.m_loc.m_ID + "_" + to_string(year);

			if (m_egg_creation_date.find(key) == m_egg_creation_date.end())
				continue;

			//CTRef emergingBegin = GetDiapauseEnd(m_weather[year-1]);

			ASSERT(m_weather[year].HavePrevious());

			if (m_weather[year].HavePrevious())
			{
				CStatistic Tmin;
				Tmin += m_weather[year - 1][NOVEMBER][H_TMIN];
				Tmin += m_weather[year - 1][DECEMBER][H_TMIN];
				Tmin += m_weather[year][JANUARY][H_TMIN];
				Tmin += m_weather[year][FEBRUARY][H_TMIN];

				CStatistic Tmean;
				Tmean += m_weather[year - 1][NOVEMBER][H_TNTX];
				Tmean += m_weather[year - 1][DECEMBER][H_TNTX];
				Tmean += m_weather[year][JANUARY][H_TNTX];
				Tmean += m_weather[year][FEBRUARY][H_TNTX];

				double obs = m_egg_creation_date.at(key);
				double sim = m_ADE[ʎa] - m_ADE[ʎb] * 1 / (1 + exp(-(Tmean[MEAN] - m_ADE[ʎ2]) / m_ADE[ʎ3]));

				if (sim < 0.1)
					sim = 0;
				if (sim > 99.9)
					sim = 100;

				stat.Add(obs, sim);
			}

		}
		return;

	}

	void CLaricobiusNigrinusModel::GetFValueDaily(CStatisticXY& stat)
	{
		bitset<3> test;
		test.reset();

		//test.set(I_EGGS);
		//test.set(I_LARVAE);
		test.set(I_EMERGED_ADULT);

		//return CalibrateDiapauseEndTh(stat);
		return CalibrateDiapauseEnd(test, stat);
		//return CalibrateOviposition(stat);

		if (!m_SAResult.empty())
		{
			if (!m_bCumul)
				m_bCumul = true;//SA always cumulative

			if (!m_weather.IsHourly())
				m_weather.ComputeHourlyVariables();

			//low and hi relative development rate must be approximatively the same
			//if (!IsParamValid())
				//return;

			for (size_t y = 0; y < m_weather.GetNbYears(); y++)
			{
				int year = m_weather[y].GetTRef().GetYear();
				if ((test[0] && m_years[I_EGGS].find(year) != m_years[I_EGGS].end()) ||
					(test[1] && m_years[I_LARVAE].find(year) != m_years[I_LARVAE].end()) ||
					(test[2] && m_years[I_EMERGED_ADULT].find(year) != m_years[I_EMERGED_ADULT].end()))
				{

					CModelStatVector output;
					CTPeriod p = m_weather[y].GetEntireTPeriod(CTM(CTM::DAILY));
					//not possible to add a second year without having problem in evaluation....
					//if (m_weather[y].HaveNext())
						//p.End() = m_weather[y + 1].GetEntireTPeriod(CTM(CTM::DAILY)).End();

					output.Init(p, NB_STATS, 0);
					ExecuteDaily(m_weather[y].GetTRef().GetYear(), m_weather, output);

					static const size_t STAT_STAGE[3] = { S_EGG, S_LARVAE, S_ACTIVE_ADULT };

					for (size_t i = 0; i < m_SAResult.size(); i++)
					{
						if (output.IsInside(m_SAResult[i].m_ref))
						{

							for (size_t j = 0; j < NB_INPUTS; j++)
							{
								if (test[j])
								{
									double obs_y = Round(m_SAResult[i].m_obs[j], ROUND_VAL);
									double sim_y = Round(output[m_SAResult[i].m_ref][STAT_STAGE[j]], ROUND_VAL);

									if (obs_y > -999)
									{
										stat.Add(obs_y, sim_y);

										double obs_x = m_SAResult[i].m_ref.GetJDay();
										double sim_x = GetSimX(STAT_STAGE[j], m_SAResult[i].m_ref, obs_y, output);

										/*if (sim_x > -999)
										{
											obs_x = Round(100 * (obs_x - m_nb_days[j][LOWEST]) / m_nb_days[j][RANGE],ROUND_VAL);
											sim_x = Round(100 * (sim_x - m_nb_days[j][LOWEST]) / m_nb_days[j][RANGE],ROUND_VAL);
											stat.Add(obs_x, sim_x);
										}*/
									}
								}
							}
						}
					}//for all results
				}//have data
			}
		}
	}
}


// Std includes
#include <memory>
#include <vector>
#include <cmath>
#include <iostream>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "s0s/solver.h"
#include "sl0/ellipsoid.h"
#include "sa0/actuators.h"
#include "sa0/behaviours.h"
#include "sa0/sensors/local.h"
#include "sa0/sensors/computer.h"
#include "sa0/agents.h"
#include "fl0w/simple_shear.h"

using TypeScalar = double;
using TypeVector = Eigen::Matrix<TypeScalar, 3, 1>;
using TypeMatrix = Eigen::Matrix<TypeScalar, 3, 3>;
using TypeState = Eigen::Matrix<TypeScalar, 12, 1>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
template<typename A, template<typename ...Args> class B, typename C>
using TypeSolver = s0s::SolverRungeKuttaFehlberg<A, B, C>;
using TypeFlow = fl0w::SimpleShear<TypeVector, TypeMatrix>;

// Agent
using TypeStepObject = sl0::StepEllipsoid<TypeState, TypeView, TypeVector, TypeMatrix, TypeFlow>;
using TypeObject = sl0::ObjectBasis<TypeState, TypeView, TypeSolver, sl0::sa0::StepActuators<TypeState, TypeStepObject, std::vector>>;

using TypeObserver = sl0::sa0::Observer<std::vector<double>, TypeState, TypeStepObject>;
using TypeBehaviour = sl0::sa0::BehaviourLinearOnOff<std::vector<double>, std::vector<double>>;
using TypeAgent = sl0::sa0::ObjectActive<TypeObject, TypeBehaviour, TypeObserver, std::vector>;
using TypeStepSwim = sl0::sa0::StepSwim<TypeState, TypeStepObject, TypeVector>;

using TypeSensorVector = sl0::sa0::Sensor<TypeVector, TypeState, TypeStepObject, TypeVector>;
using TypeSensorMatrix = sl0::sa0::Sensor<TypeMatrix, TypeState, TypeStepObject, TypeVector>;
// Local sensors
using TypeSensorDirection = sl0::sa0::SensorLocalVector<sl0::sa0::SensorDirection, TypeVector, std::vector<double>, TypeState, TypeStepObject>;
using TypeSensorJacobian = sl0::sa0::SensorLocalMatrix<sl0::sa0::SensorJacobian, TypeVector, TypeMatrix, std::vector<double>, TypeState, TypeStepObject, TypeVector, TypeFlow>;
using TypeSensorStrain = sl0::sa0::SensorLocalMatrix<sl0::sa0::SensorStrain, TypeVector, TypeMatrix, std::vector<double>, TypeState, TypeStepObject, TypeVector, TypeFlow>;
// Computers
using TypeSensorTranspose = sl0::sa0::SensorTranspose<TypeMatrix, std::vector<double>, TypeState, TypeStepObject>;
using TypeSensorMult = sl0::sa0::SensorMult<TypeVector, std::vector<double>, TypeState, TypeStepObject, TypeMatrix>;
using TypeSensorAxis = sl0::sa0::SensorAxis<std::vector<double>, TypeState, TypeStepObject, TypeVector>;

class Agent : public TypeAgent {
    public:
        using TypeAgent::TypeAgent;
    public:
        void init() override {
            TypeAgent::init();
            // Sensors
            // // Direction
            sSensorDir = std::make_shared<TypeSensorDirection>();
            sSensorDir->pointSensor.direction << 1.0, 0.0, 0.0;
            // // Jacobian
            sSensorJac = std::make_shared<TypeSensorJacobian>();
            sSensorJac->position << 0.0, 0.0, 0.0;
            sSensorJac->pointSensor.sFlow = TypeObject::step.sFlow;
            // // Transpose
            sSensorJacTranspose = std::make_shared<TypeSensorTranspose>();
            sSensorJacTranspose->sSensor = sSensorJac;
            // // Gradient
            sSensorGrad = std::make_shared<TypeSensorMult>();
            sSensorGrad->sA = sSensorJacTranspose;
            sSensorGrad->sB = sSensorDir;
            // // Strain
            sSensorStrain = std::make_shared<TypeSensorStrain>();
            sSensorStrain->position << 0.0, 0.0, 0.0;
            sSensorStrain->pointSensor.sFlow = TypeObject::step.sFlow;
            sSensorStrainTranspose = std::make_shared<TypeSensorTranspose>();
            sSensorStrainTranspose->sSensor = sSensorStrain;
            sSensorStrainGrad = std::make_shared<TypeSensorMult>();
            sSensorStrainGrad->sA = sSensorStrainTranspose;
            sSensorStrainGrad->sB = sSensorDir;
            // Observers
            sObserveDir = std::make_shared<TypeSensorAxis>();
            sObserveDir->axis = 0;
            sObserveDir->sSensor = sSensorDir;
            sObserveGrad = std::make_shared<TypeSensorAxis>();
            sObserveGrad->axis = 0;
            sObserveGrad->sSensor = sSensorGrad;
            // Add to behaviour observers
            observers.push_back(sObserveDir);
            observers.push_back(sObserveGrad);
        }
    public:
        // Sensors
        std::shared_ptr<TypeSensorDirection> sSensorDir;
        std::shared_ptr<TypeSensorJacobian> sSensorJac;
        std::shared_ptr<TypeSensorTranspose> sSensorJacTranspose;
        std::shared_ptr<TypeSensorMult> sSensorGrad;
        std::shared_ptr<TypeSensorStrain> sSensorStrain;
        std::shared_ptr<TypeSensorTranspose> sSensorStrainTranspose;
        std::shared_ptr<TypeSensorMult> sSensorStrainGrad;
        // Observers
        std::shared_ptr<TypeSensorAxis> sObserveDir;
        std::shared_ptr<TypeSensorAxis> sObserveGrad;
};

int main () {
    TypeVector x0;
    x0 << 0,
          0,
          0;
    TypeMatrix b0;
    b0 << 1, 0, 0,
          0, 1, 0,
          0, 0, 1;
    double t0 = 0.0;
    double dt = 1e-2;
    // Building the agent
    Agent agent;
    agent.step.setProportions(TypeVector(3.0, 2.0, 1.0));
    agent.step.x(agent.state) = x0;
    agent.step.basis(agent.state) = b0;
    agent.solver.t = t0;
    agent.solver.dt = dt;
    //// Building flow
    agent.step.sFlow = std::make_shared<TypeFlow>();
    agent.step.sFlow->gamma = 1.0;
    //// Building Swim
    std::shared_ptr<TypeStepSwim> sStepSwim = std::make_shared<TypeStepSwim>();
    sStepSwim->velocity << 0.0,
                           0.0,
                           0.0;
    sStepSwim->intensity = 0.0;
    agent.step.stepActuators.push_back(sStepSwim);
    //// Building behaviour
    agent.behaviour.threshold = 0.0;
    agent.behaviour.brain = {0.0, 0.0};
    // Init the agent
    agent.init();
    // Compute
    std::cout << std::endl;
    std::cout << "Start : " << std::endl;
    for(std::size_t i = 0; i < 1000; i++) {
        agent.update();
    }
    std::cout << "Dir agent local " << std::endl;
    std::cout << agent.sSensorDir->operator()(agent.step, agent.state, agent.solver.t) << std::endl;
    std::cout << "Dir agent global " << std::endl;
    std::cout << agent.step.basis(agent.state).transpose() * agent.sSensorDir->operator()(agent.step, agent.state, agent.solver.t) << std::endl;
    std::cout << "Dir agent point " << std::endl;
    std::cout << agent.sSensorDir->pointSensor(agent.step, agent.state, agent.solver.t) << std::endl;
    std::cout << "Dir true  global " << std::endl;
    std::cout << agent.sSensorDir->pointSensor.direction << std::endl;
    std::cout << "Jac agent local " << std::endl;
    std::cout << agent.sSensorJac->operator()(agent.step, agent.state, agent.solver.t) << std::endl;
    std::cout << "Jac agent global " << std::endl;
    std::cout << agent.step.basis(agent.state).transpose() * agent.sSensorJac->operator()(agent.step, agent.state, agent.solver.t) * agent.step.basis(agent.state) << std::endl;
    std::cout << "Jac agent point " << std::endl;
    std::cout << agent.sSensorJac->pointSensor(agent.step, agent.state, agent.solver.t) << std::endl;
    std::cout << "Jac true  global " << std::endl;
    std::cout << agent.step.sFlow->getJacobian(agent.step.x(agent.state), agent.solver.t) << std::endl;
    std::cout << "Grad agent local " << std::endl;
    std::cout << agent.sSensorGrad->operator()(agent.step, agent.state, agent.solver.t) << std::endl;
    std::cout << "Grad agent global " << std::endl;
    std::cout << agent.step.basis(agent.state).transpose() * agent.sSensorGrad->operator()(agent.step, agent.state, agent.solver.t) << std::endl;
    std::cout << "Grad true  global " << std::endl;
    std::cout << agent.step.sFlow->getJacobian(agent.step.x(agent.state), agent.solver.t).transpose().col(0) << std::endl;
    std::cout << "Strain agent local " << std::endl;
    std::cout << agent.sSensorStrain->operator()(agent.step, agent.state, agent.solver.t) << std::endl;
    std::cout << "Strain agent global " << std::endl;
    std::cout << agent.step.basis(agent.state).transpose() * agent.sSensorStrain->operator()(agent.step, agent.state, agent.solver.t) * agent.step.basis(agent.state) << std::endl;
    std::cout << "Strain agent point " << std::endl;
    std::cout << agent.sSensorStrain->pointSensor(agent.step, agent.state, agent.solver.t) << std::endl;
    std::cout << "Strain true  global " << std::endl;
    std::cout << agent.step.sFlow->getStrain(agent.step.x(agent.state), agent.solver.t) << std::endl;
    std::cout << "Grad Strain agent local " << std::endl;
    std::cout << agent.sSensorStrainGrad->operator()(agent.step, agent.state, agent.solver.t) << std::endl;
    std::cout << "Grad Strain agent global " << std::endl;
    std::cout << agent.step.basis(agent.state).transpose() * agent.sSensorStrainGrad->operator()(agent.step, agent.state, agent.solver.t) << std::endl;
    // out
    std::cout << std::endl;
    std::cout << "Final Position : " << std::endl;
    std::cout << agent.step.x(agent.state) << std::endl;
    std::cout << std::endl;
}


// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)

#define TEST_NAME "optim/least_absolute_deviations"
#include "optim/least_absolute_deviations.h"

#include <Eigen/Dense>

#include "util/random.h"
#include "util/testing.h"

using namespace colmap;

BOOST_AUTO_TEST_CASE(TestOverDetermined) {
  Eigen::SparseMatrix<double> A(4, 3);
  for (int i = 0; i < A.rows(); ++i) {
    for (int j = 0; j < A.cols(); ++j) {
      A.insert(i, j) = i * A.cols() + j + 1;
    }
  }
  A.coeffRef(0, 0) = 10;

  Eigen::VectorXd b(A.rows());
  for (int i = 0; i < b.size(); ++i) {
    b(i) = i + 1;
  }

  Eigen::VectorXd x = Eigen::VectorXd::Zero(A.cols());

  LeastAbsoluteDeviationsOptions options;
  BOOST_CHECK(SolveLeastAbsoluteDeviations(options, A, b, &x));

  // Reference solution obtained with Boyd's Matlab implementation.
  const Eigen::Vector3d x_ref(0, 0, 1 / 3.0);
  BOOST_CHECK(x.isApprox(x_ref));

  const Eigen::VectorXd residual = A * x - b;
  BOOST_CHECK_LE(residual.norm(), 1e-6);
}

BOOST_AUTO_TEST_CASE(TestWellDetermined) {
  Eigen::SparseMatrix<double> A(3, 3);
  for (int i = 0; i < A.rows(); ++i) {
    for (int j = 0; j < A.cols(); ++j) {
      A.insert(i, j) = i * A.cols() + j + 1;
    }
  }
  A.coeffRef(0, 0) = 10;

  Eigen::VectorXd b(A.rows());
  for (int i = 0; i < b.size(); ++i) {
    b(i) = i + 1;
  }

  Eigen::VectorXd x = Eigen::VectorXd::Zero(A.cols());

  LeastAbsoluteDeviationsOptions options;
  BOOST_CHECK(SolveLeastAbsoluteDeviations(options, A, b, &x));

  // Reference solution obtained with Boyd's Matlab implementation.
  const Eigen::Vector3d x_ref(0, 0, 1 / 3.0);
  BOOST_CHECK(x.isApprox(x_ref));

  const Eigen::VectorXd residual = A * x - b;
  BOOST_CHECK_LE(residual.norm(), 1e-6);
}

BOOST_AUTO_TEST_CASE(TestUnderDetermined) {
  // In this case, the system is rank-deficient and not positive semi-definite.
  Eigen::SparseMatrix<double> A(2, 3);
  Eigen::VectorXd b(A.rows());
  Eigen::VectorXd x = Eigen::VectorXd::Zero(A.cols());
  LeastAbsoluteDeviationsOptions options;
  BOOST_CHECK(!SolveLeastAbsoluteDeviations(options, A, b, &x));
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

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

#include "function_requiring_a_point.hpp"

struct point
{
    point(): x(), y() {}
    float x, y;
};

namespace boost { namespace geometry { namespace traits {

template <>
struct tag<point>
{
    typedef point_tag type;
};

template <>
struct coordinate_type<point>
{
    typedef float type;
};

template <>
struct coordinate_system<point>
{
    typedef bg::cs::cartesian type;
};

template <>
struct dimension<point>
{
    enum { value = 2 };
};

template <>
struct access<point, 0>
{
    static float get(point const& p)
    {
        return p.x;
    }

    static void set(point& p, float value)
    {
        p.x = value;
    }
};

template <>
struct access<point, 1>
{
    static float get(point const& p)
    {
        return p.y;
    }

    static void set(point& p, float value)
    {
        p.y = value;
    }
};

}}} // namespace bg::traits

int main()
{
    point p1;
    const point p2;
    test::function_requiring_a_point(p1, p2);
    return 0;
}


/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include <iostream>

#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "dart/common/Console.hpp"
#include "dart/dynamics/BodyNode.hpp"
#include "dart/dynamics/SimpleFrame.hpp"
#include "dart/dynamics/Skeleton.hpp"
#include "dart/math/Geometry.hpp"
#include "dart/math/Helpers.hpp"
#include "dart/math/Random.hpp"
#include "dart/simulation/World.hpp"
#include "dart/utils/SkelParser.hpp"

#include "TestHelpers.hpp"

using namespace Eigen;
using namespace dart;

//==============================================================================
class DifferentialDynamics : public ::testing::Test
{
public:
  // Get Skel file URI to test.
  const std::vector<common::Uri>& getList() const;

  void compareEquationsOfMotion(const common::Uri& uri);

protected:
  // Sets up the test fixture.
  void SetUp() override;

  // Skel file list.
  std::vector<common::Uri> fileList;
};

//==============================================================================
void DifferentialDynamics::SetUp()
{
  // fileList.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel");
  // fileList.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel");
  // fileList.push_back("dart://sample/skel/test/single_rigid_body.skel");
  // Create a list of skel files to test with
  //  fileList.push_back("dart://sample/skel/test/chainwhipa.skel");
  fileList.push_back("dart://sample/skel/test/single_pendulum.skel");
  fileList.push_back(
      "dart://sample/skel/test/single_pendulum_euler_joint.skel");
  fileList.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel");
  fileList.push_back("dart://sample/skel/test/single_rigid_body.skel");
  fileList.push_back("dart://sample/skel/test/double_pendulum.skel");
  fileList.push_back(
      "dart://sample/skel/test/double_pendulum_euler_joint.skel");
  fileList.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel");
  fileList.push_back(
      "dart://sample/skel/test/serial_chain_revolute_joint.skel");
  //    fileList.push_back("dart://sample/skel/test/serial_chain_eulerxyz_joint.skel");
  //    fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel");
  //    fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel");
  //    fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel");
  fileList.push_back("dart://sample/skel/test/simple_tree_structure.skel");
  //    fileList.push_back("dart://sample/skel/test/simple_tree_structure_euler_joint.skel");
  //    fileList.push_back("dart://sample/skel/test/simple_tree_structure_ball_joint.skel");
  fileList.push_back("dart://sample/skel/test/tree_structure.skel");
  //    fileList.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel");
  //    fileList.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel");
  fileList.push_back("dart://sample/skel/test/tree_structure_root_free.skel");
  //  fileList.push_back("dart://sample/skel/fullbody1.skel");
}

//==============================================================================
const std::vector<common::Uri>& DifferentialDynamics::getList() const
{
  return fileList;
}

//==============================================================================
template <typename T>
void printComparisonError(
    const std::string& _comparison,
    const std::string& _name,
    const std::string& _frame,
    const T& fk,
    const T& jac)
{
  std::cout << "Disagreement between FK and Jacobian results for "
            << _comparison << " of '" << _name
            << "' with a reference Frame of '" << _frame << "'\n"
            << "FK:  " << fk.transpose() << "\n"
            << "Jac: " << jac.transpose() << "\n";
}

//==============================================================================
template <typename T>
void printComparisonError(
    const std::string& _comparison,
    const std::string& _nameBN,
    const std::string& _nameBNRelativeTo,
    const std::string& _frame,
    const T& fk,
    const T& jac)
{
  std::cout << "Disagreement between FK and relative Jacobian results for "
            << _comparison << " of '" << _nameBN << "' relative to '"
            << _nameBNRelativeTo << "' with a reference Frame of '" << _frame
            << "'\n"
            << "FK:  " << fk.transpose() << "\n"
            << "Jac: " << jac.transpose() << "\n";
}

//==============================================================================
TEST_F(DifferentialDynamics, compareEquationsOfMotion)
{
  using namespace std;
  using namespace Eigen;
  using namespace dart;
  using namespace math;
  using namespace dynamics;
  using namespace simulation;
  using namespace utils;

  //---------------------------- Settings --------------------------------------
  // Number of random state tests for each skeletons
#ifndef NDEBUG // Debug mode
  int nRandomItr = 1;
#else
  int nRandomItr = 100;
#endif

  const s_t pi = constantsd::pi();

  // Lower and upper bound of configuration for system
  s_t qLB = -0.25 * pi;
  s_t qUB = 0.25 * pi;
  s_t dqLB = -0.25 * pi;
  s_t dqUB = 0.25 * pi;
  s_t ddqLB = -0.25 * pi;
  s_t ddqUB = 0.25 * pi;

  simulation::WorldPtr world;

  srand(42);

  for (const auto& uri : getList())
  {
#ifndef NDEBUG
    dtdbg << uri.toString() << std::endl;
#endif

    // TODO(keenon): remove all this special treatment when we remove finite
    // differencing for FreeJoint and BallJoint
    s_t abs_tol_C = 1e-8;
    s_t rel_tol_C = 1e-3; // 0.1 %
    s_t abs_tol_invM = 1e-8;
    s_t rel_tol_invM = 1e-3; // 0.1 %
    /*
    if (uri.toString() == "dart://sample/skel/test/chainwhipa.skel")
    {
    }
    else if (uri.toString() == "dart://sample/skel/test/tree_structure.skel")
    {
      abs_tol_invM = 2e-5;
      rel_tol_invM = 5e-3; // 0.5 %
    }
    else if (
        uri.toString()
        == "dart://sample/skel/test/double_pendulum_ball_joint.skel")
    {
      abs_tol_invM = 2e-5;
      rel_tol_invM = 5e-3; // 0.5 %
    }
    else if (
        uri.toString()
        == "dart://sample/skel/test/serial_chain_revolute_joint.skel")
    {
      abs_tol_invM = 3e-3;
      rel_tol_invM = 5e-3; // 0.5 %
    }
    else if (
        uri.toString()
        == "dart://sample/skel/test/serial_chain_ball_joint.skel")
    {
      abs_tol_invM = 2e-5;
      rel_tol_invM = 5e-3; // 0.5 %
    }
    else if (
        uri.toString()
        == "dart://sample/skel/test/tree_structure_root_free.skel")
    {
      abs_tol_invM = 2e-5;
      rel_tol_invM = 5e-3; // 0.5 %
    }
    else if (
        uri.toString()
        == "dart://sample/skel/test/serial_chain_eulerxyz_joint.skel")
    {
    }
    */

    //----------------------------- Tests --------------------------------------
    // Check whether multiplication of mass matrix and its inverse is identity
    // matrix.
    world = utils::SkelParser::readWorld(uri);
    // world->setGravity(Eigen::Vector3s::Zero());  // TODO(JS): Remove
    EXPECT_TRUE(world != nullptr);

    for (std::size_t i = 0; i < world->getNumSkeletons(); ++i)
    {
      dynamics::SkeletonPtr skel = world->getSkeleton(i);
      ASSERT_TRUE(true);

      const int dof = static_cast<int>(skel->getNumDofs());

      // Generate a random state
      VectorXs q = VectorXs::Zero(dof);
      VectorXs dq = VectorXs::Zero(dof);
      VectorXs ddq = VectorXs::Zero(dof);

      for (int j = 0; j < nRandomItr; ++j)
      {
        for (int k = 0; k < dof; ++k)
        {
          q[k] = math::Random::uniform(qLB, qUB);
          dq[k] = math::Random::uniform(dqLB, dqUB);
          ddq[k] = math::Random::uniform(ddqLB, ddqUB);
        }
        skel->setPositions(q);
        skel->setVelocities(dq);
        skel->setAccelerations(ddq);

        for (int i = 0; i < skel->getNumJoints(); i++)
        {
          // Eigen::Isometry3s Tparent = Eigen::Isometry3s::Identity();
          // Tparent.translation()(2) = 1.0;
          // skel->getJoint(i)->setTransformFromParentBodyNode(Tparent);
          // skel->getJoint(i)->setTransformFromChildBodyNode(Tparent);
          // skel->getJoint(i)->debugRelativeJacobianInPositionSpace();
        }

        // Test derivative of DM/dq*x
        {
          Eigen::VectorXs x = Eigen::VectorXs::Random(dof);
          Eigen::MatrixXs DMX_Dq_numerical = skel->finiteDifferenceJacobianOfM(
              x, neural::WithRespectTo::POSITION);
          Eigen::MatrixXs DMX_Dq_analytic
              = skel->getJacobianOfM(x, neural::WithRespectTo::POSITION);
          const bool res = equals(
              DMX_Dq_analytic, DMX_Dq_numerical, abs_tol_invM, rel_tol_invM);
          EXPECT_TRUE(res);
          if (!res)
          {
#ifdef DART_DEBUG_ANALYTICAL_DERIV
            skel->mDiffMinv.print();
#endif
            for (int i = 0; i < skel->getNumBodyNodes(); i++)
            {
              skel->getBodyNode(i)->debugJacobianOfMForward(
                  neural::WithRespectTo::POSITION, x);
            }
            for (int i = skel->getNumBodyNodes() - 1; i >= 0; i--)
            {
              skel->getBodyNode(i)->debugJacobianOfMBackward(
                  neural::WithRespectTo::POSITION, x, DMX_Dq_numerical);
            }
            cout << "[DEBUG] URI: " << uri.toString() << std::endl;

            cout << "[DEBUG] DM/Dq * x analytic\n";
            cout << DMX_Dq_analytic << std::endl;

            cout << "[DEBUG] DM/Dq * x numerical\n";
            cout << DMX_Dq_numerical << std::endl;

            cout << "[DEBUG] Diff\n";
            cout << DMX_Dq_analytic - DMX_Dq_numerical << std::endl;

            cout << "[DEBUG] range: "
                 << (DMX_Dq_analytic - DMX_Dq_numerical).minCoeff() << " - "
                 << (DMX_Dq_analytic - DMX_Dq_numerical).maxCoeff()
                 << std::endl;
            return;
          }

          EXPECT_TRUE(skel->getJacobianOfM(x, neural::WithRespectTo::VELOCITY)
                          .isZero());
          EXPECT_TRUE(
              skel->getJacobianOfM(x, neural::WithRespectTo::FORCE).isZero());
          EXPECT_TRUE(skel->finiteDifferenceJacobianOfM(
                              x, neural::WithRespectTo::VELOCITY)
                          .isZero());
          EXPECT_TRUE(
              skel->finiteDifferenceJacobianOfM(x, neural::WithRespectTo::FORCE)
                  .isZero());
        }

        // Test derivative of Coriolis matrix w.r.t. position
        {
          Eigen::MatrixXs C_q_numerical = skel->finiteDifferenceJacobianOfC(
              neural::WithRespectTo::POSITION);
          Eigen::MatrixXs C_q_analytic
              = skel->getJacobianOfC(neural::WithRespectTo::POSITION);

          const bool res
              = equals(C_q_analytic, C_q_numerical, abs_tol_C, rel_tol_C);
          EXPECT_TRUE(res);
          if (!res)
          {
            cout << "[DEBUG] URI: " << uri.toString() << std::endl;

            cout << "[DEBUG] DC/Dq analytic\n";
            cout << C_q_analytic << std::endl;

            cout << "[DEBUG] DC/Dq numerical\n";
            cout << C_q_numerical << std::endl;

            cout << "[DEBUG] Diff\n";
            cout << C_q_analytic - C_q_numerical << std::endl;

            cout << "[DEBUG] max(Diff): "
                 << (C_q_analytic - C_q_numerical).maxCoeff() << std::endl;

            for (int i = 0; i < skel->getNumBodyNodes(); i++)
            {
              std::cout << "Checking body node " << i << ":" << std::endl;
              skel->getBodyNode(i)->debugJacobianOfCForward(
                  neural::WithRespectTo::POSITION);
              skel->getBodyNode(i)->debugJacobianOfCBackward(
                  neural::WithRespectTo::POSITION);
            }

            return;
          }
        }

        // Test derivative of inverse dynamics w.r.t. position
        {
          Eigen::VectorXs x = Eigen::VectorXs::Random(dof);
          Eigen::MatrixXs DID_Dq_numerical = skel->finiteDifferenceJacobianOfID(
              x, neural::WithRespectTo::POSITION);
          Eigen::MatrixXs DID_Dq_analytic
              = skel->getJacobianOfID(x, neural::WithRespectTo::POSITION);
          const bool res = equals(
              DID_Dq_analytic, DID_Dq_numerical, abs_tol_invM, rel_tol_invM);
          EXPECT_TRUE(res);
          if (!res)
          {
#ifdef DART_DEBUG_ANALYTICAL_DERIV
            skel->mDiffMinv.print();
#endif
            cout << "[DEBUG] URI: " << uri.toString() << std::endl;

            cout << "[DEBUG] DID/Dq * x analytic\n";
            cout << DID_Dq_analytic << std::endl;

            cout << "[DEBUG] DID/Dq * x numerical\n";
            cout << DID_Dq_numerical << std::endl;

            cout << "[DEBUG] Diff\n";
            cout << DID_Dq_analytic - DID_Dq_numerical << std::endl;

            cout << "[DEBUG] max(Diff): "
                 << (DID_Dq_analytic - DID_Dq_numerical).maxCoeff()
                 << std::endl;
          }
        }

        // Test derivative of inverse dynamics w.r.t. velocity
        {
          Eigen::VectorXs x = Eigen::VectorXs::Random(dof);
          Eigen::MatrixXs DID_Ddq_numerical
              = skel->finiteDifferenceJacobianOfID(
                  x, neural::WithRespectTo::VELOCITY);
          Eigen::MatrixXs DID_Ddq_analytic
              = skel->getJacobianOfID(x, neural::WithRespectTo::VELOCITY);
          const bool res = equals(
              DID_Ddq_analytic, DID_Ddq_numerical, abs_tol_invM, rel_tol_invM);
          EXPECT_TRUE(res);
          if (!res)
          {
#ifdef DART_DEBUG_ANALYTICAL_DERIV
            skel->mDiffMinv.print();
#endif
            cout << "[DEBUG] URI: " << uri.toString() << std::endl;

            cout << "[DEBUG] DID/Ddq * x analytic\n";
            cout << DID_Ddq_analytic << std::endl;

            cout << "[DEBUG] DID/Ddq * x numerical\n";
            cout << DID_Ddq_numerical << std::endl;

            cout << "[DEBUG] Diff\n";
            cout << DID_Ddq_analytic - DID_Ddq_numerical << std::endl;

            cout << "[DEBUG] max(Diff): "
                 << (DID_Ddq_analytic - DID_Ddq_numerical).maxCoeff()
                 << std::endl;
          }
        }

        // Test derivative of inverse dynamics w.r.t. force
        {
          Eigen::VectorXs x = Eigen::VectorXs::Random(dof);
          Eigen::MatrixXs DID_Df_numerical = skel->finiteDifferenceJacobianOfID(
              x, neural::WithRespectTo::FORCE);
          Eigen::MatrixXs DID_Df_analytic
              = skel->getJacobianOfID(x, neural::WithRespectTo::FORCE);
          const bool res = equals(
              DID_Df_analytic, DID_Df_numerical, abs_tol_invM, rel_tol_invM);
          EXPECT_TRUE(res);
          if (!res)
          {
#ifdef DART_DEBUG_ANALYTICAL_DERIV
            skel->mDiffMinv.print();
#endif
            cout << "[DEBUG] URI: " << uri.toString() << std::endl;

            cout << "[DEBUG] DID/Df * x analytic\n";
            cout << DID_Df_analytic << std::endl;

            cout << "[DEBUG] DID/Df * x numerical\n";
            cout << DID_Df_numerical << std::endl;

            cout << "[DEBUG] Diff\n";
            cout << DID_Df_analytic - DID_Df_numerical << std::endl;

            cout << "[DEBUG] max(Diff): "
                 << (DID_Df_analytic - DID_Df_numerical).maxCoeff()
                 << std::endl;
          }
        }

        // Test derivative of Coriolis matrix w.r.t. velocity
        {
          Eigen::MatrixXs C_dq_analytic
              = skel->getJacobianOfC(neural::WithRespectTo::VELOCITY);
          Eigen::MatrixXs dC_numerical = skel->finiteDifferenceJacobianOfC(
              neural::WithRespectTo::VELOCITY);

          const bool res
              = equals(C_dq_analytic, dC_numerical, abs_tol_C, rel_tol_C);
          EXPECT_TRUE(res);
          if (!res)
          {
            cout << "[DEBUG] URI: " << uri.toString() << std::endl;

            cout << "[DEBUG] DC/Ddq analytic\n";
            cout << C_dq_analytic << std::endl;

            cout << "[DEBUG] DC/Ddq numerical\n";
            cout << dC_numerical << std::endl;

            cout << "[DEBUG] Diff\n";
            cout << C_dq_analytic - dC_numerical << std::endl;

            cout << "[DEBUG] max(Diff): "
                 << (C_dq_analytic - dC_numerical).maxCoeff() << std::endl;
          }
        }

        // Test derivative of Coriolis matrix w.r.t. force
        EXPECT_TRUE(
            skel->getJacobianOfC(neural::WithRespectTo::FORCE).isZero());
        EXPECT_TRUE(
            skel->finiteDifferenceJacobianOfC(neural::WithRespectTo::FORCE)
                .isZero());

        /*
        // TODO: uncomment this if we ever start using the Direct Minv Jacobian
again
        // Test derivative of M^{-1}x w.r.t. position/velocity/acceleration
        {
          Eigen::VectorXs x = Eigen::VectorXs::Random(dof);
          Eigen::MatrixXs DMinvX_Dq_numerical
              = skel->finiteDifferenceJacobianOfMinv(
                  x, neural::WithRespectTo::POSITION);
          Eigen::MatrixXs DMinvX_Dq_analytic = skel->getJacobianOfMinv_Direct(
              x, neural::WithRespectTo::POSITION);
          const bool res = equals(
              DMinvX_Dq_analytic,
              DMinvX_Dq_numerical,
              abs_tol_invM,
              rel_tol_invM);
          EXPECT_TRUE(res);
          if (!res)
          {
#ifdef DART_DEBUG_ANALYTICAL_DERIV
            skel->mDiffMinv.print();
#endif
            cout << "[DEBUG] URI: " << uri.toString() << std::endl;

            cout << "[DEBUG] D(M^{-1})/Dq * x analytic\n";
            cout << DMinvX_Dq_analytic << std::endl;

            cout << "[DEBUG] D(M^{-1})/Dq * x numerical\n";
            cout << DMinvX_Dq_numerical << std::endl;

            cout << "[DEBUG] Diff\n";
            cout << DMinvX_Dq_analytic - DMinvX_Dq_numerical << std::endl;

            cout << "[DEBUG] max(Diff): "
                 << (DMinvX_Dq_analytic - DMinvX_Dq_numerical).maxCoeff()
                 << std::endl;

            for (int i = 0; i < skel->getNumBodyNodes(); i++) {
              std::cout << "Checking body node " << i << ":" << std::endl;
              skel->getBodyNode(i)->debugJacobianOfMinvXBackward(neural::WithRespectTo::POSITION,
x);
              skel->getBodyNode(i)->debugJacobianOfMinvXForward(neural::WithRespectTo::POSITION,
x);
            }
            // TODO
          }

          EXPECT_TRUE(
              skel->getJacobianOfMinv_Direct(x, neural::WithRespectTo::VELOCITY)
                  .isZero());
          EXPECT_TRUE(
              skel->getJacobianOfMinv_Direct(x, neural::WithRespectTo::FORCE)
                  .isZero());
          EXPECT_TRUE(skel->finiteDifferenceJacobianOfMinv(
                              x, neural::WithRespectTo::VELOCITY)
                          .isZero());
          EXPECT_TRUE(skel->finiteDifferenceJacobianOfMinv(
                              x, neural::WithRespectTo::FORCE)
                          .isZero());
        }
        */

        // Test derivative of M^{-1}x (ID) w.r.t. position/velocity/acceleration
        {
          Eigen::VectorXs x = Eigen::VectorXs::Random(dof);
          Eigen::MatrixXs DMinvX_Dq_numerical
              = skel->finiteDifferenceJacobianOfMinv(
                  x, neural::WithRespectTo::POSITION);
          Eigen::MatrixXs DMinvX_Dq_analytic
              = skel->getJacobianOfMinv_ID(x, neural::WithRespectTo::POSITION);
          const bool res = equals(
              DMinvX_Dq_analytic,
              DMinvX_Dq_numerical,
              abs_tol_invM,
              rel_tol_invM);
          EXPECT_TRUE(res);
          if (!res)
          {
#ifdef DART_DEBUG_ANALYTICAL_DERIV
            skel->mDiffMinv.print();
#endif
            cout << "[DEBUG] URI: " << uri.toString() << std::endl;

            cout << "[DEBUG] D(M^{-1})/Dq * x analytic\n";
            cout << DMinvX_Dq_analytic << std::endl;

            cout << "[DEBUG] D(M^{-1})/Dq * x numerical\n";
            cout << DMinvX_Dq_numerical << std::endl;

            cout << "[DEBUG] Diff\n";
            cout << DMinvX_Dq_analytic - DMinvX_Dq_numerical << std::endl;

            cout << "[DEBUG] max(Diff): "
                 << (DMinvX_Dq_analytic - DMinvX_Dq_numerical).maxCoeff()
                 << std::endl;
          }

          EXPECT_TRUE(
              skel->getJacobianOfMinv_ID(x, neural::WithRespectTo::VELOCITY)
                  .isZero());
          EXPECT_TRUE(
              skel->getJacobianOfMinv_ID(x, neural::WithRespectTo::FORCE)
                  .isZero());
          EXPECT_TRUE(skel->finiteDifferenceJacobianOfMinv(
                              x, neural::WithRespectTo::VELOCITY)
                          .isZero());
          EXPECT_TRUE(skel->finiteDifferenceJacobianOfMinv(
                              x, neural::WithRespectTo::FORCE)
                          .isZero());
        }

        // Test derivative of forward dynamics w.r.t. position
        {
          Eigen::MatrixXs DFD_Dq_numerical = skel->finiteDifferenceJacobianOfFD(
              neural::WithRespectTo::POSITION);
          Eigen::MatrixXs DFD_Dq_analytic
              = skel->getJacobianOfFD(neural::WithRespectTo::POSITION);
          const bool res = equals(
              DFD_Dq_analytic, DFD_Dq_numerical, abs_tol_invM, rel_tol_invM);
          EXPECT_TRUE(res);
          if (!res)
          {
#ifdef DART_DEBUG_ANALYTICAL_DERIV
            skel->mDiffMinv.print();
#endif
            cout << "[DEBUG] URI: " << uri.toString() << std::endl;

            cout << "[DEBUG] DFD/Dq * x analytic\n";
            cout << DFD_Dq_analytic << std::endl;

            cout << "[DEBUG] DFD/Dq * x numerical\n";
            cout << DFD_Dq_numerical << std::endl;

            cout << "[DEBUG] Diff\n";
            cout << DFD_Dq_analytic - DFD_Dq_numerical << std::endl;

            cout << "[DEBUG] max(Diff): "
                 << (DFD_Dq_analytic - DFD_Dq_numerical).maxCoeff()
                 << std::endl;
          }
        }
      }
    }
  }
}


#ifndef FVMSCALAR1D_RUNGE_KUTTA_HPP
#define FVMSCALAR1D_RUNGE_KUTTA_HPP

#include <Eigen/Dense>
#include <map>
#include <memory>

#include <ancse/boundary_condition.hpp>
#include <ancse/rate_of_change.hpp>

/// Interface advancing the solution of a PDE by one step.
class TimeIntegrator {
  public:
    virtual ~TimeIntegrator() = default;

    /// Update 'u1' starting from 'u0' using a time-step `dt`.
    virtual void operator()(Eigen::VectorXd &u1,
                            const Eigen::VectorXd &u0,
                            double dt) const = 0;
};

/// Runge-Kutta time integration methods.
/** Note: not all methods are Runge-Kutta methods. Therefore, 'RungeKutta' can
 *        not be the interface. It should be the base for all (explicit)
 *        Runge-Kutta methods.
 */
class RungeKutta : public TimeIntegrator {
  public:
    RungeKutta(std::shared_ptr<RateOfChange> rate_of_change,
               std::shared_ptr<BoundaryCondition> boundary_condition)
        : rate_of_change(std::move(rate_of_change)),
          boundary_condition(std::move(boundary_condition)) {}

  protected:
    std::shared_ptr<RateOfChange> rate_of_change;
    std::shared_ptr<BoundaryCondition> boundary_condition;
};

/// First order explicit time integration.
class ForwardEuler : public RungeKutta {
  private:
    using super = RungeKutta;

  public:
    ForwardEuler(std::shared_ptr<RateOfChange> rate_of_change,
                 std::shared_ptr<BoundaryCondition> boundary_condition,
                 int n_cells)
        : super(std::move(rate_of_change), std::move(boundary_condition)),
          dudt(n_cells) {}

    virtual void operator()(Eigen::VectorXd &u1,
                            const Eigen::VectorXd &u0,
                            double dt) const override {

        (*rate_of_change)(dudt, u0);
        u1 = u0 + dt * dudt;
        (*boundary_condition)(u1);
    }

  private:
    mutable Eigen::VectorXd dudt;
};

std::shared_ptr<RungeKutta>
make_runge_kutta(const std::shared_ptr<RateOfChange> &rate_of_change,
                 const std::shared_ptr<BoundaryCondition> &boundary_condition,
                 int n_cells);

#endif // FVMSCALAR1D_RUNGE_KUTTA_HPP


#ifndef _RANDNUM_GENERATOR_HPP
#define _RANDNUM_GENERATOR_HPP

#include <common.hpp>
#if !SABOT_NTHRD_RNG
#	include <mutex>
#endif
#include <boost/random.hpp>

namespace randnum {
	/**
	 * \brief The types of generators supported.
	 *
	 * We can use any random generator implemented in the boost library. If you are using
	 * an older version of the library, some of these may not be available. In this case,
	 * the compiler will throw a compile-time error.
	 */
	namespace gen_type {
		using minstd_rand0 = boost::random::minstd_rand0;
		using minstd_rand = boost::random::minstd_rand;
		using rand48 = boost::random::rand48;
		using ecuyer1988 = boost::random::ecuyer1988;
		using knuth_b = boost::random::knuth_b;
		using krutzer1986 = boost::random::kreutzer1986;
		using taus88 = boost::random::taus88;
		using hellekalek1995 = boost::random::hellekalek1995;
		using mt11213b = boost::random::mt11213b;
		using mt19937 = boost::random::mt19937;
		using mt19937_64 = boost::random::mt19937_64;
		using lagged_fibonacci607 = boost::random::lagged_fibonacci607;
		using lagged_fibonacci1279 = boost::random::lagged_fibonacci1279;
		using lagged_fibonacci2281 = boost::random::lagged_fibonacci2281;
		using lagged_fibonacci3217 = boost::random::lagged_fibonacci3217;
		using lagged_fibonacci4423 = boost::random::lagged_fibonacci4423;
		using lagged_fibonacci9689 = boost::random::lagged_fibonacci9689;
		using lagged_fibonacci19937 = boost::random::lagged_fibonacci19937;
		using lagged_fibonacci23209 = boost::random::lagged_fibonacci23209;
		using lagged_fibonacci44497 = boost::random::lagged_fibonacci44497;
		using ranlux3 = boost::random::ranlux3;
		using ranlux4 = boost::random::ranlux4;
		using ranlux64_3 = boost::random::ranlux64_3;
		using ranlux64_4 = boost::random::ranlux64_4;
		using ranlux3_01 = boost::random::ranlux3_01;
		using ranlux4_01 = boost::random::ranlux4_01;
		using ranlux64_3_01 = boost::random::ranlux64_3_01;
		using ranlux64_4_01 = boost::random::ranlux64_4_01;
		using ranlux24 = boost::random::ranlux24;
		using ranlux48 = boost::random::ranlux48;
	}

	/**
	 * \brief A random number generator.
	 *
	 * Each function that returns a random value is driven by the same random number
	 * generator.
	 *
	 * \note Threadsafe if SABOT_NTHRD_RNG is false.
	 */
	class generator {
	 private:
		#if !SABOT_NTHRD_RNG
		/**
		 * \brief Standard lock type for the class.
		 */
		using lock_t = std::lock_guard<std::mutex>;
		#endif

	 public:
	 	/**
		 * \todo Finish implementing flexibility in this.
		 */
		using generator_type = gen_type::mt11213b;

		/**
		 * \brief Constructor with no supplied seed.
		 *
		 * We will try our best at picking a nontrivial seed. We look at the system clock
		 * in UTC with microsecond precision as the seed. On Windows machines this
		 * precision is reduced to milliseconds. Discussion of this is found in the Boost
		 * documentation.
		 *
		 * It is important to note that we may lose precision when using the time.
		 */
		generator();

		/**
		 * \brief Constructor takes in a seed value.
		 */
		generator(const uint32_t suppliedSeed);

		/**
		 * \brief Copy constructor is disabled.
		 */
		generator(const generator&) = delete;

		/**
		 * \brief Move constructor is disabled.
		 */
		generator(generator&&) = delete;

		/**
		 * \brief Assignment operator is disabled.
		 */
		generator& operator=(const generator&) = delete;

		/**
		 * \brief Move assignment operator is disabled.
		 */
		generator& operator=(generator&&) = delete;

		/**
		 * \brief Destructor.
		 */
		~generator() {
		}

		/**
		 * \brief Return an integer between two numbers with a uniform distribution.
		 *
		 * \note Threadsafe if SABOT_NTHRD_RNG is false.
		 */
		template <typename T, T lower, T upper>
				T get_uniform_integer() {
			static_assert(std::is_integral<T>::value,
					"T must be an integral type.");
			static_assert(lower <= upper,
					"lower cannot be larger than upper.");

			boost::random::uniform_int_distribution<T> distribution(lower, upper);

			#if !SABOT_NTHRD_RNG
			lock_t lock(accessMutex);
			#endif

			position++;

			return distribution(rng);
		}

		/**
		 * \brief Return an integer between two numbers with a uniform distribution.
		 *
		 * \warning The compiler will try to deduce the template parameter T from the
		 * function arguments if T is not supplied. Therefore it is possible to call the
		 * function without any indication it is a template function. If this is done,
		 * then the arguments must be of a consistent type.
		 *
		 * \note Threadsafe if SABOT_NTHRD_RNG is false.
		 */
		template <typename T>
				T get_uniform_integer(const T lower = 0, const T upper = 100) {
			static_assert(std::is_integral<T>::value,
					"T must be an integral type.");

			#ifdef SABOT_THROW
			if(UNLIKELY(lower > upper)) {
				throw std::invalid_argument(err_msg::_badvals);
			}
			#endif

			boost::random::uniform_int_distribution<T> distribution(lower, upper);

			#if !SABOT_NTHRD_RNG
			lock_t lock(accessMutex);
			#endif

			position++;

			return distribution(rng);
		}

		/**
		 * \brief Return a real number between two numbers with a uniform distribution.
		 *
		 * \note Threadsafe if SABOT_NTHRD_RNG is false.
		 */
		template <typename T, T lower, T upper>
				T get_uniform_real() {
			static_assert(std::is_floating_point<T>::value,
					"T must be a floating point type.");
			static_assert(lower <= upper, "lower cannot be larger than upper.");

			boost::random::uniform_real_distribution<T> distribution(lower, upper);

			#if !SABOT_NTHRD_RNG
			lock_t lock(accessMutex);
			#endif

			position++;

			return distribution(rng);
		}

		/**
		 * \brief Return a real number between two numbers with a uniform distribution.
		 *
		 * \warning The compiler will try to deduce the template parameter T from the
		 * function arguments if T is not supplied. Therefore it is possible to call the
		 * function without any indication it is a template function. If this is done,
		 * then the arguments must be of a consistent type.
		 *
		 * \note Threadsafe if SABOT_NTHRD_RNG is false.
		 */
		template <typename T>
				T get_uniform_real(const T lower = 0.0, const T upper = 1.0) {
			static_assert(std::is_floating_point<T>::value,
					"T must be a floating point type.");

			#ifdef SABOT_THROW
			if(UNLIKELY(lower > upper)) {
				throw std::invalid_argument(err_msg::_badvals);
			}
			#endif

			boost::random::uniform_real_distribution<T> distribution(lower, upper);

			#if !SABOT_NTHRD_RNG
			lock_t lock(accessMutex);
			#endif

			position++;

			return distribution(rng);
		}

		/**
		 * \brief Return an integer number between 0 and the number of elements in the
		 * supplied weight list, where each value in the list corresponds to the numbers
		 * weight.
		 *
		 * \warning The compiler will try to deduce the template parameter T from the
		 * function arguments if T is not supplied. Therefore it is possible to call the
		 * function without any indication it is a template function. If this is done,
		 * then the arguments must be of a consistent type.
		 *
		 * \note Threadsafe if SABOT_NTHRD_RNG is false.
		 */
		template <typename T>
				std::size_t get_weighted_integer(const T* const weights,
				const SABOT_UINTN size) {
			#ifdef SABOT_THROW
			if(UNLIKELY(weights == 0)) {
				throw std::invalid_argument(err_msg::_nllpntr);
			}
			if(UNLIKELY(size == 0)) {
				throw std::invalid_argument(err_msg::_zrlngth);
			}
			#endif

			T sample(get_uniform_real((T)0, std::accumulate(weights, weights+size, (T)0)));

			for(SABOT_UINTN i = 0; i < size; i++) {
				if(sample < weights[i]) {
					return i;
				}
				sample -= weights[i];
			}

			throw std::logic_error(err_msg::_unrchcd);
		}

		/**
		 * \brief Return the current seed.
		 *
		 * \note Threadsafe if SABOT_NTHRD_RNG is false.
		 */
		inline uint32_t seed() const {
			return rngSeed;
		}

		/**
		 * \brief Return the current position of the random number generator source.
		 *
		 * \note Threadsafe if SABOT_NTHRD_RNG is false.
		 */
		inline SABOT_UINTN get_position() {
			#if !SABOT_NTHRD_RNG
			lock_t lock(accessMutex);
			#endif

			return position;
		}

		/**
		 * \brief Discard n samples.
		 *
		 * \note Threadsafe if SABOT_NTHRD_RNG is false.
		 */
		inline void discard(const SABOT_UINTN n) {
			#if !SABOT_NTHRD_RNG
			lock_t lock(accessMutex);
			#endif

			rng.discard(n);
			position += n;
		}

	 private:
		/**
		 * \brief The seed of the generator.
		 */
		SABOT_UINTN rngSeed;

		/**
		 * \brief The current position of the generator.
		 */
		SABOT_UINTN position;

		#if !SABOT_NTHRD_RNG
		/**
		 * \brief Mutex for interacting with the generator.
		 */
		std::mutex accessMutex;
		#endif

		/**
		 * \brief The RNG generator.
		 */
		generator_type rng;
	};
}

#endif


// -*- C++ -*-
//
// Copyright Sylvain Bougerel 2009 - 2012.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file COPYING or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_TEST_DYN_LINK
#define SPATIAL_ENABLE_ASSERT // detect interal issues that should not occur

#include <boost/test/unit_test.hpp>
#include "../../src/bits/spatial_node.hpp"
#include "spatial_test_types.hpp"

using namespace spatial;

struct header_fixture
{
  details::Node<details::Kdtree_link<int, int> > header;
  header_fixture()
  {
    header.parent = &header;
    header.left = &header;
    header.right = &header;
  }
};

struct int_node_fixture
{
  /*          H
              |
             Root
             /   \
             L     R
            /  \
          LL    LR       */
  typedef details::Node<details::Kdtree_link<int, int> > node_type;
  node_type header;
  node_type node_root;
  node_type node_left;
  node_type node_left_left;
  node_type node_left_right;
  node_type node_right;

  int_node_fixture()
  {
    header.parent = &node_root;
    header.left = &header;
    header.right = &node_right;
    node_root.parent = &header;
    node_root.left = &node_left;
    node_root.right = &node_right;
    node_left.parent = &node_root;
    node_left.left = &node_left_left;
    node_left.right = &node_left_right;
    node_right.parent = &node_root;
    node_right.left = 0;
    node_right.right = 0;
    node_left_right.parent = &node_left;
    node_left_right.left = 0;
    node_left_right.right = 0;
    node_left_left.parent = &node_left;
    node_left_left.left = 0;
    node_left_left.right = 0;
  }
};

struct int2_node_fixture
{
  /*           H
               |
             (2,2)
             /   \
         (1,1)   (3,3)
         /  \
     (0,0)  (1,1)   */
  typedef details::Node<details::Kdtree_link<int2, int2> > node_type;
  node_type header;
  node_type* leftmost;
  details::Kdtree_link<int2, int2> node_root;
  details::Kdtree_link<int2, int2> node_left;
  details::Kdtree_link<int2, int2> node_left_left;
  details::Kdtree_link<int2, int2> node_left_right;
  details::Kdtree_link<int2, int2> node_right;

  int2_node_fixture()
  {
    using namespace ::spatial::details;
    header.parent = &node_root;
    header.left = &header;
    header.right = &node_right;
    leftmost = &node_left_left;
    node_root.parent = &header;
    node_root.left = &node_left;
    node_root.right = &node_right;
    value(&node_root) = twos;
    node_left.parent = &node_root;
    node_left.left = &node_left_left;
    node_left.right = &node_left_right;
    value(&node_left) = ones;
    node_right.parent = &node_root;
    node_right.left = 0;
    node_right.right = 0;
    value(&node_right) = threes;
    node_left_right.parent = &node_left;
    node_left_right.left = 0;
    node_left_right.right = 0;
    value(&node_left_right) = ones;
    node_left_left.parent = &node_left;
    node_left_left.left = 0;
    node_left_left.right = 0;
    value(&node_left_left) = zeros;
  }
};

struct int2_relaxed_node_fixture
{
  /*         H
             |
           (2,2)5
           /   \
      (1,1)3  (3,3)1
       /  \
  (0,0)1  (1,1)1       */
  typedef details::Node<details::Relaxed_kdtree_link<int2, int2> > node_type;
  node_type header;
  node_type* leftmost;
  details::Relaxed_kdtree_link<int2, int2> node_root;
  details::Relaxed_kdtree_link<int2, int2> node_left;
  details::Relaxed_kdtree_link<int2, int2> node_left_left;
  details::Relaxed_kdtree_link<int2, int2> node_left_right;
  details::Relaxed_kdtree_link<int2, int2> node_right;

  int2_relaxed_node_fixture()
  {
    using namespace ::spatial::details;
    header.parent = &node_root;
    header.left = &header;
    header.right = &node_right;
    leftmost = &node_left_left;
    node_root.parent = &header;
    node_root.left = &node_left;
    node_root.right = &node_right;
    link(&node_root)->weight = 5;
    value(&node_root) = twos;
    node_left.parent = &node_root;
    node_left.left = &node_left_left;
    node_left.right = &node_left_right;
    link(&node_left)->weight = 3;
    value(&node_left) = ones;
    node_right.parent = &node_root;
    node_right.left = 0;
    node_right.right = 0;
    link(&node_right)->weight = 1;
    value(&node_right) = threes;
    node_left_right.parent = &node_left;
    node_left_right.left = 0;
    node_left_right.right = 0;
    link(&node_left_right)->weight = 1;
    value(&node_left_right) = ones;
    node_left_left.parent = &node_left;
    node_left_left.left = 0;
    node_left_left.right = 0;
    link(&node_left_left)->weight = 1;
    value(&node_left_left) = zeros;
  }
};

struct large_int2_relaxed_node_fixture
{
  /*              H
                  |
               (2,2)7
            /          \
      (1,1)3           (3,3)3
       /  \             /
  (0,0)1  (1,1)1     (3,3)2
                       \
                     (3,3)1     */
  typedef details::Node<details::Relaxed_kdtree_link<int2, int2> > node_type;
  node_type header;
  node_type* leftmost;
  node_type node_root;
  node_type node_left;
  node_type node_left_left;
  node_type node_left_right;
  node_type node_right;
  node_type node_right_left;
  node_type node_right_left_right;

  large_int2_relaxed_node_fixture()
  {
    using namespace ::spatial::details;
    header.parent = &node_root;
    header.left = &header;
    header.right = &node_right;
    leftmost = &node_left_left;
    node_root.parent = &header;
    node_root.left = &node_left;
    node_root.right = &node_right;
    link(&node_root)->weight = 7;
    value(&node_root) = twos;
    node_left.parent = &node_root;
    node_left.left = &node_left_left;
    node_left.right = &node_left_right;
    link(&node_left)->weight = 3;
    value(&node_left) = ones;
    node_right.parent = &node_root;
    node_right.left = &node_right_left;
    node_right.right = 0;
    link(&node_right)->weight = 3;
    value(&node_right) = threes;
    node_right_left.parent = &node_right;
    node_right_left.left = 0;
    node_right_left.right = &node_right_left_right;
    link(&node_right_left)->weight = 2;
    value(&node_right_left) = threes;
    node_right_left_right.parent = &node_right_left;
    node_right_left_right.left = 0;
    node_right_left_right.right = 0;
    link(&node_right_left_right)->weight = 1;
    value(&node_right_left_right) = threes;
    node_left_right.parent = &node_left;
    node_left_right.left = 0;
    node_left_right.right = 0;
    link(&node_left_right)->weight = 1;
    value(&node_left_right) = ones;
    node_left_left.parent = &node_left;
    node_left_left.left = 0;
    node_left_left.right = 0;
    link(&node_left_left)->weight = 1;
    value(&node_left_left) = zeros;
  }
};

BOOST_AUTO_TEST_CASE( test_header )
{
  using namespace ::spatial::details;
  header_fixture fix1;
  BOOST_CHECK(header(&fix1.header));
  int_node_fixture fix2;
  BOOST_CHECK(header(&fix2.header));
  BOOST_CHECK(!header(&fix2.node_root));
  BOOST_CHECK(!header(&fix2.node_left));
  BOOST_CHECK(!header(&fix2.node_left_left));
  BOOST_CHECK(!header(&fix2.node_left_right));
  BOOST_CHECK(!header(&fix2.node_right));
  BOOST_CHECK(!header(&fix2.node_right));
}

BOOST_AUTO_TEST_CASE( test_5_node )
{
  using namespace ::spatial::details;
  int_node_fixture fix;
  BOOST_CHECK(header(&fix.header));
  BOOST_CHECK(!header(&fix.node_root));
  BOOST_CHECK(!header(&fix.node_left));
  BOOST_CHECK(!header(&fix.node_right));
  BOOST_CHECK(!header(&fix.node_left_right));
  BOOST_CHECK(!header(&fix.node_left_left));
  BOOST_CHECK(minimum(&fix.node_root) == &fix.node_left_left);
  BOOST_CHECK(maximum(&fix.node_root) == &fix.node_right);
  Node<Kdtree_link<int, int> >* node = &fix.node_left_left;
  BOOST_REQUIRE((node = increment(node)) == &fix.node_left);
  BOOST_REQUIRE((node = increment(node)) == &fix.node_left_right);
  BOOST_REQUIRE((node = increment(node)) == &fix.node_root);
  BOOST_REQUIRE((node = increment(node)) == &fix.node_right);
  BOOST_REQUIRE((node = increment(node)) == &fix.header);
  BOOST_REQUIRE((node = decrement(node)) == &fix.node_right);
  BOOST_REQUIRE((node = decrement(node)) == &fix.node_root);
  BOOST_REQUIRE((node = decrement(node)) == &fix.node_left_right);
  BOOST_REQUIRE((node = decrement(node)) == &fix.node_left);
  BOOST_REQUIRE((node = decrement(node)) == &fix.node_left_left);
  const Node<Kdtree_link<int, int> >* cnode = &fix.node_root;
  BOOST_REQUIRE((cnode = preorder_increment(cnode))
                == &fix.node_left);
  BOOST_REQUIRE((cnode = preorder_increment(cnode))
                == &fix.node_left_left);
  BOOST_REQUIRE((cnode = preorder_increment(cnode))
                == &fix.node_left_right);
  BOOST_REQUIRE((cnode = preorder_increment(cnode))
                == &fix.node_right);
  BOOST_REQUIRE((cnode = preorder_increment(cnode))
                == &fix.header);
}

/**
 *  In this test, we attempt to test all swapping combination possible in
 *  the 5-node tree defined above. Everytime, after swapping all nodes shall
 *  be reacheable, and the expected layout should be tested.
 */
BOOST_AUTO_TEST_CASE( test_swap_node_aux )
{
  using namespace ::spatial::details;
  typedef int_node_fixture::node_type node_type;
  { // Swap left_left with right
    int_node_fixture fix;
    node_type* left_left = &fix.node_left_left;
    node_type* right = &fix.node_right;
    swap_node_aux(left_left, right);
    BOOST_CHECK(left_left == &fix.node_left_left);
    BOOST_CHECK(right == &fix.node_right);
    BOOST_CHECK(fix.node_left_left.left == 0);
    BOOST_CHECK(fix.node_left_left.right == 0);
    BOOST_CHECK(fix.node_left_left.parent == &fix.node_root);
    BOOST_CHECK(fix.node_root.right == &fix.node_left_left);
    BOOST_CHECK(fix.node_root.left == &fix.node_left);
    BOOST_CHECK(fix.node_root.parent == &fix.header);
    BOOST_CHECK(fix.node_right.left == 0);
    BOOST_CHECK(fix.node_right.right == 0);
    BOOST_CHECK(fix.node_right.parent == &fix.node_left);
    BOOST_CHECK(fix.node_left.right == &fix.node_left_right);
    BOOST_CHECK(fix.node_left.left == &fix.node_right);
    BOOST_CHECK(fix.node_left.parent == &fix.node_root);
  }
  { // Swap right with left_left (mirror procedure)
    int_node_fixture fix;
    node_type* left_left = &fix.node_left_left;
    node_type* right = &fix.node_right;
    swap_node_aux(right, left_left);
    BOOST_CHECK(left_left == &fix.node_left_left);
    BOOST_CHECK(right == &fix.node_right);
    BOOST_CHECK(fix.node_left_left.left == 0);
    BOOST_CHECK(fix.node_left_left.right == 0);
    BOOST_CHECK(fix.node_left_left.parent == &fix.node_root);
    BOOST_CHECK(fix.node_root.right == &fix.node_left_left);
    BOOST_CHECK(fix.node_root.left == &fix.node_left);
    BOOST_CHECK(fix.node_root.parent == &fix.header);
    BOOST_CHECK(fix.node_right.left == 0);
    BOOST_CHECK(fix.node_right.right == 0);
    BOOST_CHECK(fix.node_right.parent == &fix.node_left);
    BOOST_CHECK(fix.node_left.right == &fix.node_left_right);
    BOOST_CHECK(fix.node_left.left == &fix.node_right);
    BOOST_CHECK(fix.node_left.parent == &fix.node_root);
  }
  { // Swap left_left with root
    int_node_fixture fix;
    node_type* left_left = &fix.node_left_left;
    node_type* root = &fix.node_root;
    swap_node_aux(left_left, root);
    BOOST_CHECK(left_left == &fix.node_left_left);
    BOOST_CHECK(root == &fix.node_root);
    BOOST_CHECK(fix.node_left_left.left == &fix.node_left);
    BOOST_CHECK(fix.node_left_left.right == &fix.node_right);
    BOOST_CHECK(fix.node_left_left.parent == &fix.header);
    BOOST_CHECK(fix.node_root.right == 0);
    BOOST_CHECK(fix.node_root.left == 0);
    BOOST_CHECK(fix.node_root.parent == &fix.node_left);
    BOOST_CHECK(fix.node_right.left == 0);
    BOOST_CHECK(fix.node_right.right == 0);
    BOOST_CHECK(fix.node_right.parent == &fix.node_left_left);
    BOOST_CHECK(fix.node_left.right == &fix.node_left_right);
    BOOST_CHECK(fix.node_left.left == &fix.node_root);
    BOOST_CHECK(fix.node_left.parent == &fix.node_left_left);
    BOOST_CHECK(fix.header.parent == &fix.node_left_left);
    BOOST_CHECK(fix.header.left == &fix.header);
    BOOST_CHECK(fix.header.right == &fix.node_right);
  }
  { // Swap root with left_left (mirror procedure)
    int_node_fixture fix;
    node_type* left_left = &fix.node_left_left;
    node_type* root = &fix.node_root;
    swap_node_aux(root, left_left);
    BOOST_CHECK(left_left == &fix.node_left_left);
    BOOST_CHECK(root == &fix.node_root);
    BOOST_CHECK(fix.node_left_left.left == &fix.node_left);
    BOOST_CHECK(fix.node_left_left.right == &fix.node_right);
    BOOST_CHECK(fix.node_left_left.parent == &fix.header);
    BOOST_CHECK(fix.node_root.right == 0);
    BOOST_CHECK(fix.node_root.left == 0);
    BOOST_CHECK(fix.node_root.parent == &fix.node_left);
    BOOST_CHECK(fix.node_right.left == 0);
    BOOST_CHECK(fix.node_right.right == 0);
    BOOST_CHECK(fix.node_right.parent == &fix.node_left_left);
    BOOST_CHECK(fix.node_left.right == &fix.node_left_right);
    BOOST_CHECK(fix.node_left.left == &fix.node_root);
    BOOST_CHECK(fix.node_left.parent == &fix.node_left_left);
    BOOST_CHECK(fix.header.parent == &fix.node_left_left);
    BOOST_CHECK(fix.header.left == &fix.header);
    BOOST_CHECK(fix.header.right == &fix.node_right);
  }
  { // Swap left_left with left
    int_node_fixture fix;
    node_type* left_left = &fix.node_left_left;
    node_type* left = &fix.node_left;
    swap_node_aux(left_left, left);
    BOOST_CHECK(left_left == &fix.node_left_left);
    BOOST_CHECK(left == &fix.node_left);
    BOOST_CHECK(fix.node_left.left == 0);
    BOOST_CHECK(fix.node_left.right == 0);
    BOOST_CHECK(fix.node_left.parent == &fix.node_left_left);
    BOOST_CHECK(fix.node_left_left.left == &fix.node_left);
    BOOST_CHECK(fix.node_left_left.right == &fix.node_left_right);
    BOOST_CHECK(fix.node_left_left.parent == &fix.node_root);
    BOOST_CHECK(&fix.node_left_left == fix.node_root.left);
    BOOST_CHECK(&fix.node_left_left == fix.node_left_right.parent);
  }
  { // Swap left with left_left (invertion)
    int_node_fixture fix;
    node_type* left_left = &fix.node_left_left;
    node_type* left = &fix.node_left;
    swap_node_aux(left, left_left);
    BOOST_CHECK(left_left == &fix.node_left_left);
    BOOST_CHECK(left == &fix.node_left);
    BOOST_CHECK(fix.node_left.left == 0);
    BOOST_CHECK(fix.node_left.right == 0);
    BOOST_CHECK(fix.node_left.parent == &fix.node_left_left);
    BOOST_CHECK(fix.node_left_left.left == &fix.node_left);
    BOOST_CHECK(fix.node_left_left.right == &fix.node_left_right);
    BOOST_CHECK(fix.node_left_left.parent == &fix.node_root);
    BOOST_CHECK(&fix.node_left_left == fix.node_root.left);
    BOOST_CHECK(&fix.node_left_left == fix.node_left_right.parent);
  }
  { // Swap left_right with left
    int_node_fixture fix; // swap with right child
    node_type* left_right = &fix.node_left_right;
    node_type* left = &fix.node_left;
    swap_node_aux(left_right, left);
    BOOST_CHECK(left_right == &fix.node_left_right);
    BOOST_CHECK(left == &fix.node_left);
    BOOST_CHECK(fix.node_left.left == 0);
    BOOST_CHECK(fix.node_left.right == 0);
    BOOST_CHECK(fix.node_left.parent == &fix.node_left_right);
    BOOST_CHECK(fix.node_left_right.left == &fix.node_left_left);
    BOOST_CHECK(fix.node_left_right.right == &fix.node_left);
    BOOST_CHECK(fix.node_left_right.parent == &fix.node_root);
    BOOST_CHECK(&fix.node_left_right == fix.node_root.left);
    BOOST_CHECK(&fix.node_left_right == fix.node_left_left.parent);
  }
  { // Swap left with left_right (invertion)
    int_node_fixture fix;
    node_type* left_right = &fix.node_left_right;
    node_type* left = &fix.node_left;
    swap_node_aux(left, left_right);
    BOOST_CHECK(left_right == &fix.node_left_right);
    BOOST_CHECK(left == &fix.node_left);
    BOOST_CHECK(fix.node_left.left == 0);
    BOOST_CHECK(fix.node_left.right == 0);
    BOOST_CHECK(fix.node_left.parent == &fix.node_left_right);
    BOOST_CHECK(fix.node_left_right.left == &fix.node_left_left);
    BOOST_CHECK(fix.node_left_right.right == &fix.node_left);
    BOOST_CHECK(fix.node_left_right.parent == &fix.node_root);
    BOOST_CHECK(&fix.node_left_right == fix.node_root.left);
    BOOST_CHECK(&fix.node_left_right == fix.node_left_left.parent);
  }
  { // Swap left and root
    int_node_fixture fix;
    node_type* root = &fix.node_root;
    node_type* left = &fix.node_left;
    swap_node_aux(left, root);
    BOOST_CHECK(root == &fix.node_root);
    BOOST_CHECK(left == &fix.node_left);
    BOOST_CHECK(fix.node_left.left == &fix.node_root);
    BOOST_CHECK(fix.node_left.right == &fix.node_right);
    BOOST_CHECK(fix.node_left.parent == &fix.header);
    BOOST_CHECK(fix.node_root.left == &fix.node_left_left);
    BOOST_CHECK(fix.node_root.right == &fix.node_left_right);
    BOOST_CHECK(fix.node_root.parent == &fix.node_left);
    BOOST_CHECK(fix.header.parent == &fix.node_left);
    BOOST_CHECK(&fix.node_root == fix.node_left_left.parent);
    BOOST_CHECK(&fix.node_root == fix.node_left_right.parent);
  }
  { // Swap root and left (invertion)
    int_node_fixture fix;
    node_type* root = &fix.node_root;
    node_type* left = &fix.node_left;
    swap_node_aux(root, left);
    BOOST_CHECK(root == &fix.node_root);
    BOOST_CHECK(left == &fix.node_left);
    BOOST_CHECK(fix.node_left.left == &fix.node_root);
    BOOST_CHECK(fix.node_left.right == &fix.node_right);
    BOOST_CHECK(fix.node_left.parent == &fix.header);
    BOOST_CHECK(fix.node_root.left == &fix.node_left_left);
    BOOST_CHECK(fix.node_root.right == &fix.node_left_right);
    BOOST_CHECK(fix.node_root.parent == &fix.node_left);
    BOOST_CHECK(fix.header.parent == &fix.node_left);
    BOOST_CHECK(&fix.node_root == fix.node_left_left.parent);
    BOOST_CHECK(&fix.node_root == fix.node_left_right.parent);
  }
  { // Swap right and root
    int_node_fixture fix;
    node_type* root = &fix.node_root;
    node_type* right = &fix.node_right;
    swap_node_aux(right, root);
    BOOST_CHECK(root == &fix.node_root);
    BOOST_CHECK(right == &fix.node_right);
    BOOST_CHECK(fix.node_right.left == &fix.node_left);
    BOOST_CHECK(fix.node_right.right == &fix.node_root);
    BOOST_CHECK(fix.node_right.parent == &fix.header);
    BOOST_CHECK(fix.node_root.left == 0);
    BOOST_CHECK(fix.node_root.right == 0);
    BOOST_CHECK(fix.node_root.parent == &fix.node_right);
    BOOST_CHECK(fix.header.parent == &fix.node_right);
    BOOST_CHECK(&fix.node_right == fix.node_left.parent);
  }
  { // Swap root and right (invertion)
    int_node_fixture fix;
    node_type* root = &fix.node_root;
    node_type* right = &fix.node_right;
    swap_node_aux(root, right);
    BOOST_CHECK(root == &fix.node_root);
    BOOST_CHECK(right == &fix.node_right);
    BOOST_CHECK(fix.node_right.left == &fix.node_left);
    BOOST_CHECK(fix.node_right.right == &fix.node_root);
    BOOST_CHECK(fix.node_right.parent == &fix.header);
    BOOST_CHECK(fix.node_root.left == 0);
    BOOST_CHECK(fix.node_root.right == 0);
    BOOST_CHECK(fix.node_root.parent == &fix.node_right);
    BOOST_CHECK(fix.header.parent == &fix.node_right);
    BOOST_CHECK(&fix.node_right == fix.node_left.parent);
  }
}

BOOST_AUTO_TEST_CASE( test_swap_kdtree_node )
{ // swap only position (tested before). Do not swap values!
  using namespace ::spatial::details;
  int2_node_fixture fix;
  int2_node_fixture::node_type* left_left = &fix.node_left_left;
  int2_node_fixture::node_type* right = &fix.node_right;
  swap_node(left_left, right);
  BOOST_CHECK(fix.node_left_left.value == zeros);
  BOOST_CHECK(fix.node_right.value == threes);
  BOOST_CHECK(right == &fix.node_left_left);
  BOOST_CHECK(left_left == &fix.node_right);
}

BOOST_AUTO_TEST_CASE( test_swap_relaxed_kdtree_node )
{ // swap position and weight, but not values!
  using namespace ::spatial::details;
  int2_relaxed_node_fixture fix;
  int2_relaxed_node_fixture::node_type* left = &fix.node_left;
  int2_relaxed_node_fixture::node_type* right = &fix.node_right;
  swap_node(left, right);
  BOOST_CHECK_EQUAL(fix.node_left.weight, 1u);
  BOOST_CHECK_EQUAL(fix.node_right.weight, 3u);
  BOOST_CHECK(fix.node_left.value == ones);
  BOOST_CHECK(fix.node_right.value == threes);
  BOOST_CHECK(right == &fix.node_left);
  BOOST_CHECK(left == &fix.node_right);
}

BOOST_AUTO_TEST_CASE( test_Node_iterator )
{
  using namespace ::spatial::details;
  // increment and decrement tested earlier, check pre/post correctness and
  // derefencing
  {
    quad test_object(1, 2, 3, 4);
    Kdtree_link<quad, quad> test_node;
    test_node.parent = &test_node;
    test_node.left = 0;
    test_node.right = 0;
    test_node.value = test_object;
    Node_iterator<Kdtree_link<quad, quad> >
      iter(&test_node);
    BOOST_CHECK(*iter == test_object);
    BOOST_CHECK_EQUAL(iter->x, 1);
    BOOST_CHECK_EQUAL(iter->y, 2);
    BOOST_CHECK_EQUAL(iter->z, 3);
    BOOST_CHECK_EQUAL(iter->w, 4);
    // should compile: non-const iterator!
    BOOST_CHECK((*iter = test_object) == test_object);
    BOOST_CHECK_EQUAL((iter->x = 3), 3);
  }
  {
    quad test_object(1, 2, 3, 4);
    Relaxed_kdtree_link<quad, quad> test_node;
    test_node.parent = &test_node;
    test_node.left = 0;
    test_node.right = 0;
    test_node.value = test_object;
    Node_iterator<Relaxed_kdtree_link<quad, quad> >
      iter(&test_node);
    BOOST_CHECK(*iter == test_object);
    BOOST_CHECK_EQUAL(iter->x, 1);
    BOOST_CHECK_EQUAL(iter->y, 2);
    BOOST_CHECK_EQUAL(iter->z, 3);
    BOOST_CHECK_EQUAL(iter->w, 4);
    // should compile: non-const iterator!
    BOOST_CHECK((*iter = test_object) == test_object);
    BOOST_CHECK_EQUAL((iter->x = 3), 3);
  }
  {
    // Make sure that constant iterator does not invalidate anything
    int2_node_fixture fix;
    Node_iterator<Kdtree_link<int2, int2> > iter(&fix.node_root);
    BOOST_CHECK(*iter == twos);
  }
  {
    int2_node_fixture fix;
    Node_iterator<Kdtree_link<int2, int2> > iter(&fix.node_root);
    BOOST_REQUIRE((++iter).node == &fix.node_right);
    BOOST_REQUIRE((--iter).node == &fix.node_root);
    BOOST_REQUIRE((iter++).node == &fix.node_root);
    BOOST_REQUIRE(iter.node == &fix.node_right);
    BOOST_REQUIRE((iter--).node == &fix.node_right);
    BOOST_REQUIRE(iter.node == &fix.node_root);
  }
}

BOOST_AUTO_TEST_CASE( test_Const_Node_iterator )
{
  using namespace ::spatial::details;
  // increment and decrement tested earlier, check pre/post correctness and
  // derefencing
  {
    quad test_object(1, 2, 3, 4);
    Kdtree_link<quad, quad> test_node;
    test_node.parent = &test_node;
    test_node.left = 0;
    test_node.right = 0;
    test_node.value = test_object;
    Const_node_iterator<Kdtree_link<quad, quad> >
      iter(&test_node);
    BOOST_CHECK(*iter == test_object);
    BOOST_CHECK_EQUAL(iter->x, 1);
    BOOST_CHECK_EQUAL(iter->y, 2);
    BOOST_CHECK_EQUAL(iter->z, 3);
    BOOST_CHECK_EQUAL(iter->w, 4);
  }
  {
    quad test_object(1, 2, 3, 4);
    Relaxed_kdtree_link<quad, quad> test_node;
    test_node.parent = &test_node;
    test_node.left = 0;
    test_node.right = 0;
    test_node.value = test_object;
    Const_node_iterator<Relaxed_kdtree_link<quad, quad> >
      iter(&test_node);
    BOOST_CHECK(*iter == test_object);
    BOOST_CHECK_EQUAL(iter->x, 1);
    BOOST_CHECK_EQUAL(iter->y, 2);
    BOOST_CHECK_EQUAL(iter->z, 3);
    BOOST_CHECK_EQUAL(iter->w, 4);
  }
  {
    // Make sure that constant iterator does not invalidate anything
    int2_node_fixture fix;
    Const_node_iterator<Kdtree_link<int2, int2> >
      iter(&fix.node_root);
    BOOST_CHECK(*iter == twos);
  }
  {
    int2_node_fixture fix;
    Const_node_iterator<Kdtree_link<int2, int2> >
      iter(&fix.node_root);
    BOOST_REQUIRE((++iter).node == &fix.node_right);
    BOOST_REQUIRE((--iter).node == &fix.node_root);
    BOOST_REQUIRE((iter++).node == &fix.node_root);
    BOOST_REQUIRE(iter.node == &fix.node_right);
    BOOST_REQUIRE((iter--).node == &fix.node_right);
    BOOST_REQUIRE(iter.node == &fix.node_root);
  }
}

BOOST_AUTO_TEST_CASE( test_Preorder_node_iterator )
{
  using namespace ::spatial::details;
  // increment and decrement tested earlier, check pre/post correctness and
  // derefencing
  {
    quad test_object(1, 2, 3, 4);
    Kdtree_link<quad, quad> test_node;
    test_node.parent = &test_node;
    test_node.left = 0;
    test_node.right = 0;
    test_node.value = test_object;
    Preorder_node_iterator<Kdtree_link<quad, quad> >
      iter(&test_node);
    BOOST_CHECK(*iter == test_object);
    BOOST_CHECK_EQUAL(iter->x, 1);
    BOOST_CHECK_EQUAL(iter->y, 2);
    BOOST_CHECK_EQUAL(iter->z, 3);
    BOOST_CHECK_EQUAL(iter->w, 4);
  }
  {
    int2_node_fixture fix;
    Preorder_node_iterator<Kdtree_link<int2, int2> >
      iter(&fix.node_root);
    BOOST_REQUIRE((++iter).node == &fix.node_left);
    BOOST_REQUIRE((iter++).node == &fix.node_left);
    BOOST_REQUIRE(iter.node == &fix.node_left_left);
  }
  {
    int2_node_fixture fix;
    Preorder_node_iterator<Kdtree_link<int2, int2> >
      a(&fix.node_root), b(&fix.node_root), c(&fix.node_left);
    BOOST_CHECK(a == b);
    BOOST_CHECK(a != c);
  }
  {
    int2_node_fixture fix;
    Preorder_node_iterator<Kdtree_link<int2, int2> >
      a(&fix.node_root), b(a);
    BOOST_CHECK(a == b);
  }
}