//***********************************************************
// 17/10/2020	1.0.0	Rémi Saint-Amant   Creation
//***********************************************************
#include "LeucopisModel.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>
#include <boost/math/distributions/lognormal.hpp>
#include "ModelBase/SimulatedAnnealingVector.h"

//Best parameters (separate)
//			Th1		Th2		mu		s		delta
//La G1		3.2		12.5	199.5	31.38	44
//La G2		3.4		40.5	861.9	54.45	39
//Ln		4.6		12.4	394.9	22.87	44

//Final parameters
//
//Th1 =  2.9
//Th2 = 19.1
//delta = 45 (February 15)
//		 mu		 s
//La G1	239.6	41.4
//La G2	876.0	55.6
//Ln	625.2	38.3

using namespace WBSF::HOURLY_DATA;
using namespace std;

static const bool BEGIN_JULY = true;
static const size_t FIRST_Y = BEGIN_JULY ? 1 : 0;

namespace WBSF
{
	//static const CDegreeDays::TDailyMethod DD_METHOD = CDegreeDays::MODIFIED_ALLEN_WAVE;
	static const CDegreeDays::TDailyMethod DD_METHOD = CDegreeDays::ALLEN_WAVE;
	enum { O_CDD, O_EMERGENCE_LA_G1, O_EMERGENCE_LA_G2, O_EMERGENCE_LP, NB_OUTPUTS };

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

	CLeucopisModel::CLeucopisModel()
	{
		//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.0 (2020)";

		m_bCumul = false;

		m_P[Τᴴ¹] = 2.9;
		m_P[Τᴴ²] = 19.1;
		m_P[delta] = 45;
		m_P[μ1] = 239.6;
		m_P[ѕ1] = 41.4;
		m_P[μ2] = 876.0;
		m_P[ѕ2] = 55.6;
		m_P[μ3] = 625.2;
		m_P[ѕ3] = 38.3;

	}

	CLeucopisModel::~CLeucopisModel()
	{
	}

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

		size_t c = 0;

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

		if (parameters.size() == 1 + NB_PARAMS)
		{
			for (size_t p = 0; p < NB_PARAMS; p++)
				m_P[p] = parameters[c++].GetFloat();
		}

		return msg;
	}

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

		/*	if (m_weather.GetNbYears() < 2)
			{
				msg.ajoute("Laricobius Osakensis 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_OUTPUTS, -999);

		CModelStatVector CDD;
		GetCDD(m_weather, CDD);

		boost::math::logistic_distribution<double> emerge_dist1(m_P[μ1], m_P[ѕ1]);
		boost::math::logistic_distribution<double> emerge_dist2(m_P[μ2], m_P[ѕ2]);
		boost::math::logistic_distribution<double> emerge_dist3(m_P[μ3], m_P[ѕ3]);
		//boost::math::logistic_distribution<double> emerge_dist4(m_P[μ4], m_P[ѕ4]);

		for (CTRef d = p.Begin(); d <= p.End(); d++)
		{
			m_output[d][O_CDD] = CDD[d][0];
			if (CDD[d][0] > -999)
			{
				m_output[d][O_EMERGENCE_LA_G1] = Round(100 * cdf(emerge_dist1, CDD[d][0]), 1);
				m_output[d][O_EMERGENCE_LA_G2] = Round(100 * cdf(emerge_dist2, CDD[d][0]), 1);
				m_output[d][O_EMERGENCE_LP] = Round(100 * cdf(emerge_dist3, CDD[d][0]), 1);
				//m_output[d][O_EMERGENCE_LN] = Round(100 * cdf(emerge_dist4, CDD[d][0]), 1);
			}
		}

		return msg;
	}

	//void CLeucopisModel::GetCDD(int year, const CWeatherYears& weather, CModelStatVector& CDD)
	//{
	//	CDegreeDays DDmodel(DD_METHOD, m_P[Τᴴ¹], m_P[Τᴴ²]);
	//	CModelStatVector DD;
	//	DDmodel.Execute(weather[year], DD);

	//	CDD.Init(DD.GetTPeriod(), 1, 0.0);

	//	//for (CTRef TRef = DD.GetFirstTRef(); TRef <= DD.GetLastTRef(); TRef++)
	//	CDD[0][0] = DD[0][CDegreeDays::S_DD];
	//	for (size_t i = 1; i < DD.size(); i++)
	//		CDD[i][0] = CDD[i - 1][0] + DD[i][CDegreeDays::S_DD];
	//}

	void CLeucopisModel::GetCDD(const CWeatherYears& weather, CModelStatVector& CDD)
	{
		CDegreeDays DDmodel(DD_METHOD, m_P[Τᴴ¹], m_P[Τᴴ²]);
		CModelStatVector DD;
		DDmodel.Execute(weather, DD);

		CDD.Init(DD.GetTPeriod(), 1, -999);

		for (size_t y = 1; y < weather.GetNbYears(); y++)
		{
			CTPeriod p = weather[y].GetEntireTPeriod();
			p.Begin() = p.Begin() + int(m_P[delta]);
			CDD[p.Begin()][0] = DD[p.Begin()][CDegreeDays::S_DD];

			for (CTRef TRef = p.Begin() + 1; TRef <= p.End(); TRef++)
				CDD[TRef][0] = CDD[TRef - 1][0] + DD[TRef][CDegreeDays::S_DD];
		}

	}

	enum TSpecies { S_LA_G1, S_LA_G2, S_LP, S_LN };
	enum TInput { I_N, I_S/*, I_P, I_CDD*/, NB_INPUTS };
	void CLeucopisModel::AddDailyResult(const StringVector& header, const StringVector& data)
	{
		ASSERT(data.size() == 13);
		//SYC	site	Year	collection	col_date	emerge_date	daily_count	species	n_days P Time

		if (data[7] != "LN")
		{

			CSAResult obs;

			CStatistic egg_creation_date;

			obs.m_ref.FromFormatedString(data[5]);
			obs.m_obs.resize(NB_INPUTS);
			obs.m_obs[I_N] = stod(data[6]);
			//obs.m_obs[I_S] = ;
			//obs.m_obs[I_P] = stod(data[10]);
			//obs.m_obs[I_CDD] = stod(data[11]);
			//obs.m_obs[I_P] = stod(data[12]);

			if (data[7] == "LA" && data[8] == "1")
				obs.m_obs[I_S] = S_LA_G1;
			else if (data[7] == "LA" && data[8] == "2")
				obs.m_obs[I_S] = S_LA_G2;
			else if (data[7] == "LP")
				obs.m_obs[I_S] = S_LP;
			else if (data[7] == "LN")
				obs.m_obs[I_S] = S_LN;

			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 CLeucopisModel::IsParamValid()const
	{
		bool bValid = true;

		if (m_P[Τᴴ¹] >= m_P[Τᴴ²])
			bValid = false;

		return bValid;
	}

	//static const int ROUND_VAL = 4;
	//CTRef CLeucopisModel::GetEmergence(const CWeatherYear& weather)
	//{
	//	CTPeriod p = weather.GetEntireTPeriod(CTM(CTM::DAILY));

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

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

	//		//if (ii < m_ADE[ʎ0])
	//			sumDD += DD;
	//	}

	//	boost::math::logistic_distribution<double> begin_dist(m_ADE[ʎ2], m_ADE[ʎ3]);
	//	int begin = (int)Round(m_ADE[ʎ0] + m_ADE[ʎ1] * cdf(begin_dist, sumDD), 0);
	//	return  p.Begin() + begin;
	//}
	enum TPout {P_CDD, P_CE, LA_G1= P_CE, P_LA_G2, P_LP, P_LN, NB_P};//CE = cumulative emergence
	void CLeucopisModel::GetPobs(CModelStatVector& P)
	{
		string ID = GetInfo().m_loc.m_ID;
		string SY = ID.substr(0, ID.length() - 2);

		//compute CDD for all temperature rprofile
		array< double, 4> total = { 0 };
		vector<tuple<double, CTRef, double, bool, size_t>> d;
		const CSimulatedAnnealingVector& SA = GetSimulatedAnnealingVector();

		for (size_t i = 0; i < SA.size(); i++)
		{
			string IDi = SA[i]->GetInfo().m_loc.m_ID;
			string SYi = IDi.substr(0, IDi.length() - 2);
			if (SYi == SY)
			{
				CModelStatVector CDD;
				GetCDD(SA[i]->m_weather, CDD);
				const CSAResultVector& v = SA[i]->GetSAResult();
				for (size_t ii = 0; ii < v.size(); ii++)
				{
					d.push_back(make_tuple(CDD[v[ii].m_ref][0], v[ii].m_ref, v[ii].m_obs[I_N], IDi == ID, v[ii].m_obs[I_S]));
					total[v[ii].m_obs[I_S]] += v[ii].m_obs[I_N];
				}
			}
		}

		sort(d.begin(), d.end());

		P.Init(m_weather.GetEntireTPeriod(CTM::DAILY), NB_P, 0);
		array< double, 4> sum = { 0 };
		for (size_t i = 0; i < d.size(); i++)
		{
			size_t s = std::get<4>(d[i]);
			sum[s] += std::get<2>(d[i]);
			if (std::get<3>(d[i]))
			{
				CTRef Tref = std::get<1>(d[i]);
				/*double obsP = -999;
				for (size_t k = 0; k < m_SAResult.size(); k++)
					if (m_SAResult[k].m_ref == Tref)
						obsP = m_SAResult[k].m_obs[I_P];*/

				double CDD = std::get<0>(d[i]);
				double p = Round(100 * sum[s] / total[s], 1);

				P[Tref][P_CDD] = CDD;
				P[Tref][P_CE + s] = p;
			}
		}
	}

	void CLeucopisModel::CalibrateEmergence(CStatisticXY& stat)
	{
		if (m_SAResult.empty())
			return;

		//boost::math::lognormal_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);

		//boost::math::weibull_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);
		//boost::math::beta_distribution<double> emerge_dist(m_P[μ], m_P[ѕ]);
		//boost::math::exponential_distribution<double> emerge_dist(m_P[ѕ]);
		//boost::math::rayleigh_distribution<double> emerge_dist(m_P[ѕ]);

		//CModelStatVector CDD;
		//GetCDD(m_weather, CDD);

		double n = 0;

		for (size_t i = 0; i < m_SAResult.size(); i++)
			n += m_SAResult[i].m_obs[I_N];

		CModelStatVector P;
		GetPobs(P);

		//array<boost::math::logistic_distribution<double>,4> emerge_dist(mu, S);
		//		array<boost::math::logistic_distribution<double>, 4> emerge_dist = { {m_P[μ1], m_P[ѕ1], m_P[μ1], m_P[ѕ1]} };

		for (size_t i = 0; i < m_SAResult.size(); i++)
		{
			size_t s = m_SAResult[i].m_obs[I_S];
			double mu = m_P[μ1 + 2 * s];
			double S = m_P[ѕ1 + 2 * s];

			boost::math::logistic_distribution<double> emerge_dist(mu, S);

			double CDD = P[m_SAResult[i].m_ref][P_CDD];
			double obs = P[m_SAResult[i].m_ref][P_CE+s];
			ASSERT(obs >= 0 && obs <= 100);

			double sim = Round(100 * cdf(emerge_dist, max(0.0, CDD)), 1);
			for (size_t ii = 0; ii < log(n); ii++)
				stat.Add(obs, sim);

		}//for all results

		return;

	}

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

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

		if (!IsParamValid())
			return;

		return CalibrateEmergence(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_L1, 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
		//	}
		//}
	}

	//void CLeucopisModel::FinalizeStat(CStatisticXY& stat)
	//{
	//
	//}
}


#include <chrono>
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include "relative_pose/relative_pose.hpp"
#include "precomp.hpp"
#include "relative_pose_estimator.hpp"
#include "nullqr.h"

namespace pc_2pot
{
using namespace cv;

class PC2POTNullEEstimatorCallback CV_FINAL : public RelativePoseEstimatorCallback
{
    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const CV_OVERRIDE
    {
        Mat3d q1 = _m1.getMat(), q2 = _m2.getMat();
        CV_Assert(q1.cols == 1 && q2.cols == 1);

        double abuf[2][3], t[1][3];
        Mat1d A(2, 3, &abuf[0][0]);
        for (int si = 0; si < 2; ++si)
            A.row(si) = Mat(q2(si, 0)).reshape(1, 1) * skew(q1(si, 0));

        nullQR<2, 3>(abuf, t);
        Mat1d tvec(1, 3, &t[0][0]);
        _model.assign(skew(tvec));

        return 1;
    }
};

}

namespace cv
{
int RANSACUpdateNumIters_2pot( double p, double ep, int modelPoints, int maxIters )
{
    if( modelPoints <= 0 )
        CV_Error( Error::StsOutOfRange, "the number of model points should be positive" );

    p = MAX(p, 0.);
    p = MIN(p, 1.);
    ep = MAX(ep, 0.);
    ep = MIN(ep, 1.);

    // avoid inf's & nan's
    double num = MAX(1. - p, DBL_MIN);
    double denom = 1. - std::pow(1. - ep, modelPoints);
    if( denom < DBL_MIN )
        return 0;

    num = std::log(num);
    denom = std::log(denom);

    return denom >= 0 || -num >= maxIters*(-denom) ? maxIters : cvRound(num/denom);
}
class RANSACPointSetRegistrator_2pot : public PointSetRegistrator
{
public:
    RANSACPointSetRegistrator_2pot(const Ptr<PointSetRegistrator::Callback>& _cb=Ptr<PointSetRegistrator::Callback>(),
                              int _modelPoints=0, double _threshold=0, double _confidence=0.99, int _maxIters=1000)
      : cb(_cb), modelPoints(_modelPoints), threshold(_threshold), confidence(_confidence), maxIters(_maxIters) {}

    int findInliers( const Mat& m1, const Mat& m2, const Mat& model, Mat& err, Mat& mask, double thresh ) const
    {
        cb->computeError( m1, m2, model, err );
        mask.create(err.size(), CV_8U);

        CV_Assert( err.isContinuous() && err.type() == CV_32F && mask.isContinuous() && mask.type() == CV_8U);
        const float* errptr = err.ptr<float>();
        uchar* maskptr = mask.ptr<uchar>();
        float t = (float)(thresh*thresh);
        int i, n = (int)err.total(), nz = 0;
        for( i = 0; i < n; i++ )
        {
            int f = errptr[i] <= t;
            maskptr[i] = (uchar)f;
            nz += f;
        }
        // {
        //     Mat E = model;
        //     Mat R1, R2, t;
        //     decomposeEssentialMat(E, R1, R2, t);
        //     t *= (t.at<double>(2) > 0 ? 1 : -1);
        //     std::cout << "---------------" << std::endl;
        //     std::cout << "t = " << t.t() << " " << trace(R1)[0] << " " << trace(R2)[0] << " " << nz << std::endl;
        // }
        return nz;
    }

    bool getSubset( const Mat& m1, const Mat& m2,
                    Mat& ms1, Mat& ms2, RNG& rng,
                    int maxAttempts=1000 ) const
    {
        cv::AutoBuffer<int> _idx(modelPoints);
        int* idx = _idx.data();
        int i = 0, j, k, iters = 0;
        int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
        int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
        int esz1 = (int)m1.elemSize1()*d1, esz2 = (int)m2.elemSize1()*d2;
        int count = m1.checkVector(d1), count2 = m2.checkVector(d2);
        const int *m1ptr = m1.ptr<int>(), *m2ptr = m2.ptr<int>();

        ms1.create(modelPoints, 1, CV_MAKETYPE(m1.depth(), d1));
        ms2.create(modelPoints, 1, CV_MAKETYPE(m2.depth(), d2));

        int *ms1ptr = ms1.ptr<int>(), *ms2ptr = ms2.ptr<int>();

        CV_Assert( count >= modelPoints && count == count2 );
        CV_Assert( (esz1 % sizeof(int)) == 0 && (esz2 % sizeof(int)) == 0 );
        esz1 /= sizeof(int);
        esz2 /= sizeof(int);

        for(; iters < maxAttempts; iters++)
        {
            for( i = 0; i < modelPoints && iters < maxAttempts; )
            {
                int idx_i = 0;
                for(;;)
                {
                    idx_i = idx[i] = rng.uniform(0, count);
                    for( j = 0; j < i; j++ )
                        if( idx_i == idx[j] )
                            break;
                    if( j == i )
                        break;
                }
                for( k = 0; k < esz1; k++ )
                    ms1ptr[i*esz1 + k] = m1ptr[idx_i*esz1 + k];
                for( k = 0; k < esz2; k++ )
                    ms2ptr[i*esz2 + k] = m2ptr[idx_i*esz2 + k];
                i++;
            }
            if( i == modelPoints && !cb->checkSubset(ms1, ms2, i) )
                continue;
            break;
        }

        return i == modelPoints && iters < maxAttempts;
    }

    bool run(InputArray _m1, InputArray _m2, OutputArray _model, OutputArray _mask) const CV_OVERRIDE
    {
        bool result = false;
        Mat m1 = _m1.getMat(), m2 = _m2.getMat();
        Mat err, mask, model, bestModel, ms1, ms2;

        int iter, niters = MAX(maxIters, 1);
        int d1 = m1.channels() > 1 ? m1.channels() : m1.cols;
        int d2 = m2.channels() > 1 ? m2.channels() : m2.cols;
        int count = m1.checkVector(d1), count2 = m2.checkVector(d2), maxGoodCount = 0;

        RNG rng((uint64)-1);

        CV_Assert( cb );
        CV_Assert( confidence > 0 && confidence < 1 );

        CV_Assert( count >= 0 && count2 == count );
        if( count < modelPoints )
            return false;

        Mat bestMask0, bestMask;

        if( _mask.needed() )
        {
            _mask.create(count, 1, CV_8U, -1, true);
            bestMask0 = bestMask = _mask.getMat();
            CV_Assert( (bestMask.cols == 1 || bestMask.rows == 1) && (int)bestMask.total() == count );
        }
        else
        {
            bestMask.create(count, 1, CV_8U);
            bestMask0 = bestMask;
        }

        if( count == modelPoints )
        {
            if( cb->runKernel(m1, m2, bestModel) <= 0 )
                return false;
            bestModel.copyTo(_model);
            bestMask.setTo(Scalar::all(1));
            return true;
        }

        for( iter = 0; iter < niters; iter++ )
        {
            int i, nmodels;
            if( count > modelPoints )
            {
                bool found = getSubset( m1, m2, ms1, ms2, rng, 10000 );
                if( !found )
                {
                    if( iter == 0 )
                        return false;
                    break;
                }
            }

            nmodels = cb->runKernel( ms1, ms2, model );
            if( nmodels <= 0 )
                continue;
            CV_Assert( model.rows % nmodels == 0 );
            Size modelSize(model.cols, model.rows/nmodels);

            for( i = 0; i < nmodels; i++ )
            {
                Mat model_i = model.rowRange( i*modelSize.height, (i+1)*modelSize.height );
                int goodCount = findInliers( m1, m2, model_i, err, mask, threshold );

                if( goodCount > MAX(maxGoodCount, modelPoints-1) )
                {
                    // Mat cheir_mask;
                    // checkPositiveDepth(m1, m2, model_i, mask, cheir_mask);
                    // if (!cheir_mask.empty()) mask = cheir_mask;
                    // goodCount = countNonZero(mask);
                    // if (goodCount > maxGoodCount)
                    {
                        std::swap(mask, bestMask);
                        model_i.copyTo(bestModel);
                        maxGoodCount = goodCount;
                        niters = RANSACUpdateNumIters_2pot( confidence, (double)(count - goodCount)/count, modelPoints, niters );
                    }
                }
            }
        }

        if( maxGoodCount > 0 )
        {
            if( bestMask.data != bestMask0.data )
            {
                if( bestMask.size() == bestMask0.size() )
                    bestMask.copyTo(bestMask0);
                else
                    transpose(bestMask, bestMask0);
            }
            bestModel.copyTo(_model);
            result = true;
        }
        else
            _model.release();

        return result;
    }

    void setCallback(const Ptr<PointSetRegistrator::Callback>& _cb) CV_OVERRIDE { cb = _cb; }

    Ptr<PointSetRegistrator::Callback> cb;
    int modelPoints;
    double threshold;
    double confidence;
    int maxIters;
};

Mat estimateRelativePose_PC2POT(
        InputArray _rays1, InputArray _rays2,
        int method, double prob, double threshold, OutputArray _mask)
{
    // CV_INSTRUMENT_REGION();
    Mat rays1, rays2;
    processInputArray(_rays1, _rays2, rays1, rays2);

    Mat models;
    if( method == RANSAC )
    //     createRANSACPointSetRegistrator_2pot(
    //             makePtr<pc_2pot::PC2POTNullEEstimatorCallback>(), 2, threshold, prob)->run(
    //             rays1, rays2, models, _mask);
    {
        auto reg = RANSACPointSetRegistrator_2pot(
                makePtr<pc_2pot::PC2POTNullEEstimatorCallback>(), 4, threshold, prob);
        reg.run(rays1, rays2, models, _mask);
    }
    else
        createLMeDSPointSetRegistrator(
                makePtr<pc_2pot::PC2POTNullEEstimatorCallback>(), 2, prob)->run(
                rays1, rays2, models, _mask);

    return models;
}

}


#ifndef TYPES_HH
#define TYPES_HH

#include <Eigen/Dense>
#include <array>
#include <type_traits>
#include "unused.hh"
typedef double Real;

#include <MeshFEM_export.h>

template<size_t N>
using VectorND = Eigen::Matrix<Real, N, 1, Eigen::ColMajor, N, 1>;
template<size_t N>
using PointND = VectorND<N>;
template<size_t N>
using IVectorND = std::array<int, N>;

typedef  PointND<3>  Point3D;
typedef VectorND<3> Vector3D;
typedef  PointND<2>  Point2D;
typedef VectorND<2> Vector2D;

MESHFEM_EXPORT extern Eigen::IOFormat pointFormatter;

// Types templated on floating point representation.
template<typename Real_> using  Vec3_T = Eigen::Matrix<Real_, 3, 1>;
template<typename Real_> using   Pt3_T = Vec3_T<Real_>;
template<typename Real_> using  Vec2_T = Eigen::Matrix<Real_, 2, 1>;
template<typename Real_> using  VecX_T = Eigen::Matrix<Real_, Eigen::Dynamic, 1>;
template<typename Real_> using  Mat3_T = Eigen::Matrix<Real_, 3, 3>;
template<typename Real_> using  Mat2_T = Eigen::Matrix<Real_, 2, 2>;
template<typename Real_> using MatX3_T = Eigen::Matrix<Real_, Eigen::Dynamic, 3>;

extern Eigen::IOFormat pointFormatter;

template<class EmbeddingSpace, class Enable = void> struct Padder;
template<class EmbeddingSpace, class Enable = void> struct Truncator;

template<class EigenType, int RowSize, int ColSize, class Enable = void>
struct isMatrixOfSize : std::false_type { };

template<class EigenType, int RowSize, int ColSize>
struct isMatrixOfSize<EigenType, RowSize, ColSize, typename std::enable_if<(EigenType::RowsAtCompileTime == RowSize) &&
                                                                           (EigenType::ColsAtCompileTime == ColSize), void>::type> : std::true_type { };

template<class EigenType, class Enable = void>
struct isCompileTimeSizedEigen : std::false_type { };

template<class EigenType>
struct isCompileTimeSizedEigen<EigenType, typename std::enable_if<(EigenType::RowsAtCompileTime > 0) &&
                                                                  (EigenType::ColsAtCompileTime > 0), void>::type> : std::true_type { };

template<class EigenType, int RowSize, int ColSize, typename T = void>
using EnableIfMatrixOfSize = typename std::enable_if<isMatrixOfSize<EigenType, RowSize, ColSize>::value, T>::type;

template<class EigenType, int VectorSize, typename T = void>
using EnableIfVectorOfSize = EnableIfMatrixOfSize<EigenType, VectorSize, 1, T>;

template<class EigenType> using V3MatchingScalarType = Eigen::Matrix<typename EigenType::Scalar, 3, 1>;
template<class EigenType> using V2MatchingScalarType = Eigen::Matrix<typename EigenType::Scalar, 2, 1>;
template<class EigenType> using V1MatchingScalarType = Eigen::Matrix<typename EigenType::Scalar, 1, 1>;

// Padding, truncation of 2D, 3D vectors
template<class EigenType> struct    Padder<EigenType, EnableIfVectorOfSize<EigenType, 1>> { static V3MatchingScalarType<EigenType> run(const EigenType &p) { return V3MatchingScalarType<EigenType>(p[0],  0.0, 0.0); } };
template<class EigenType> struct    Padder<EigenType, EnableIfVectorOfSize<EigenType, 2>> { static V3MatchingScalarType<EigenType> run(const EigenType &p) { return V3MatchingScalarType<EigenType>(p[0], p[1], 0.0); } };
template<class EigenType> struct    Padder<EigenType, EnableIfVectorOfSize<EigenType, 3>> { static const EigenType &               run(const EigenType &p) { return p; } }; // pass-through
template<class EigenType> struct Truncator<EigenType, EnableIfVectorOfSize<EigenType, 1>> { template<typename InEigenType> static       EnableIfVectorOfSize<InEigenType, 3, V1MatchingScalarType<EigenType>>  run(const InEigenType &pt3D) { if ((std::abs(pt3D[1]) > 1e-6) || (std::abs(pt3D[1]) > 1e-6)) throw std::runtime_error("Nonzero y or z component in embedded Point1D"); return V1MatchingScalarType<EigenType>(pt3D[0]); }
                                                                                template<typename InEigenType> static       EnableIfVectorOfSize<InEigenType, 2, V1MatchingScalarType<EigenType>>  run(const InEigenType &pt2D) { if ( std::abs(pt2D[1]) > 1e-6                               ) throw std::runtime_error("Nonzero y component in embedded Point1D");      return V1MatchingScalarType<EigenType>(pt2D[0]); }
                                                                                template<typename InEigenType> static const EnableIfVectorOfSize<InEigenType, 1,                     InEigenType> &run(const InEigenType &pt1D) { return pt1D; } }; // pass-through
template<class EigenType> struct Truncator<EigenType, EnableIfVectorOfSize<EigenType, 2>> { template<typename InEigenType> static       EnableIfVectorOfSize<InEigenType, 3, V2MatchingScalarType<EigenType>>  run(const InEigenType &pt3D) { if (std::abs(pt3D[2]) > 1e-6) throw std::runtime_error("Nonzero z component in embedded Point2D"); return V2MatchingScalarType<EigenType>(pt3D[0], pt3D[1]); }
                                                                                template<typename InEigenType> static const EnableIfVectorOfSize<InEigenType, 2,                     InEigenType> &run(const InEigenType &pt2D) { return pt2D; } }; // pass-through
template<class EigenType> struct Truncator<EigenType, EnableIfVectorOfSize<EigenType, 3>> { template<typename InEigenType> static const EnableIfVectorOfSize<InEigenType, 3,                     InEigenType> &run(const InEigenType &pt3D) { return pt3D; } }; // pass-through

// Provide padding/truncation for points of eigen type.
template<                       class InPointDerived> V3MatchingScalarType<InPointDerived> padTo3D(const Eigen::MatrixBase<InPointDerived> &p) { return    Padder<Eigen::MatrixBase< InPointDerived>>::run(p); }
template<class OutPointDerived, class InPointDerived> OutPointDerived               truncateFrom3D(const Eigen::MatrixBase<InPointDerived> &p) { return Truncator<Eigen::MatrixBase<OutPointDerived>>::run(p).template cast<typename OutPointDerived::Scalar>(); }

// Also provide padding/truncation for points of eigen type nested inside, e.g., a MeshIO::IOVertex instance.
template<class InVertex                       , class NestedPointType = decltype(InVertex().point)> V3MatchingScalarType<NestedPointType> padTo3D(const InVertex &v) { return    Padder<NestedPointType                   >::run(v.point); }
template<class OutPointDerived, class InVertex, class NestedPointType = decltype(InVertex().point)> OutPointDerived                truncateFrom3D(const InVertex &v) { return Truncator<Eigen::MatrixBase<OutPointDerived>>::run(v.point).template cast<typename OutPointDerived::Scalar>(); }

// Compile-time sizes with compile-time checking
template<class EmbeddingSpace, class InputDerived, typename std::enable_if<isCompileTimeSizedEigen<  InputDerived>::value &&
                                                                           isCompileTimeSizedEigen<EmbeddingSpace>::value, int>::type = 0>
EmbeddingSpace truncateFromND(const Eigen::DenseBase<InputDerived> &p) {
    constexpr int  inRows =   InputDerived::RowsAtCompileTime,
                   inCols =   InputDerived::ColsAtCompileTime,
                  outRows = EmbeddingSpace::RowsAtCompileTime,
                  outCols = EmbeddingSpace::ColsAtCompileTime;
    static_assert((inRows > 0) && (outRows > 0), "Vectors must be statically sized, nonempty");
    static_assert((inCols == 1) && (outCols == 1), "We operate only on vectors");
    static_assert(inRows >= outRows, "Truncation cannot upsize");
    EmbeddingSpace result = p.template head<outRows>();
    for (int i = outRows; i < inRows; ++i) {
        if (std::abs(p[i]) > 1e-6)
            throw std::runtime_error("Nonzero component truncated.");
    }
    return result;
}

// Dynamic input size, compile-time output size with partial compile-time checking.
template<class EmbeddingSpace, class InputDerived, typename std::enable_if<!isCompileTimeSizedEigen<  InputDerived>::value &&
                                                                            isCompileTimeSizedEigen<EmbeddingSpace>::value, int>::type = 0>
EmbeddingSpace truncateFromND(const Eigen::DenseBase<InputDerived> &p) {
    constexpr int outRows = EmbeddingSpace::RowsAtCompileTime,
                  outCols = EmbeddingSpace::ColsAtCompileTime;
    const     int  inRows = p.rows();
    static_assert(outRows > 0, "Output vector must be statically sized, nonempty");
    static_assert(outCols == 1, "Output must be a vector");

    assert((inRows >= outRows) && "Truncation cannot upsize");
    assert((p.cols() == outCols) && "Input must be a vector");
    EmbeddingSpace result = p.template head<outRows>();
    for (int i = outRows; i < inRows; ++i) {
        if (std::abs(p[i]) > 1e-6)
            throw std::runtime_error("Nonzero component truncated.");
    }
    return result;
}

// Work around alignment issues for C++ versions before C++17:
// http://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html
#include <Eigen/StdVector>
#include <vector>
template<typename T>
using aligned_std_vector = std::vector<T, Eigen::aligned_allocator<T>>;

#endif /* end of include guard: TYPES_HH */


#ifndef __num_t_hpp__
#define __num_t_hpp__

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>

using int_t = boost::multiprecision::number
<
   boost::multiprecision::cpp_int_backend<>,
   boost::multiprecision::expression_template_option::et_off
>;

using num_t = boost::multiprecision::number
<
   boost::multiprecision::cpp_dec_float<1024>,
   boost::multiprecision::expression_template_option::et_off
>;

#endif


#include <iostream>

#include <adept_arrays.h>

#include "Timer.h"

template<bool IsActive>
double
time_operation(int n, int nrepeat, bool is_col_major)
{
  adept::Array<2,double,IsActive> A, B, C;
  Timer timer;
  int matmul_timer_id = timer.new_activity("matmul");
  if (is_col_major) {
    A.resize_column_major(adept::expression_size(n,n));
    B.resize_column_major(adept::expression_size(n,n));
    C.resize_column_major(adept::expression_size(n,n));
  }
  else {
    A.resize(n,n);
    B.resize(n,n);
    C.resize(n,n);
  }
  for (int irepeat = -nrepeat/10; irepeat < nrepeat; ++irepeat) {
    A = 1.1;
    B = 2.2;
    if (IsActive) {
      adept::active_stack()->new_recording();
    }
    if (irepeat >= 0) {
      timer.start(matmul_timer_id);
    }
    C = A ** B;
    if (irepeat >= 0) {
      timer.stop();
    }
  }
  if (IsActive && n < 8) {
    std::cout << "C=" << C;
    std::cout << *adept::active_stack();
    adept::active_stack()->print_statements();
  }
  return timer.timing(matmul_timer_id) / nrepeat;
}

int
main(int argc, char* argv[])
{
  int ibegin = 1;
  int iend = 18;
  int nrepeat = 10;
  bool is_col_major = false;

  adept::Stack stack;
  int n = 2;
  std::cout << "Dense N-by-N matrix-matrix multiplication\n";
  std::cout << " N        inactive time (us)   inactive flops    active time (us)    active flops\n";
  for (int i = ibegin; i <= iend; ++i) {
    std::cout << n << "  ";

    double t = time_operation<false>(n, nrepeat, is_col_major);
    std::cout << t*1.0e6 << "  " << (n*n*n) / t << "  ";

    t = time_operation<true>(n, nrepeat, is_col_major);
    std::cout << t*1.0e6 << "  " << (n*n*n) / t;

    std::cout << "\n";

    n *= 2;
  }
  return 0;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/property_map.h>
#include <boost/iterator/zip_iterator.hpp>
#include <utility>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3                                     Point_3;
typedef boost::tuple<Point_3,int>                           Point_and_int;

typedef CGAL::Random_points_in_cube_3<Point_3>              Random_points_iterator;
typedef CGAL::Search_traits_3<Kernel>                       Traits_base;
typedef CGAL::Search_traits_adapter<Point_and_int,
  CGAL::Nth_of_tuple_property_map<0, Point_and_int>,
  Traits_base>                                              Traits;

typedef CGAL::Orthogonal_k_neighbor_search<Traits>          K_neighbor_search;
typedef K_neighbor_search::Tree                             Tree;
typedef K_neighbor_search::Distance                         Distance;

int main() {
  const unsigned int K = 5;
  // generator for random data points in the cube ( (-1,-1,-1), (1,1,1) )
  Random_points_iterator rpit( 1.0);
  std::vector<Point_3> points;
  std::vector<int>     indices;

  points.push_back(Point_3(*rpit++));
  points.push_back(Point_3(*rpit++));
  points.push_back(Point_3(*rpit++));
  points.push_back(Point_3(*rpit++));
  points.push_back(Point_3(*rpit++));
  points.push_back(Point_3(*rpit++));
  points.push_back(Point_3(*rpit++));

  indices.push_back(0);
  indices.push_back(1);
  indices.push_back(2);
  indices.push_back(3);
  indices.push_back(4);
  indices.push_back(5);
  indices.push_back(6);

  // Insert number_of_data_points in the tree
  Tree tree(
    boost::make_zip_iterator(boost::make_tuple( points.begin(),indices.begin() )),
    boost::make_zip_iterator(boost::make_tuple( points.end(),indices.end() ) )
  );
  Point_3 query(0.0, 0.0, 0.0);
  Distance tr_dist;

  // search K nearest neighbours
  K_neighbor_search search(tree, query, K);
  for(K_neighbor_search::iterator it = search.begin(); it != search.end(); it++){
    std::cout << " d(q, nearest neighbor)=  "
	      << tr_dist.inverse_of_transformed_distance(it->second) << " "
              << boost::get<0>(it->first)<< " " << boost::get<1>(it->first) << std::endl;
  }
  return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SECPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SECPI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing secpi capabilities

    secant of the angle in pi multiples: \f$1/\cos(\pi x)\f$.

    @par Semantic:

    For every parameter of floating type

    @code
    auto r = secpi(x);
    @endcode

    @see secd, sec, cospi, cos

  **/
  Value secpi(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/secpi.hpp>
#include <boost/simd/function/simd/secpi.hpp>

#endif


# ifndef MC_FUNCTIONS_H
#include "MC_functions.hpp"
# endif

# ifndef COMMON_HEADERS_H
#include "common_headers.hpp"
# endif

//#include <iomanip>

#include <boost/random/uniform_int_distribution.hpp>
#include <boost/format.hpp> // It seems like I might as well just include <boost/kitchen_sink.hpp>
#include <boost/math/special_functions/ellint_1.hpp>
#include <boost/math/special_functions/ellint_2.hpp>

using std::string;
namespace br = boost::random;
namespace bu = boost::uuids;

Params::Params(size_t len, size_t LL, size_t samples, uint64_t eqSteps,
        double tmp, string bName, bu::uuid id, br::mt19937_64 engine,
        br::uniform_real_distribution<double> dist)
{
    L = len;
    N = LL;
    numSamp = samples;
    numEqSteps = eqSteps;
//    beta = bta;
    temp = tmp;
    baseName = bName;
    uid = id;
    rng = engine;
    dist01 = dist;
}

double Params::GetRNG01()
{
    return dist01(rng);
}

double GetCurrentMagnetization(int64_t *sigma, size_t L)
{
    double M = 0;
    for(size_t i = 0; i < L*L; i++)
    {
            M += sigma[i];
    }

    return M /= (L*L);
}

double GetProperties(int64_t *s, int64_t *plus1, int64_t *minus1, size_t L)
{
    double E = 0;

    for(size_t j = 0; j < L; j++)
    {
        for(size_t i = 0; i < L; i++)
        {
            E -= 0.5 * s[i + L*j] * ( s[plus1[i] + L*j] + s[minus1[i] + L*j] + s[i + L*plus1[j]] + s[i + L*minus1[j]] );
        }
    }
    return E/(L*L);
}

double Update(Params *Pars, int64_t *s, br::uniform_int_distribution<size_t> *dist0L,
        int64_t *plus1, int64_t *minus1)
{
    size_t k = 0;
    size_t l = 0;
    size_t L = Pars->L;
    int64_t deltaE = 0;
    double M = 0;

    for(size_t i = 0; i < Pars->N; i++) { M += s[i]; }
    for(size_t u = 0; u < Pars->N; u++)
    {
        k = (*dist0L)(Pars->rng);
        l = (*dist0L)(Pars->rng);
        deltaE = 2.0 * s[k + L*l] * ( s[plus1[k] + L*l] + s[minus1[k] + L*l] + s[k + L*plus1[l]] + s[k + L*minus1[l]] );
        if(deltaE <= 0)
        {
            s[k + L*l] *= -1;   // Flip that spin
            M += 2*s[k + L*l];     // Add to M
        }

        else if (Pars->GetRNG01() <= (double)exp(-deltaE / Pars->temp))
        {
            s[k + L*l] *= -1;   // Flip that spin
            M += 2*s[k + L*l];     // Add to M
        }
    }

    return M/Pars->N;
}

void MonteCarlo(Params Pars)
{
    int64_t sigma[Pars.N];
    for(size_t i = 0; i < Pars.N; i++)
    {
        sigma[i] = Pars.GetRNG01() < 0.5 ? -1 : 1;
    }

    // Expectation value accumulator
    // Order is: E, M
    double expecValues[2] = { 0.0, 0.0 };
    size_t sampleCount = 0;

    // Miscellaneous variables and utilities
    br::uniform_int_distribution<size_t> dist0L(0, Pars.L-1);
    string uid_string = bu::to_string(Pars.uid);

    string Lcomp = "-L_" + std::to_string(Pars.L);
    boost::format tempFormatter("%06.3f");
    string tempComp = "-T_" + (tempFormatter % Pars.temp).str();
    string fileName = "output" + Lcomp + tempComp + "-" + uid_string + ".dat";

    FILE *fP;   // File pointer
    fP = fopen(fileName.c_str(), "w");
    fprintf(fP, "# PIMCID: %s\n", uid_string.c_str());
    fprintf(fP, "#\tE\t\tM\n");
    fclose(fP);

    // Create lookup tables for PBCs. Why compute it every time?
    int64_t plus1[Pars.L];
    int64_t minus1[Pars.L];
    for(size_t i = 0; i < Pars.L; i++)
    {
        plus1[i] = i+1;
        minus1[i] = i-1;
    }
    plus1[Pars.L-1] = 0;
    minus1[0] = Pars.L-1;

// === Begin MC loop here =====================================================
    // Equilibriate
    printf("Equlibriating...\n");
    expecValues[1] = GetCurrentMagnetization(sigma, Pars.L);

    for(uint64_t step = 0; step < Pars.numEqSteps; step++)
    {
        // Perform the updates
        expecValues[1] = Update(&Pars, sigma, &dist0L, plus1, minus1);
    }

    // Collect data
    printf("Collecting data...\n");
    while(sampleCount < Pars.numSamp)
    {
        // Perform the updates
        expecValues[1] = Update(&Pars, sigma, &dist0L, plus1, minus1);
        expecValues[0] = GetProperties(sigma, plus1, minus1, Pars.L);

        fP = fopen(fileName.c_str(), "a");
        fprintf(fP, "%f\t%f\n", expecValues[0], expecValues[1]);
        fclose(fP);

        sampleCount++;
        if(sampleCount % 1024 == 0)
        {
            printf("Writing to disk sample %10ld / %10ld\r", sampleCount, Pars.numSamp);
        }
    }
    printf("Data collection is done.\n");
    printf("The UUID is: %s\n", uid_string.c_str());

    double q = 2 * sinh(2/Pars.temp) / (cosh(2/Pars.temp) * cosh(2/Pars.temp));
    double exactEatT = -1/tanh(2/Pars.temp) * ( 1 + 2/M_PI * (2 * tanh(2/Pars.temp) * tanh(2/Pars.temp)- 1) * boost::math::ellint_1(q) );
    printf("Exact E at %f K: %f\n\n", Pars.temp, exactEatT);
}


/*!
 * @file
 * This file defines unit tests for `boost::mpl::reachable_set`.
 */

#include <boost/mpl/reachable_set.hpp>
#include <boost/mpl/set.hpp>
#include <boost/mpl/set_equal.hpp>
#include <boost/mpl/vector.hpp>

using namespace boost::mpl;

template <typename ...AdjacentVertices>
struct vertex {
    using open_neighborhood = vector<AdjacentVertices...>;
};

struct dummy_graph;

namespace test_cyclic_graph {
    // cycle: u0 -> u2 -> u1 -> u0
    struct u0 : vertex<struct u2> { };
    struct u1 : vertex<u0> { };
    struct u2 : vertex<u1> { };

    static_assert(set_equal<
        reachable_set<dummy_graph, u0>::type, set<u0, u2, u1>
    >::value, "");

    static_assert(set_equal<
        reachable_set<dummy_graph, u1>::type, set<u1, u0, u2>
    >::value, "");

    static_assert(set_equal<
        reachable_set<dummy_graph, u2>::type, set<u2, u1, u0>
    >::value, "");
}

namespace test_acyclic_graph {
    struct v0 : vertex<> { };
    struct v1 : vertex<v0> { };
    struct v2 : vertex<v0, v1> { };
    struct v3 : vertex<v1> { };

    static_assert(set_equal<
        reachable_set<dummy_graph, v0>::type, set<v0>
    >::value, "");

    static_assert(set_equal<
        reachable_set<dummy_graph, v1>::type, set<v1, v0>
    >::value, "");

    static_assert(set_equal<
        reachable_set<dummy_graph, v2>::type, set<v2, v0, v1>
    >::value, "");

    static_assert(set_equal<
        reachable_set<dummy_graph, v3>::type, set<v3, v1, v0>
    >::value, "");
}

int main() { }


// ParticleUtilities.cpp
// Copyright 2019 Mikko Lauri
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "ParticleUtilities.h"
#include <boost/random/uniform_real_distribution.hpp>
#include <eigen3/Eigen/Dense>
#include "common.hpp"

namespace pgi {
double normalize(std::vector<double>& weights) {
  if (weights.empty()) {
    return 0.0;
  } else {
    Eigen::Map<Eigen::VectorXd> w(weights.data(), weights.size());
    const double wsum = w.lpNorm<1>();
    if (!is_almost_zero(wsum)) w /= wsum;
    return wsum;
  }
}

// Implements systematic resampling
void resample(const std::vector<double>& weights,
              std::vector<std::size_t>& resample_indices, PRNG& rng) {
  resample_indices.resize(weights.size());
  double cdf = 0.0;
  std::size_t curr_index = 0;

  const double incr = 1.0 / static_cast<double>(weights.size());
  boost::random::uniform_real_distribution<double> U(0, incr);
  double u = rng(U);
  for (auto& resample_index : resample_indices) {
    while (u > cdf) {
      cdf += weights[curr_index];
      ++curr_index;
    }
    resample_index = curr_index - 1;
    u += incr;
  }
}

double effective_size(const std::vector<double>& weights) {
  Eigen::Map<const Eigen::VectorXd> w(weights.data(), weights.size());
  return 1.0 / w.squaredNorm();
}

}  // namespace pgi


/*
Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.

NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
*/

// Generate input data for an example C++ code that demonstrates usage of PnP solvers.
// See pnp_test for more thorough tests of the PnP API.

#include "engine/core/logger.hpp"
#include "packages/pnp/gems/pnp.hpp"
#include "packages/pnp/gems/tests/simu.hpp"

#include <cmath>
#include <iomanip>
#include <iostream>
#include <random>

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

// Print an Eigen matrix such that it can be copy-pasted into a C++ source file.
void PrintMatrixCode(const isaac::MatrixXd& mat) {
  for (int i = 0; i < mat.rows(); i++) {
    for (int j = 0; j < mat.cols(); j++) {
      std::cout << mat(i, j);
      if (j + 1 < mat.cols()) std::cout << ", ";
    }
    if (i + 1 == mat.rows())
      std::cout << ";\n";
    else
      std::cout << ",\n";
  }
}

// Print an isaac pose for example C++ code.
std::ostream& operator<<(std::ostream& os, const isaac::Pose3d& pose) {
  std::cout << "translation = (" << pose.translation.transpose() << "), ";
  std::cout << "angle = " << pose.rotation.angle() << ", ";
  std::cout << "axis = (" << pose.rotation.axis().transpose() << ")";
  return os;
}

// Test equivalence of two poses (a and b) are similar within tolerance parameters.
void TestPoseEquivalence(const isaac::Pose3d& a, const isaac::Pose3d& b, double max_distance = 1e-9,
                         double max_angle_degrees = 1e-9) {
  // Calculate difference in orientation.
  isaac::SO3d delta_rot = a.rotation.inverse() * b.rotation;
  double angle = std::fmod(isaac::RadToDeg(std::abs(delta_rot.angle())), 360.0);
  angle = std::min(angle, 360.0 - angle);

  // Calculate difference in position.
  // Convert world origin in camera frame to camera position in world frame. This can make
  // a significant difference in case of a moving camera far away from world origin.
  isaac::Vector3d position1 = -a.rotation.matrix().transpose() * a.translation;
  isaac::Vector3d position2 = -b.rotation.matrix().transpose() * b.translation;

  ASSERT_LT((position1 - position2).norm(), max_distance);
  ASSERT_LT(angle, max_angle_degrees);
}

// Generate an example for PnP and test EPnP on the example data.
TEST(PnpTest, TestEpnpExample) {
  // Generate synthetic camera.
  const int width = 1280;
  const int height = 720;
  const double focal = 700.0;
  isaac::pnp::Camera camera = isaac::pnp::GenerateRandomCamera(width, height, focal);

  // Construct isaac::Pose3d object from the generated camera pose.
  // This is the known ground-truth pose to compare to the output of pose estimation.
  isaac::Vector3d gt_angle_axis = isaac::pnp::AngleAxisFromMatrix(camera.rotation_matrix);
  isaac::Pose3d gt_pose{isaac::SO3d::FromAngleAxis(gt_angle_axis.norm(), gt_angle_axis),
                        -camera.rotation_matrix * camera.position};
  std::cout << gt_pose << std::endl;

  // Print camera intrinsics.
  double focal_u = camera.calib_matrix(0, 0);
  double focal_v = camera.calib_matrix(1, 1);
  double principal_u = camera.calib_matrix(0, 2);
  double principal_v = camera.calib_matrix(1, 2);
  std::cout << "--------------- synthetic C++ example below this line\n";
  std::cout << "focal_u = " << focal_u << ";\n";
  std::cout << "focal_v = " << focal_v << ";\n";
  std::cout << "principal_u = " << principal_u << ";\n";
  std::cout << "principal_v = " << principal_v << ";\n";

  // Generate inlier 2D-3D matches without noise.
  // 3D points are within camera FoV between near and far planes.
  const unsigned num_points = 6;
  const double near = 2.0;
  const double far = 100.0;
  isaac::Matrix3Xd points3;
  isaac::Matrix2Xd points2;
  isaac::pnp::GenerateFovPoints(num_points, camera, near, far, &points3, &points2);

  // Print generated 2D and 3D points.
  std::cout << "isaac::Matrix3Xd points3(3," << points3.cols() << ");\n";
  std::cout << "isaac::Matrix2Xd points2(2," << points2.cols() << ");\n";
  std::cout << "points3 << \n";
  PrintMatrixCode(points3);
  std::cout << "points2 << \n";
  PrintMatrixCode(points2);
  std::cout << "--------------- end of example\n";

  // Test EPnP camera pose estimation.
  isaac::Pose3d pose;
  ASSERT_EQ(isaac::pnp::ComputeCameraPoseEpnp(points3, points2, focal_u, focal_v, principal_u,
                                              principal_v, &pose),
            isaac::pnp::Status::kSuccess);

  // Test if computed pose matches the ground-truth pose.
  TestPoseEquivalence(pose, gt_pose);

  // Test reprojection error calculation for the example code.
  isaac::Matrix3d calib_matrix;
  calib_matrix << focal_u, 0, principal_u, 0, focal_v, principal_v, 0, 0, 1;
  for (int i = 0; i < points3.cols(); i++) {
    isaac::Vector3d proj =
        calib_matrix * (pose.rotation.matrix() * points3.col(i) + pose.translation);
    isaac::Vector2d point2(proj(0) / proj(2), proj(1) / proj(2));

    // Reprojection error in pixels should be near zero.
    ASSERT_LT((points2.col(i) - point2).norm(), 1e-6);
  }
}

// Generate synthetic input for the RANSAC-EPnP pose estimation example code.
// (1) Generate a camera with a random pose.
// (2) Generate a fixed number of 2D/3D point match inliers and add a fixed percentage of outliers.
// (3) Run RANSAC-EPnP and test the result.
TEST(PnpTest, TestEpnpRansacExample) {
  // Generate synthetic data.
  const int width = 1280;
  const int height = 720;
  const double focal = 700.0;
  isaac::pnp::Camera camera = isaac::pnp::GenerateRandomCamera(width, height, focal);

  // Construct isaac::Pose3d object from the generated camera pose.
  // This is the known ground-truth pose to compare to the output of pose estimation.
  isaac::Vector3d gt_angle_axis = isaac::pnp::AngleAxisFromMatrix(camera.rotation_matrix);
  isaac::Pose3d gt_pose{isaac::SO3d::FromAngleAxis(gt_angle_axis.norm(), gt_angle_axis),
                        -camera.rotation_matrix * camera.position};
  std::cout << gt_pose << std::endl;

  // Print camera intrinsics.
  double focal_u = camera.calib_matrix(0, 0);
  double focal_v = camera.calib_matrix(1, 1);
  double principal_u = camera.calib_matrix(0, 2);
  double principal_v = camera.calib_matrix(1, 2);
  std::cout << "--------------- synthetic C++ example below this line\n";
  std::cout << "focal_u = " << focal_u << ";\n";
  std::cout << "focal_v = " << focal_v << ";\n";
  std::cout << "principal_u = " << principal_u << ";\n";
  std::cout << "principal_v = " << principal_v << ";\n";

  // Generate inlier 2D-3D matches without noise.
  // 3D points are within camera FoV between near and far planes.
  const unsigned num_inliers = 7;
  const double near = 2.0;
  const double far = 6.0;
  isaac::Matrix3Xd points3;
  isaac::Matrix2Xd points2;
  isaac::pnp::GenerateFovPoints(num_inliers, camera, near, far, &points3, &points2);

  // Generate outlier 2D-3D matches
  const unsigned num_outliers = 3;
  isaac::pnp::InsertOutliers(num_outliers, camera, 30.0, &points3, &points2);

  // Print generated 2D and 3D points.
  std::cout << "isaac::Matrix3Xd points3(3," << points3.cols() << ");\n";
  std::cout << "isaac::Matrix2Xd points2(2," << points2.cols() << ");\n";
  std::cout << "points3 << \n";
  PrintMatrixCode(points3);
  std::cout << "points2 << \n";
  PrintMatrixCode(points2);
  std::cout << "--------------- end of example\n";

  // Calculate the number of ransac experiments to run.
  unsigned ransac_rounds = isaac::pnp::EvaluateRansacFormula(0.99, 0.3, 6);

  // The standard RANSAC formula is very optimistic for small input sizes. An ad-hoc fix.
  if (points3.cols() < 20) ransac_rounds = std::max(ransac_rounds, 100u);

  // Compute top pose hypotheses.
  std::random_device rnd;
  const double ransac_threshold = 1.0;
  const unsigned max_top_poses = 3;
  const unsigned rand_seed = 73;
  std::vector<isaac::pnp::PoseHypothesis> top_hypotheses = isaac::pnp::ComputeCameraPoseEpnpRansac(
      points3, points2, focal_u, focal_v, principal_u, principal_v, ransac_rounds, ransac_threshold,
      max_top_poses, rand_seed);

  std::cout << top_hypotheses.size() << " pose hypotheses:\n";

  if (top_hypotheses.size()) {
    // There is a single valid hypothesis.
    ASSERT_EQ(top_hypotheses.size(), 1);

    // Print the hypothesis.
    const auto& hyp = top_hypotheses.front();
    std::cout << hyp.pose << std::endl;
    std::cout << "score=" << hyp.score;
    std::cout << ", " << hyp.inliers.size() << " inliers";
    if (hyp.inliers.size()) {
      std::cout << ":";
      for (auto index : hyp.inliers) std::cout << " " << index;
      std::cout << std::endl;
    }

    // Calculate and print reprojection errors.
    std::cout << "Reprojection error (pixels):" << std::fixed << std::setprecision(1);
    isaac::Matrix3d calib_matrix;
    calib_matrix << focal_u, 0, principal_u, 0, focal_v, principal_v, 0, 0, 1;
    for (int i = 0; i < points3.cols(); i++) {
      isaac::Vector3d proj =
          calib_matrix * (hyp.pose.rotation.matrix() * points3.col(i) + hyp.pose.translation);
      isaac::Vector2d point2(proj(0) / proj(2), proj(1) / proj(2));
      double repr_error = (points2.col(i) - point2).norm();
      std::cout << "  " << repr_error;
    }
    std::cout << std::endl;
  }
}

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


#include "problem.hpp"
#include "statistics.hpp"

#include <string>
#include <armadillo>
#include <yaml-cpp/yaml.h>

Problem::Problem(const std::string &file_config)
{
  YAML::Node config = YAML::LoadFile(file_config);
  nchannel_ = config["nchannel"].as<int>();
  nx_ = config["nx"].as<int>();
  std::string file_data = config["file_data"].as<std::string>();

  // random x0
  arma::arma_rng::set_seed_random();
  x0_ = 2. * (arma::randu<arma::vec>(nx_) - 0.5);

  // (t1, t2, t3) -> idx
  int idx = 0;
  for (int k = 0; k < nx_; ++k) {
    for (int j = 0; j <= k; ++j) {
      for (int i = 0; i <= j; ++i) {
        if (i == 0 && k > 0 && k == j) continue;
        idx++;
      }
    }
  }
  num_c4_ = idx;

  c4y_.set_size(num_c4_, nchannel_);

  // load traces
  // arma::mat data(nx_, nchannel_, arma::fill::zeros);
  arma::mat data;
  data.load(file_data);
  std::cout << data.n_rows << " " << data.n_cols << std::endl;

  // fourth-order cumulants of records
  for (int i = 0; i < nchannel_; ++i) {
    arma::vec trace = data.col(i);
    trace -= arma::mean(trace);
    arma::vec cum4(num_c4_, arma::fill::zeros);
    cumulants_4th(trace, nx_, num_c4_, cum4);
    c4y_.col(i) = cum4;
  }
}

arma::vec Problem::x0() const
{
  return x0_;
}

double Problem::fitness(const arma::vec &x)
{
  arma::vec m4h(num_c4_, arma::fill::zeros);
  moment_function_4th(x, nx_, m4h);

  double misfit = 0;
  for (int i = 0; i < nchannel_; ++i) {
    misfit += arma::sum(arma::pow(c4y_.col(i) - m4h, 2));
  }
  return misfit;
}

arma::vec Problem::gradient(const arma::vec &x)
{
  arma::vec m4h(num_c4_, arma::fill::zeros);
  moment_function_4th(x, nx_, m4h);

  arma::mat jac(nx_, num_c4_, arma::fill::zeros);
  int idx = 0;
  for (int k = 0; k < nx_; ++k) {
    for (int j = 0; j <= k; ++j) {
      for (int i = 0; i <= j; ++i) {
        if (i == 0 && k > 0 && k == j) continue;

        arma::vec sums(nx_, arma::fill::zeros);
        for (int l = 0; l < nx_ - k; ++l)
          sums(l) += x(l + i) * x(l + j) * x(l + k);
        for (int l = i; l < nx_ - k + i; ++l)
          sums(l) += x(l - i) * x(l - i + j) * x(l - i + k);
        for (int l = j; l < nx_ - k + j; ++l)
          sums(l) += x(l - j) * x(l - j + i) * x(l - j + k);
        for (int l = k; l < nx_; ++l)
          sums(l) += x(l - k) * x(l - k + i) * x(l - k + j);

        jac.col(idx++) = sums;
      }
    }
  }

  arma::vec grad(nx_, arma::fill::zeros);
  for (int i = 0; i < nchannel_; ++i) {
    arma::vec error = m4h - c4y_.col(i);
    grad += 2. * jac * error;
  }

  return grad;

}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2016 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

// =============================== Double Pendulum Example ===============================
//
// Author: Vincent Acary
//
// Keywords: LagrangianDS, LagrangianLinear relation, MoreauJeanOSI TimeStepping, LCP.
//
// =============================================================================================

#include "SiconosKernel.hpp"
#include <stdlib.h>
using namespace std;

#include <boost/progress.hpp>

double gravity = 10.0;
double m1 = 1.0;
double m2 = 1.0 ;
double l1 = 1.0 ;
double l2 = 1.0 ;

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

    // ================= Creation of the model =======================

    // User-defined main parameters
    unsigned int nDof = 2;           // degrees of freedom for robot arm
    double t0 = 0;                   // initial computation time
    double T = 5.0;                   // final computation time
    double h = 0.0005;                // time step
    double criterion = 0.05;
    unsigned int maxIter = 20000;
    double e = 1.0;                  // nslaw
    double e1 = 0.0;

    // -> mind to set the initial conditions below.

    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    // --- DS: Double Pendulum ---

    // Initial position (angles in radian)
    SP::SiconosVector q0(new SiconosVector(nDof));
    SP::SiconosVector v0(new SiconosVector(nDof));

    q0->zero();
    v0->zero();

    // (*q0)(0) = 1.5;
    // (*q0)(1) = 1.5;

    // for sympy plugins uncomment below (relative parametrization)
    // Note, we have the relation :
    // absolute[(*q0)(0)] + relative[(*q0)(1)] = absolute[(*q0)(1)]
    // (*q0)(0) = 0.1;
    // (*q0)(1) = 0.1;

    (*q0)(0) = 0.1;
    (*q0)(1) = 0.2;

    /*REGULAR PLUGINS - uncomment to use*/
    SP::LagrangianDS doublependulum(new LagrangianDS(q0, v0, "DoublePendulumPlugin:mass"));
    doublependulum->setComputeFGyrFunction("DoublePendulumPlugin", "FGyr");
    doublependulum->setComputeJacobianFGyrqDotFunction("DoublePendulumPlugin", "jacobianVFGyr");
    doublependulum->setComputeJacobianFGyrqFunction("DoublePendulumPlugin", "jacobianFGyrq");
    doublependulum->setComputeFIntFunction("DoublePendulumPlugin", "FInt");
    doublependulum->setComputeJacobianFIntqDotFunction("DoublePendulumPlugin", "jacobianVFInt");
    doublependulum->setComputeJacobianFIntqFunction("DoublePendulumPlugin", "jacobianFIntq");

    /*SYMPY PLUGINS - uncomment to use*/
    // SP::LagrangianDS doublependulum(new LagrangianDS(q0, v0, "DoublePendulumSymPyPlugin:mass"));
    // doublependulum->setComputeFGyrFunction("DoublePendulumSymPyPlugin", "FGyr");
    // doublependulum->setComputeJacobianFGyrqDotFunction("DoublePendulumSymPyPlugin", "jacobianVFGyr");
    // doublependulum->setComputeJacobianFGyrqFunction("DoublePendulumSymPyPlugin", "jacobianFGyrq");

    // -------------------
    // --- Interactions---
    // -------------------

    // -- relations --

    string G = "DoublePendulumPlugin:G0";
    SP::NonSmoothLaw nslaw(new NewtonImpactNSL(e));
    SP::Relation relation(new LagrangianScleronomousR("DoublePendulumPlugin:h0", G));
    SP::Interaction inter(new Interaction(1, nslaw, relation));

    string G1 = "DoublePendulumPlugin:G1";
    SP::NonSmoothLaw nslaw1(new NewtonImpactNSL(e1));
    SP::Relation relation1(new LagrangianScleronomousR("DoublePendulumPlugin:h1", G1));
    SP::Interaction inter1(new Interaction(1, nslaw1, relation1));

    // -------------
    // --- Model ---
    // -------------

    SP::Model Pendulum(new Model(t0, T));
    Pendulum->nonSmoothDynamicalSystem()->insertDynamicalSystem(doublependulum);
    Pendulum->nonSmoothDynamicalSystem()->link(inter,doublependulum);
    Pendulum->nonSmoothDynamicalSystem()->link(inter1,doublependulum);

    // ----------------
    // --- Simulation ---
    // ----------------

    // -- Time discretisation --
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));

    SP::TimeStepping s(new TimeStepping(t));
    //        s->setUseRelativeConvergenceCriteron(true);

    // -- OneStepIntegrators --

    //double theta=0.500001;
    double theta = 0.500001;

    SP::MoreauJeanOSI OSI(new MoreauJeanOSI(theta));
    s->insertIntegrator(OSI);

    // -- OneStepNsProblem --
    SP::OneStepNSProblem osnspb(new LCP());

    s->insertNonSmoothProblem(osnspb);

    cout << "=== End of model loading === " << endl;

    // =========================== End of model definition ===========================  dataPlot(k,7) = (*inter->y(0))(0);

    // ================================= Computation =================================

    // --- Simulation initialization ---
    Pendulum->initialize(s);
    cout << "End of simulation initialisation" << endl;

    int k = 0;
    int N = ceil((T - t0) / h);
    cout << "Number of time step   " << N << endl;
    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 12;
    SimpleMatrix dataPlot(N + 1, outputSize);
    // For the initial time step:
    // time
    SP::SiconosVector q = doublependulum->q();
    SP::SiconosVector v = doublependulum->velocity();

    dataPlot(k, 0) =  t0;
    dataPlot(k, 1) = (*q)(0);
    dataPlot(k, 2) = (*v)(0);
    dataPlot(k, 3) = (*q)(1);
    dataPlot(k, 4) = (*v)(1);
    dataPlot(k, 5) =  l1 * sin((*q)(0));
    dataPlot(k, 6) = -l1 * cos((*q)(0));
    dataPlot(k, 7) =  l1 * sin((*q)(0)) + l2 * sin((*q)(1));
    dataPlot(k, 8) = -l1 * cos((*q)(0)) - l2 * cos((*q)(1));
    dataPlot(k, 9) =  l1 * cos((*q)(0)) * ((*v)(0));
    dataPlot(k, 10) = l1 * cos((*q)(0)) * ((*v)(0)) + l2 * cos((*q)(1)) * ((*v)(1));

    boost::timer time;
    time.restart();

    // --- Time loop ---
    cout << "Start computation ... " << endl;

    boost::progress_display show_progress(N);

    while (s->hasNextEvent())
    {
      k++;
      ++show_progress;
      //  if (!(div(k,1000).rem))  cout <<"Step number "<< k << "\n";

      // Solve problem
      s->newtonSolve(criterion, maxIter);
      // Data Output
      dataPlot(k, 0) =  s->nextTime();
      dataPlot(k, 1) = (*q)(0);
      dataPlot(k, 2) = (*v)(0);
      dataPlot(k, 3) = (*q)(1);
      // sympy plugin with relative parametrization:
      // dataPlot(k, 3) = (*q)(0) + (*q)(1);
      dataPlot(k, 4) = (*v)(1);
      // sympy plugin with relative parametrization:
      // dataPlot(k, 4) = (*v)(0) + (*v)(1);
      dataPlot(k, 5) =  l1 * sin((*q)(0));
      dataPlot(k, 6) = -l1 * cos((*q)(0));
      dataPlot(k, 7) =  l1 * sin((*q)(0)) + l2 * sin((*q)(1));
      dataPlot(k, 8) = -l1 * cos((*q)(0)) - l2 * cos((*q)(1));
      dataPlot(k, 9) =  l1 * cos((*q)(0)) * ((*v)(0));
      dataPlot(k, 10) = l1 * cos((*q)(0)) * ((*v)(0)) + l2 * cos((*q)(1)) * ((*v)(1));
      s->nextStep();
    }

    cout << "End of computation - Number of iterations done: " << k << endl;
    cout << "Computation Time " << time.elapsed()  << endl;

    // --- Output files ---
    ioMatrix::write("DoublePendulumResult.dat", "ascii", dataPlot, "noDim");
  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught in \'sample/MultiBeadsColumn\'" << endl;
  }
}


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson, John B. Mains
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__FEEDBACK__QP_HPP_
#define SMOOTH__FEEDBACK__QP_HPP_

/**
 * @file
 * @brief Quadratic Programming.
 */

#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>

#include <chrono>
#include <iomanip>
#include <iostream>
#include <limits>
#include <optional>

namespace smooth::feedback {

/**
 * @brief Quadratic program definition.
 *
 * @tparam M number of constraints
 * @tparam N number of variables
 *
 * The quadratic program is on the form
 * \f[
 * \begin{cases}
 *  \min_{x} & \frac{1}{2} x^T P x + q^T x, \\
 *  \text{s.t.} & l \leq A x \leq u,
 * \end{cases}
 * \f]
 * where \f$ P \in \mathbb{R}^{n \times n}, q \in \mathbb{R}^n, l, u \in \mathbb{R}^m, A \in
 * \mathbb{R}^{m \times n} \f$.
 */
template<Eigen::Index M, Eigen::Index N, typename Scalar = double>
struct QuadraticProgram
{
  /// Positive semi-definite square cost (only upper trianglular part is used)
  Eigen::Matrix<Scalar, N, N> P;
  /// Linear cost
  Eigen::Matrix<Scalar, N, 1> q;

  /// Inequality matrix
  Eigen::Matrix<Scalar, M, N> A;
  /// Inequality lower bound
  Eigen::Matrix<Scalar, M, 1> l;
  /// Inequality upper bound
  Eigen::Matrix<Scalar, M, 1> u;
};

/**
 * @brief Sparse quadratic program definition.
 *
 * The quadratic program is on the form
 * \f[
 * \begin{cases}
 *  \min_{x} & \frac{1}{2} x^T P x + q^T x, \\
 *  \text{s.t.} & l \leq A x \leq u,
 * \end{cases}
 * \f]
 * where \f$ P \in \mathbb{R}^{n \times n}, q \in \mathbb{R}^n, l, u \in \mathbb{R}^m, A \in
 * \mathbb{R}^{m \times n} \f$.
 */
template<typename Scalar = double>
struct QuadraticProgramSparse
{
  /// Positive semi-definite square cost (only upper trianglular part is used)
  Eigen::SparseMatrix<Scalar> P;
  /// Linear cost
  Eigen::Matrix<Scalar, -1, 1> q;

  /**
   * @brief Inequality matrix
   *
   * @note The constraint matrix is stored in row-major format,
   * i.e. coefficients for each constraint are contiguous in memory
   */
  Eigen::SparseMatrix<Scalar, Eigen::RowMajor> A;
  /// Inequality lower bound
  Eigen::Matrix<Scalar, -1, 1> l;
  /// Inequality upper bound
  Eigen::Matrix<Scalar, -1, 1> u;
};

/// Solver exit codes
enum class QPSolutionStatus : int {
  Optimal,           /// Solution satisifes optimality condition. Solution is polished if
                     /// `QPSolverParams::polish = true`.
  PolishFailed,      /// Solution satisfies optimality condition but is not polished
  PrimalInfeasible,  /// A certificate of primal infeasibility was found, no solution returned
  DualInfeasible,    /// A certificate of dual infeasibility was found, no solution returned
  MaxIterations,     /// Max number of iterations was reached, returned solution is not optimal
  MaxTime,           /// Max time was reached, returned solution is not optimal
  Unknown            /// Solution is useless because of other reasons, no solution returned
};

/// Solver solution
template<Eigen::Index M, Eigen::Index N, typename Scalar = double>
struct QPSolution
{
  /// Exit code
  QPSolutionStatus code = QPSolutionStatus::Unknown;
  /// Number of iterations
  uint64_t iter;
  /// Primal vector
  Eigen::Matrix<Scalar, N, 1> primal;
  /// Dual vector
  Eigen::Matrix<Scalar, M, 1> dual;
  /// Solution objective value
  double objective;
};

/**
 * @brief Options for solve_qp
 */
struct QPSolverParams
{
  /// print solver info to stdout
  bool verbose = false;

  /// relaxation parameter
  float alpha = 1.6;
  /// first dul step size
  float rho = 0.1;
  /// second dual step length
  float sigma = 1e-6;

  /// scale problem
  bool scaling = true;

  /// absolute threshold for convergence
  float eps_abs = 1e-3;
  /// relative threshold for convergence
  float eps_rel = 1e-3;
  /// threshold for primal infeasibility
  float eps_primal_inf = 1e-4;
  /// threshold for dual infeasibility
  float eps_dual_inf = 1e-4;

  /// max number of iterations
  std::optional<uint32_t> max_iter = {};

  /// max solution time
  std::optional<std::chrono::nanoseconds> max_time = {};

  /// iterations between checking stopping criterion
  uint32_t stop_check_iter = 25;

  /// run solution polishing (uses dynamic memory)
  bool polish = true;
  /// number of iterations to refine polish
  uint32_t polish_iter = 5;
  /// regularization parameter for polishing
  float delta = 1e-6;
};

namespace detail {

// Traits to figure solution type from problem type
// \cond
template<typename T>
struct qp_solution;

template<Eigen::Index M, Eigen::Index N, typename Scalar>
struct qp_solution<QuadraticProgram<M, N, Scalar>>
{
  using type = QPSolution<M, N, Scalar>;
};

template<typename Scalar>
struct qp_solution<QuadraticProgramSparse<Scalar>>
{
  using type = QPSolution<-1, -1, Scalar>;
};

template<typename T>
using qp_solution_t = typename qp_solution<T>::type;
// \endcond

/**
 * @brief Re-scale a QuadraticProgram
 *
 * @param pbm problem \f$ (P, q, A, l, u) \f$ to rescale.
 *
 * @returns tuple `(spbm, s, c)` where `spbm` is a scaled problem \f$ (\bar P, \bar q, \bar A, \bar
 * l, \bar u)\f$.
 *
 * The scaled problem is defined as
 *
 * * \f$ \bar P = c S_x P S_x \f$,
 * * \f$ \bar q = c q S_x \f$,
 * * \f$ \bar A = S_e A S_x \f$,
 * * \f$ \bar l = S_e l \f$,
 * * \f$ \bar u = S_e u \f$,
 *
 * where \f$ S_x = diag(s_{0:n}), S_e = diag(s_{n:n+m}) \f$.
 *
 * The relation between scaled variables and original variables are
 *
 * * Primal: \f$ \bar x = S_x^{-1} x \f$,
 * * Dual: \f$ \bar y = c E_x^{-1} x \f$.
 *
 * The objective of the rescaling is the make the columns of
 * \f[
 *   \begin{bmatrix} \bar P & \bar A^T \\ \bar A & 0 \end{bmatrix}
 * \f]
 * have similar \f$ l_\infty \f$ norm, and similarly for
 * the columns of
 * \f[
 *  \begin{bmatrix} \bar P & \bar q \end{bmatrix}.
 * \f]
 */
template<typename Pbm>
auto scale_qp(const Pbm & pbm)
{
  using AmatT                  = decltype(Pbm::A);
  using Scalar                 = typename AmatT::Scalar;
  static constexpr bool sparse = std::is_base_of_v<Eigen::SparseMatrixBase<AmatT>, AmatT>;

  Pbm ret = pbm;

  static constexpr Eigen::Index M = AmatT::RowsAtCompileTime;
  static constexpr Eigen::Index N = AmatT::ColsAtCompileTime;
  static constexpr Eigen::Index K = (N == -1 || M == -1) ? Eigen::Index(-1) : N + M;

  const Eigen::Index n = ret.A.cols(), m = ret.A.rows(), k = n + m;
  const auto norm = [](auto && t) -> Scalar { return t.template lpNorm<Eigen::Infinity>(); };

  Eigen::Matrix<Scalar, K, 1> scale = Eigen::Matrix<Scalar, K, 1>::Ones(k);  // scaling
  Eigen::Matrix<Scalar, K, 1> d_scale(k);                                    // incremental scaling

  // find "norm" of cost function
  if constexpr (sparse) {
    for (auto i = 0u; i != n; ++i) {
      d_scale(i) = 0;
      // traverse each col of P
      for (Eigen::SparseMatrix<double>::InnerIterator it(ret.P, i); it; ++it) {
        d_scale(i) = std::max(d_scale(i), std::abs(it.value()));
      }
    }
  } else {
    d_scale.template head<N>(n) = ret.P.colwise().template lpNorm<Eigen::Infinity>();
  }

  // if there are "zero cols"
  for (auto i = 0u; i != n; ++i) {
    if (d_scale(i) == 0) { d_scale(i) = 1; }
  }

  // scale cost function
  Scalar c = Scalar(1) / std::max({1e-6, d_scale.template head<N>(n).mean(), norm(ret.q)});
  ret.P *= c;
  ret.q *= c;

  int iter = 0;

  // calculate norm for every column of [P A' ; A 0]
  do {
    if constexpr (sparse) {
      // P is stored col wise
      for (auto i = 0u; i != n; ++i) {
        d_scale(i) = 0;
        for (Eigen::SparseMatrix<double>::InnerIterator it(ret.P, i); it; ++it) {
          // upper left block of H
          d_scale(i) = std::max(d_scale(i), std::abs(it.value()));
        }
      }
      // A is stored row wise
      for (auto i = 0u; i != m; ++i) {
        d_scale(n + i) = 0;
        for (Eigen::SparseMatrix<double, Eigen::RowMajor>::InnerIterator it(ret.A, i); it; ++it) {
          // bottom left block of H
          d_scale(it.index()) = std::max(d_scale(it.index()), std::abs(it.value()));
          // upper right block of H
          d_scale(n + i) = std::max(d_scale(n + i), std::abs(it.value()));
        }
      }
    } else {
      d_scale.template head<N>(n) = ret.P.colwise().template lpNorm<Eigen::Infinity>().cwiseMax(
        ret.A.colwise().template lpNorm<Eigen::Infinity>());
      d_scale.template segment<M>(n, m) = ret.A.rowwise().template lpNorm<Eigen::Infinity>();
    }

    // if there are "zero cols" we don't scale
    for (auto i = 0u; i != k; ++i) {
      if (d_scale(i) == 0) { d_scale(i) = 1; }
    }

    d_scale = d_scale.cwiseMax(1e-8).cwiseInverse().cwiseSqrt();

    // perform scaling
    if constexpr (sparse) {
      ret.P =
        d_scale.template head<N>(n).asDiagonal() * ret.P * d_scale.template head<N>(n).asDiagonal();
      ret.A = d_scale.template segment<M>(n, m).asDiagonal() * ret.A
            * d_scale.template head<N>(n).asDiagonal();
    } else {
      ret.P.applyOnTheLeft(d_scale.template head<N>(n).asDiagonal());
      ret.P.applyOnTheRight(d_scale.template head<N>(n).asDiagonal());
      ret.A.applyOnTheLeft(d_scale.template segment<M>(n, m).asDiagonal());
      ret.A.applyOnTheRight(d_scale.template head<N>(n).asDiagonal());
    }
    ret.q.applyOnTheLeft(d_scale.template head<N>(n).asDiagonal());
    ret.l.applyOnTheLeft(d_scale.template segment<M>(n, m).asDiagonal());
    ret.u.applyOnTheLeft(d_scale.template segment<M>(n, m).asDiagonal());

    scale.applyOnTheLeft(d_scale.asDiagonal());
  } while (iter++ < 10 && (d_scale.array() - 1).abs().maxCoeff() > 0.1);

  return std::make_tuple(std::move(ret), std::move(scale), c);
}

/**
 * @brief Polish solution of quadratic program
 *
 * @tparam Pbm problem type
 *
 * @param[in] pbm problem formulation
 * @param[in, out] sol solution to polish
 * @param[in] prm solver options
 *
 * @warning This function allocates heap memory even for static-sized problems.
 */
template<typename Pbm>
bool polish_qp(const Pbm & pbm, qp_solution_t<Pbm> & sol, const QPSolverParams & prm)
{
  using AmatT                  = decltype(Pbm::A);
  using Scalar                 = typename AmatT::Scalar;
  using VecX                   = Eigen::Matrix<Scalar, -1, 1>;
  static constexpr bool sparse = std::is_base_of_v<Eigen::SparseMatrixBase<AmatT>, AmatT>;

  static constexpr Scalar inf = std::numeric_limits<Scalar>::infinity();
  static constexpr Scalar eps = std::numeric_limits<Scalar>::epsilon();

  static constexpr Eigen::Index N = AmatT::ColsAtCompileTime;
  const Eigen::Index n = pbm.A.cols(), m = pbm.A.rows();

  // FIND ACTIVE CONSTRAINT SETS

  Eigen::Index nl = 0, nu = 0;
  for (Eigen::Index idx = 0; idx < m; ++idx) {
    if (sol.dual[idx] < -100 * eps && pbm.l[idx] != -inf) { nl++; }
    if (sol.dual[idx] > 100 * eps && pbm.u[idx] != inf) { nu++; }
  }

  Eigen::Matrix<Eigen::Index, -1, 1> LU_idx(nl + nu);
  for (Eigen::Index idx = 0, lcntr = 0, ucntr = 0; idx < m; ++idx) {
    if (sol.dual[idx] < -100 * eps && pbm.l[idx] != -inf) { LU_idx(lcntr++) = idx; }
    if (sol.dual[idx] > 100 * eps && pbm.u[idx] != inf) { LU_idx(nl + ucntr++) = idx; }
  }

  // FORM REDUCED SYSTEMS (27) AND (30)

  // square symmetric system matrix
  using HT = std::conditional_t<sparse, Eigen::SparseMatrix<Scalar>, Eigen::Matrix<Scalar, -1, -1>>;
  HT H(n + nl + nu, n + nl + nu), Hp(n + nl + nu, n + nl + nu);

  // fill up H
  if constexpr (sparse) {
    // preallocate nonzeros
    Eigen::Matrix<int, -1, 1> nnz(n + nl + nu);
    for (auto i = 0u; i != n; ++i) {
      nnz(i) = pbm.P.outerIndexPtr()[i + 1] - pbm.P.outerIndexPtr()[i];
    }
    for (auto i = 0u; i != nl + nu; ++i) {
      nnz(n + i) = pbm.A.outerIndexPtr()[LU_idx(i) + 1] - pbm.A.outerIndexPtr()[LU_idx(i)];
    }
    H.reserve(nnz);
    Hp.reserve(nnz + Eigen::Matrix<int, -1, 1>::Ones(n + nl + nu));

    using PIter = typename Eigen::SparseMatrix<Scalar, Eigen::ColMajor>::InnerIterator;
    using AIter = typename Eigen::SparseMatrix<Scalar, Eigen::RowMajor>::InnerIterator;

    // fill P in top left block
    for (Eigen::Index p_col = 0u; p_col != n; ++p_col) {
      for (PIter it(pbm.P, p_col); it && it.index() <= p_col; ++it) {
        H.insert(it.index(), p_col)  = it.value();
        Hp.insert(it.index(), p_col) = it.value();
      }
    }

    // fill selected rows of A in top right block
    for (auto a_row = 0u; a_row != nl + nu; ++a_row) {
      for (AIter it(pbm.A, LU_idx(a_row)); it; ++it) {
        H.insert(it.index(), n + a_row)  = it.value();
        Hp.insert(it.index(), n + a_row) = it.value();
      }
    }
  } else {
    H.setZero();
    H.topLeftCorner(n, n) = pbm.P;
    for (auto i = 0u; i != nl + nu; ++i) {
      H.col(n + i).template head<N>(n) = pbm.A.row(LU_idx(i));
    }
    Hp = H;
  }

  // add perturbing diagonal elements to Hp
  if constexpr (sparse) {
    for (auto i = 0u; i != n; ++i) { Hp.coeffRef(i, i) += prm.delta; }
    for (auto i = 0u; i != nl + nu; ++i) { Hp.coeffRef(n + i, n + i) -= prm.delta; }
    H.makeCompressed();
    Hp.makeCompressed();
  } else {
    Hp.topLeftCorner(n, n) += VecX::Constant(n, prm.delta).asDiagonal();
    Hp.bottomRightCorner(nl + nu, nl + nu) -= VecX::Constant(nl + nu, prm.delta).asDiagonal();
  }

  VecX h(n + nl + nu);
  h.head(n) = -pbm.q;
  for (auto i = 0u; i != nl; ++i) { h(n + i) = pbm.l(LU_idx(i)); }
  for (auto i = 0u; i != nu; ++i) { h(n + nl + i) = pbm.u(LU_idx(nl + i)); }

  // ITERATIVE REFINEMENT

  // factorize Hp
  std::conditional_t<
    sparse,
    Eigen::SimplicialLDLT<decltype(H), Eigen::Upper>,
    Eigen::LDLT<Eigen::Ref<decltype(H)>, Eigen::Upper>>
    ldlt(Hp);

  if (ldlt.info()) { return false; }

  VecX t_hat = VecX::Zero(n + nl + nu);
  for (auto i = 0u; i != prm.polish_iter; ++i) {
    t_hat += ldlt.solve(h - H.template selfadjointView<Eigen::Upper>() * t_hat);
  }

  // UPDATE SOLUTION

  sol.primal = t_hat.template head<N>(n);
  for (Eigen::Index i = 0; i < nl; ++i) { sol.dual(LU_idx(i)) = t_hat(n + i); }
  for (Eigen::Index i = 0; i < nu; ++i) { sol.dual(LU_idx(nl + i)) = t_hat(n + nl + i); }
  sol.objective = sol.primal.dot(0.5 * pbm.P * sol.primal + pbm.q);

  return true;
}

/**
 * @brief Check stopping criterion for QP solver.
 */
template<typename Pbm, typename D1, typename D2, typename D3, typename D4, typename D5>
std::optional<QPSolutionStatus> qp_check_stopping(
  const Pbm & pbm,
  const Eigen::MatrixBase<D1> & x,
  const Eigen::MatrixBase<D2> & y,
  const Eigen::MatrixBase<D3> & z,
  const Eigen::MatrixBase<D4> & dx,
  const Eigen::MatrixBase<D5> & dy,
  const QPSolverParams & prm)
{
  using Scalar = typename decltype(Pbm::A)::Scalar;

  const Eigen::Index n = pbm.A.cols(), m = pbm.A.rows();
  static constexpr Scalar inf = std::numeric_limits<Scalar>::infinity();
  const auto norm = [](auto && t) -> Scalar { return t.template lpNorm<Eigen::Infinity>(); };

  // working memory
  Eigen::Matrix<Scalar, decltype(Pbm::A)::ColsAtCompileTime, 1> Px(n), Aty(n);
  Eigen::Matrix<Scalar, decltype(Pbm::A)::RowsAtCompileTime, 1> Ax(m);

  // OPTIMALITY

  // check primal
  Ax.noalias() = pbm.A * x;
  if (norm(Ax - z) <= prm.eps_abs + prm.eps_rel * std::max<Scalar>(norm(Ax), norm(z))) {
    // primal succeeded, check dual
    Px.noalias()            = pbm.P * x;
    Aty.noalias()           = pbm.A.transpose() * y;
    const Scalar dual_scale = std::max<Scalar>({norm(Px), norm(pbm.q), norm(Aty)});
    if (norm(Px + pbm.q + Aty) <= prm.eps_abs + prm.eps_rel * dual_scale) {
      return QPSolutionStatus::Optimal;
    }
  }

  // PRIMAL INFEASIBILITY

  Aty.noalias()         = pbm.A.transpose() * dy;  // note new value A' * dy
  const Scalar Edy_norm = norm(dy);

  Scalar u_dyp_plus_l_dyn = Scalar(0);
  for (auto i = 0u; i != m; ++i) {
    if (pbm.u(i) != inf) {
      u_dyp_plus_l_dyn += pbm.u(i) * std::max<Scalar>(Scalar(0), dy(i));
    } else if (dy(i) > prm.eps_primal_inf * Edy_norm) {
      // contributes +inf to sum --> no certificate
      u_dyp_plus_l_dyn = inf;
      break;
    }
    if (pbm.l(i) != -inf) {
      u_dyp_plus_l_dyn += pbm.l(i) * std::min<Scalar>(Scalar(0), dy(i));
    } else if (dy(i) < -prm.eps_primal_inf * Edy_norm) {
      // contributes +inf to sum --> no certificate
      u_dyp_plus_l_dyn = inf;
      break;
    }
  }

  if (std::max<Scalar>(norm(Aty), u_dyp_plus_l_dyn) < prm.eps_primal_inf * Edy_norm) {
    return QPSolutionStatus::PrimalInfeasible;
  }

  // DUAL INFEASIBILITY

  Ax.noalias()         = pbm.A * dx;  // note new value A * dx
  const Scalar dx_norm = norm(dx);

  bool dual_infeasible = (norm(pbm.P * dx) <= prm.eps_dual_inf * dx_norm)
                      && (pbm.q.dot(dx) <= prm.eps_dual_inf * dx_norm);
  for (auto i = 0u; i != m && dual_infeasible; ++i) {
    if (pbm.u(i) == inf) {
      dual_infeasible &= (Ax(i) >= -prm.eps_dual_inf * dx_norm);
    } else if (pbm.l(i) == -inf) {
      dual_infeasible &= (Ax(i) <= prm.eps_dual_inf * dx_norm);
    } else {
      dual_infeasible &= std::abs(Ax(i)) < prm.eps_dual_inf * dx_norm;
    }
  }

  if (dual_infeasible) { return QPSolutionStatus::DualInfeasible; }

  return std::nullopt;
}

}  // namespace detail

/**
 * @brief Solve a quadratic program using the operator splitting approach.
 *
 * @tparam Pbm problem type (QuadraticProgram or QuadraticProgramSparse)
 *
 * @param pbm problem formulation
 * @param prm solver options
 * @param warmstart provide initial guess for primal and dual variables
 * @return solution as QuasraticProgramSolution<M, N>
 *
 * @note dynamic problem sizes (`M == -1 || N == -1`) are supported
 *
 * This is a third-party implementation of the algorithm described in the following paper:
 * * Stellato, B., Banjac, G., Goulart, P. et al.
 * **OSQP: an operator splitting solver for quadratic programs.**
 * *Math. Prog. Comp.* 12, 637–672 (2020).
 * https://doi.org/10.1007/s12532-020-00179-2
 *
 * For the official C implementation, see https://osqp.org/.
 */
template<typename Pbm>
detail::qp_solution_t<Pbm> solve_qp(
  const Pbm & pbm,
  const QPSolverParams & prm,
  std::optional<std::reference_wrapper<const detail::qp_solution_t<Pbm>>> warmstart = {})
{
  using AmatT                  = decltype(Pbm::A);
  using Scalar                 = typename AmatT::Scalar;
  static constexpr bool sparse = std::is_base_of_v<Eigen::SparseMatrixBase<AmatT>, AmatT>;

  // static sizes
  static constexpr Eigen::Index M = AmatT::RowsAtCompileTime;
  static constexpr Eigen::Index N = AmatT::ColsAtCompileTime;
  static constexpr Eigen::Index K = (N == -1 || M == -1) ? Eigen::Index(-1) : N + M;

  // typedefs
  using Rn = Eigen::Matrix<Scalar, N, 1>;
  using Rm = Eigen::Matrix<Scalar, M, 1>;
  using Rk = Eigen::Matrix<Scalar, K, 1>;

  // dynamic sizes
  const Eigen::Index n = pbm.A.cols(), m = pbm.A.rows(), k = n + m;

  static constexpr Scalar inf = std::numeric_limits<Scalar>::infinity();

  // cast parameters to scalar type
  const Scalar rho_bar    = static_cast<Scalar>(prm.rho);
  const Scalar alpha      = static_cast<Scalar>(prm.alpha);
  const Scalar alpha_comp = Scalar(1) - alpha;
  const Scalar sigma      = static_cast<Scalar>(prm.sigma);

  // return code: when set algorithm is finished
  std::optional<QPSolutionStatus> ret_code = std::nullopt;

  // allocate working arrays
  Rn x_us(n), dx_us(n);
  Rm z_next(m), y_us(m), z_us(m), dy_us(m), rho(m);
  Rk p(k);

  // scale problem
  Scalar c                      = 1;
  Eigen::Matrix<Scalar, K, 1> S = Eigen::Matrix<Scalar, K, 1>::Ones(k);
  Pbm spbm                      = pbm;
  if (prm.scaling) { std::tie(spbm, S, c) = detail::scale_qp(pbm); }

  for (auto i = 0u; i != m; ++i) {
    if (spbm.l(i) == inf || spbm.u(i) == -inf || spbm.u(i) - spbm.l(i) < Scalar(0.)) {
      ret_code = QPSolutionStatus::PrimalInfeasible;  // feasible set trivially empty
    }

    // set rho depending on constraint type
    if (spbm.l(i) == -inf && spbm.u(i) == inf) {
      rho(i) = Scalar(1e-6);  // unbounded
    } else if (abs(spbm.l(i) - spbm.u(i)) < 1e-5) {
      rho(i) = Scalar(1e3) * rho_bar;  // equality
    } else {
      rho(i) = rho_bar;  // inequality
    }
  }

  const auto t0 = std::chrono::high_resolution_clock::now();

  // fill square symmetric system matrix H
  std::conditional_t<sparse, Eigen::SparseMatrix<Scalar>, Eigen::Matrix<Scalar, K, K>> H(k, k);
  if constexpr (sparse) {
    // preallocate nonzeros in H
    Eigen::Matrix<int, -1, 1> nnz(k);
    for (auto i = 0u; i != n; ++i) {
      nnz(i) = spbm.P.outerIndexPtr()[i + 1] - spbm.P.outerIndexPtr()[i] + 1;
    }
    for (auto i = 0u; i != m; ++i) {
      nnz(n + i) = spbm.A.outerIndexPtr()[i + 1] - spbm.A.outerIndexPtr()[i] + 1;
    }
    H.reserve(nnz);

    // fill nonzeros in H
    using PIter = typename Eigen::SparseMatrix<Scalar, Eigen::ColMajor>::InnerIterator;
    using AIter = typename Eigen::SparseMatrix<Scalar, Eigen::RowMajor>::InnerIterator;
    for (Eigen::Index col = 0u; col != n; ++col) {
      for (PIter it(spbm.P, col); it && it.index() <= col; ++it) {
        H.insert(it.index(), col) = it.value();
      }
      H.coeffRef(col, col) += sigma;
    }
    for (auto row = 0u; row != m; ++row) {
      for (AIter it(spbm.A, row); it; ++it) { H.insert(it.index(), n + row) = it.value(); }
      H.insert(n + row, n + row) = Scalar(-1) / rho(row);
    }
    H.makeCompressed();
  } else {
    H.template topLeftCorner<N, N>(n, n) = spbm.P;
    H.template topLeftCorner<N, N>(n, n) += Rn::Constant(n, sigma).asDiagonal();
    H.template topRightCorner<N, M>(n, m)    = spbm.A.transpose();
    H.template bottomRightCorner<M, M>(m, m) = (-rho).cwiseInverse().asDiagonal();
  }

  const auto t_fill = std::chrono::high_resolution_clock::now();

  if (prm.verbose) {
    using std::cout, std::left, std::setw, std::right;
    // clang-format off
    cout << "========================= QP Solver =========================" << '\n';
    cout << "Solving " << (sparse ? "sparse" : "dense") << " QP with n=" << n << ", m=" << m << '\n';
    cout << setw(8)  << right << "ITER"
         << setw(14) << right << "OBJ"
         << setw(14) << right << "PRI_RES"
         << setw(14) << right << "DUA_RES"
         << setw(10) << right << "TIME" << '\n';
    // clang-format on
  }

  // factorize H
  std::conditional_t<
    sparse,
    Eigen::SimplicialLDLT<decltype(H), Eigen::Upper>,
    Eigen::LDLT<Eigen::Ref<decltype(H)>, Eigen::Upper>>
    ldlt(H);

  const auto t_factor = std::chrono::high_resolution_clock::now();

  if (ldlt.info()) { ret_code = QPSolutionStatus::Unknown; }

  // initialize solver variables
  Rn x;
  Rm z, y;
  if (warmstart.has_value()) {
    // warmstart variables must be scaled
    x = warmstart.value().get().primal;
    x.applyOnTheLeft(S.template head<N>(n).cwiseInverse().asDiagonal());
    y = warmstart.value().get().dual;
    y.applyOnTheLeft(S.template segment<M>(n, m).cwiseInverse().asDiagonal());
    y *= c;
    z.noalias() = spbm.A * x;
  } else {
    x.setZero(n);
    y.setZero(m);
    z.setZero(m);
  }

  // main optimization loop
  auto iter = 0u;
  for (; (!prm.max_iter || iter != prm.max_iter.value()) && !ret_code; ++iter) {
    p.template head<N>(n)       = sigma * x - spbm.q;
    p.template segment<M>(n, m) = z - rho.cwiseInverse().cwiseProduct(y);
    p                           = ldlt.solve(p);

    if (iter % prm.stop_check_iter == 1) {
      // termination checking requires difference, store old scaled values
      dx_us = x, dy_us = y;
    }

    x      = alpha * p.template head<N>(n) + alpha_comp * x;
    z_next = (alpha * rho.cwiseInverse().cwiseProduct(p.template segment<M>(n, m))
              + alpha_comp * rho.cwiseInverse().cwiseProduct(y) + z)
               .cwiseMax(spbm.l)
               .cwiseMin(spbm.u);
    y = alpha_comp * y + alpha * p.template segment<M>(n, m) + rho.cwiseProduct(z)
      - rho.cwiseProduct(z_next);
    z = z_next;

    if (iter % prm.stop_check_iter == 1) {
      // check stopping criteria for unscaled problem and unscaled variables
      x_us     = S.template head<N>(n).cwiseProduct(x);
      y_us     = S.template segment<M>(n, m).cwiseProduct(y) / c;
      z_us     = S.template segment<M>(n, m).cwiseInverse().cwiseProduct(z);
      dx_us    = S.template head<N>(n).cwiseProduct(x - dx_us);
      dy_us    = S.template segment<M>(n, m).cwiseProduct(y - dy_us) / c;
      ret_code = detail::qp_check_stopping(pbm, x_us, y_us, z_us, dx_us, dy_us, prm);

      if (prm.verbose) {
        using std::cout, std::setw, std::right, std::chrono::microseconds;
        // clang-format off
        cout << setw(7) << right << iter << ":"
          << std::scientific
          << setw(14) << right << (0.5 * pbm.P * x_us + pbm.q).dot(x_us)
          << setw(14) << right << (pbm.A * x_us - z_us).template lpNorm<Eigen::Infinity>()
          << setw(14) << right << (pbm.P * x_us + pbm.q + pbm.A.transpose() * y_us).template lpNorm<Eigen::Infinity>()
          << setw(10) << right << duration_cast<microseconds>(std::chrono::high_resolution_clock::now() - t0).count()
          << '\n';
        // clang-format on
      }

      // check for timeout
      if (!ret_code) {
        if (prm.max_time && std::chrono::high_resolution_clock::now() > t0 + prm.max_time.value()) {
          ret_code = QPSolutionStatus::MaxTime;
        }
      }
    }
  }

  double obj = x.dot(0.5 * pbm.P * x + pbm.q);

  detail::qp_solution_t<Pbm> sol{
    .code      = ret_code.value_or(QPSolutionStatus::MaxIterations),
    .iter      = iter - 1,
    .primal    = std::move(x),
    .dual      = std::move(y),
    .objective = obj,
  };

  const auto t_iter = std::chrono::high_resolution_clock::now();

  // polish solution if optimal
  if (sol.code == QPSolutionStatus::Optimal && prm.polish) {
    if (detail::polish_qp(spbm, sol, prm)) {
      if (prm.verbose) {
        using std::cout, std::setw, std::right, std::chrono::microseconds;
        x_us = S.template head<N>(n).cwiseProduct(sol.primal);          // NOTE: x std::moved to sol
        y_us = S.template segment<M>(n, m).cwiseProduct(sol.dual) / c;  // NOTE: y std::moved to sol
        z_us = S.template segment<M>(n, m).cwiseInverse().cwiseProduct(z);
        // clang-format off
        cout << setw(8) << right << "polish:"
          << std::scientific
          << setw(14) << right << (0.5 * pbm.P * x_us + pbm.q).dot(x_us)
          << setw(14) << right << (pbm.A * x_us - z_us).template lpNorm<Eigen::Infinity>()
          << setw(14) << right << (pbm.P * x_us + pbm.q + pbm.A.transpose() * y_us).template lpNorm<Eigen::Infinity>()
          << setw(10) << right << duration_cast<microseconds>(std::chrono::high_resolution_clock::now() - t0).count()
          << '\n';
        // clang-format on
      }

    } else {
      if (prm.verbose) { std::cout << "Polish failed" << '\n'; }
      sol.code = QPSolutionStatus::PolishFailed;
    }
  }

  const auto t_polish = std::chrono::high_resolution_clock::now();

  // unscale solution
  sol.primal.applyOnTheLeft(S.template head<N>(n).asDiagonal());
  sol.dual.applyOnTheLeft(S.template segment<M>(n, m).asDiagonal());
  sol.dual /= c;
  sol.objective = sol.primal.dot(0.5 * pbm.P * sol.primal + pbm.q);

  if (prm.verbose) {
    using std::cout, std::left, std::right, std::setw, std::chrono::microseconds;

    // clang-format off
    cout << "QP solver summary:" << '\n';
    cout << "Result " << static_cast<int>(sol.code) << '\n';

    cout << setw(25) << left << "Iterations"        << setw(10) << right << iter - 1                                               << '\n';
    cout << setw(26) << left << "Total time (µs)"   << setw(10) << right << duration_cast<microseconds>(t_polish - t0).count()     << '\n';
    cout << setw(25) << left << "  Matrix filling"  << setw(10) << right << duration_cast<microseconds>(t_fill - t0).count()       << '\n';
    cout << setw(25) << left << "  Factorization"   << setw(10) << right << duration_cast<microseconds>(t_factor - t_fill).count() << '\n';
    cout << setw(25) << left << "  Iteration"       << setw(10) << right << duration_cast<microseconds>(t_iter - t_factor).count() << '\n';
    cout << setw(25) << left << "  Polish"          << setw(10) << right << duration_cast<microseconds>(t_polish - t_iter).count() << '\n';
    cout << "=============================================================" << '\n';
    // clang-format on
  }

  return sol;
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__QP_HPP_


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// MetricTools.hpp
// This file is part of the Garamon Generator.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file MetricTools.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Analyse and transform the metric related to the specified algebra.

#ifndef GARAGEN_METRICTOOLS_HPP
#define GARAGEN_METRICTOOLS_HPP

#include <Eigen/Core>
#include <vector>
#include <Eigen/Sparse>
#include <Eigen/LU> // for inverting

#include "Utilities.hpp"

bool isMatrixDiagonal(const Eigen::MatrixXd &metric, const double epsilon);

bool isMatrixIdentity(const Eigen::MatrixXd &metric, const double epsilon);

bool isMatrixPermutationOfDiagonal(const Eigen::MatrixXd &metric, const double epsilon);

unsigned int getRank(const Eigen::MatrixXd &metric);

void eigenDecomposition(const Eigen::MatrixXd &M, Eigen::MatrixXd &P, Eigen::MatrixXd &A);

double minAbsNonZeroValue(Eigen::VectorXd x);

Eigen::MatrixXd eigenRefinement(Eigen::MatrixXd &P, Eigen::MatrixXd &D, Eigen::MatrixXd &Pinv);

Eigen::MatrixXd numericalCleanUp(const Eigen::MatrixXd &M, const double epsilon);

Eigen::VectorXd vectorNumericalCleanUp(const Eigen::VectorXd& original, const double epsilon);

bool checkNumericalCleanUp(const Eigen::MatrixXd &M, const Eigen::MatrixXd &P, const Eigen::MatrixXd &A, const double epsilon);

bool checkNumericalCleanUp(const Eigen::MatrixXd &M, const Eigen::MatrixXd &P, const Eigen::MatrixXd &A, const Eigen::MatrixXd &Pinv, const double epsilon);

Eigen::SparseMatrix<double> numericalCleanUpSparse(const Eigen::MatrixXd &M, const double epsilon);

Eigen::SparseMatrix<double, Eigen::ColMajor> computePerGradeTransformationMatrix(const Eigen::MatrixXd &vectorTransformationMatrix,
                                         const unsigned int dimension, const unsigned int grade, const double epsilon);

Eigen::SparseMatrix<double, Eigen::ColMajor>  computeInverseTransformationMatrix(const Eigen::SparseMatrix<double, Eigen::ColMajor>& transformationMatrix, const double epsilon);

std::pair<std::vector<double>,std::vector<double>> computeTransformationMatricesToVector(const Eigen::MatrixXd &P, const double epsilon,std::vector<unsigned int>& transformationMatricesSizes,
                                                                                std::vector<Eigen::SparseMatrix<double, Eigen::ColMajor> >& allTransformationMatrices,
                                                                                std::vector<Eigen::SparseMatrix<double, Eigen::ColMajor> >& allInverseTransformationMatrices);

std::vector<double> transformationMatricesToVectorOfComponents(const Eigen::SparseMatrix<double, Eigen::ColMajor> &transformationMatrix, const int grade, const bool isInverse);

#endif //GARAGEN_METRICTOOLS_HPP


/*
 dparallel_recursion: distributed parallel_recursion skeleton
 Copyright (C) 2015-2018 Carlos H. Gonzalez, Basilio B. Fraguela. Universidade da Coruna

 Licensed under the Apache License, Version 2.0 (the "License");
 you may not use this file except in compliance with the License.
 You may obtain a copy of the License at

 http://www.apache.org/licenses/LICENSE-2.0

 Unless required by applicable law or agreed to in writing, software
 distributed under the License is distributed on an "AS IS" BASIS,
 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 See the License for the specific language governing permissions and
 limitations under the License.
 */

#ifdef OPENBLAS

#include "cblas.h"

void mxm(const MMatrix& a, const MMatrix& b, MMatrix& c)
{
  openblas_set_num_threads(1);

  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
              a.rows(), c.cols(), a.cols(),
              1.0, a.raw(), a.row_stride(),
              b.raw(), b.row_stride(),
              0.0, c.raw(), c.row_stride());
}

#else

#ifndef NOBOOST

#define BOOST_UBLAS_SHALLOW_ARRAY_ADAPTOR
#define BOOST_UBLAS_NDEBUG

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>

void mxm(const MMatrix& a, const MMatrix& b, MMatrix& c)
{
  using namespace boost::numeric::ublas;

  typedef matrix<double, row_major, shallow_array_adaptor<double> > MyMatrixType;

  shallow_array_adaptor<double> am(a.rows() * a.row_stride(), a.raw());
  shallow_array_adaptor<double> bm(b.rows() * b.row_stride(), b.raw());
  shallow_array_adaptor<double> cm(c.rows() * c.row_stride(), c.raw());

  MyMatrixType ax(a.rows(), a.row_stride(), am);
  MyMatrixType bx(b.rows(), b.row_stride(), bm);
  MyMatrixType cx(c.rows(), c.row_stride(), cm);

  matrix_range<MyMatrixType> mra (ax, range (0, a.rows()), range (0, a.cols()));
  matrix_range<MyMatrixType> mrb (bx, range (0, b.rows()), range (0, b.cols()));
  matrix_range<MyMatrixType> mrc (cx, range (0, c.rows()), range (0, c.cols()));

  axpy_prod(mra, mrb, mrc, true);
}

#else

void mxm(const MMatrix& a, const MMatrix& b, MMatrix& c)
{
  const int common_dim = a.cols();
  assert(common_dim == b.rows());
  for (int i = 0; i < c.rows(); i++) {
    for (int j = 0; j < c.cols(); j++) {
      double r = 0.0;
      for (int k = 0; k < common_dim; k++) {
        r += a(i, k) * b(k, j);
      }
      c(i, j) = r;
    }
  }
}

#endif

#endif