#include "Utilities/DBC.hh"
#include <Eigen/Dense>
#include <iostream>

namespace Spheral {

//------------------------------------------------------------------------------
// Default constructor
//------------------------------------------------------------------------------
inline
BiQuadraticInterpolator::BiQuadraticInterpolator():
  mnx1(),
  mny1(),
  mxmin(),
  mxmax(),
  mymin(),
  mymax(),
  mxstep(),
  mystep(),
  mcoeffs() {
}

//------------------------------------------------------------------------------
// Construct with tabulated data
//------------------------------------------------------------------------------
template<typename Func>
BiQuadraticInterpolator::BiQuadraticInterpolator(const double xmin,
                                                 const double xmax,
                                                 const double ymin,
                                                 const double ymax,
                                                 const size_t nx,
                                                 const size_t ny,
                                                 const Func& F):
  mnx1(),
  mny1(),
  mxmin(),
  mxmax(),
  mxstep(),
  mcoeffs() {
  this->initialize(xmin, xmax, ymin, ymax, nx, ny, F);
}

//------------------------------------------------------------------------------
// Destructor
//------------------------------------------------------------------------------
inline
BiQuadraticInterpolator::~BiQuadraticInterpolator() {
}

//------------------------------------------------------------------------------
// Initialize the interpolation to fit the given data
//------------------------------------------------------------------------------
template<typename Func>
inline
void
BiQuadraticInterpolator::initialize(const double xmin,
                                    const double xmax,
                                    const double ymin,
                                    const double ymax,
                                    const size_t nx,
                                    const size_t ny,
                                    const Func& F) {

  // Size stuff up.
  REQUIRE(nx > 2u);
  REQUIRE(ny > 2u);
  mnx1 = nx - 2u;
  mny1 = ny - 2u;
  mcoeffs.resize(6*mnx1*mny1);

  // Figure out the sampling steps.
  mxmin = xmin;
  mxmax = xmax;
  mymin = ymin;
  mymax = ymax;
  mxstep = (xmax - xmin)/(nx - 1u);
  mystep = (ymax - ymin)/(ny - 1u);

  // Fit the coefficients
  Eigen::Vector2d x00, x01, x02, x10, x11, x12, x20, x21, x22;
  Eigen::MatrixXd A(9, 6);
  Eigen::VectorXd b(9), c(9);
  for (auto i = 0u; i < mnx1; ++i) {
    for (auto j = 0u; j < mny1; ++j) {
      x00 = {xmin + i      *mxstep, ymin + j      *mystep};
      x10 = {xmin + (i + 1)*mxstep, ymin + j      *mystep};
      x20 = {xmin + (i + 2)*mxstep, ymin + j      *mystep};
      x01 = {xmin + i      *mxstep, ymin + (j + 1)*mystep};
      x11 = {xmin + (i + 1)*mxstep, ymin + (j + 1)*mystep};
      x21 = {xmin + (i + 2)*mxstep, ymin + (j + 1)*mystep};
      x02 = {xmin + i      *mxstep, ymin + (j + 2)*mystep};
      x12 = {xmin + (i + 1)*mxstep, ymin + (j + 2)*mystep};
      x22 = {xmin + (i + 2)*mxstep, ymin + (j + 2)*mystep};
      A << 1.0, x00[0], x00[1], x00[0]*x00[1], x00[0]*x00[0], x00[1]*x00[1],
           1.0, x01[0], x01[1], x01[0]*x01[1], x01[0]*x01[0], x01[1]*x01[1],
           1.0, x02[0], x02[1], x02[0]*x02[1], x02[0]*x02[0], x02[1]*x02[1],
           1.0, x10[0], x10[1], x10[0]*x10[1], x10[0]*x10[0], x10[1]*x10[1],
           1.0, x11[0], x11[1], x11[0]*x11[1], x11[0]*x11[0], x11[1]*x11[1],
           1.0, x12[0], x12[1], x12[0]*x12[1], x12[0]*x12[0], x12[1]*x12[1],
           1.0, x20[0], x20[1], x20[0]*x20[1], x20[0]*x20[0], x20[1]*x20[1],
           1.0, x21[0], x21[1], x21[0]*x21[1], x21[0]*x21[0], x21[1]*x21[1],
           1.0, x22[0], x22[1], x22[0]*x22[1], x22[0]*x22[0], x22[1]*x22[1];
      b << F(x00[0], x00[1]),
           F(x01[0], x01[1]),
           F(x02[0], x02[1]),
           F(x10[0], x10[1]),
           F(x11[0], x11[1]),
           F(x12[0], x12[1]),
           F(x20[0], x20[1]),
           F(x21[0], x21[1]),
           F(x22[0], x22[1]);
      CHECK2(b == b, "BiQuadraticInterpoolator function return error: \n"
             << x00 << " : " << b[0] << "\n"
             << x01 << " : " << b[1] << "\n"
             << x02 << " : " << b[2] << "\n"
             << x10 << " : " << b[3] << "\n"
             << x11 << " : " << b[4] << "\n"
             << x12 << " : " << b[5] << "\n"
             << x20 << " : " << b[6] << "\n"
             << x21 << " : " << b[7] << "\n"
             << x22 << " : " << b[8] << "\n");
      c = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
      // std::cerr << "------------------------------------------------------------------------------\n"
      //           << "x00: " << x00 << "\n"
      //           << "x10: " << x10 << "\n"
      //           << "x20: " << x20 << "\n"
      //           << "x01: " << x01 << "\n"
      //           << "x11: " << x11 << "\n"
      //           << "x21: " << x21 << "\n"
      //           << "x02: " << x02 << "\n"
      //           << "x12: " << x12 << "\n"
      //           << "x22: " << x22 << "\n"
      //           << "A:\n" << A << "\n"
      //           << "b:\n" << b << "\n"
      //           << "c:\n" << c << "\n";
      const auto k = 6*(i + j*mnx1);
      mcoeffs[k    ] = c(0);
      mcoeffs[k + 1] = c(1);
      mcoeffs[k + 2] = c(2);
      mcoeffs[k + 3] = c(3);
      mcoeffs[k + 4] = c(4);
      mcoeffs[k + 5] = c(5);
    }
  }
}

//------------------------------------------------------------------------------
// Interpolate for the given coordinate.
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::operator()(const double xi,
                                    const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  // std::cerr << "================================================================================\n"
  //           << "mxlog, mylog : " << mxlog << " " << mylog << "\n"
  //           << "pos   : " << pos << "\n"
  //           << "(x,y) : " << x << " " << y << "\n"
  //           << "i0    : " << i0 << "\n"
  //           << "coeffs: " << mcoeffs[i0] << " " << mcoeffs[i0 + 1] << " " << mcoeffs[i0 + 2] << " " << mcoeffs[i0 + 3] << " " << mcoeffs[i0 + 4] << " " << mcoeffs[i0 + 5] << "\n"
  //           << "F(x,y): " << mcoeffs[i0] + mcoeffs[i0 + 1]*x + mcoeffs[i0 + 2]*y + mcoeffs[i0 + 3]*x*y + mcoeffs[i0 + 4]*x*x + mcoeffs[i0 + 5]*y*y << "\n";
  return mcoeffs[i0] + mcoeffs[i0 + 1]*x + mcoeffs[i0 + 2]*y + mcoeffs[i0 + 3]*x*y + mcoeffs[i0 + 4]*x*x + mcoeffs[i0 + 5]*y*y;
}

//------------------------------------------------------------------------------
// Interpolate for the gradient (x)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime_x(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return mcoeffs[i0 + 1] + mcoeffs[i0 + 3]*y + 2.0*mcoeffs[i0 + 4]*x;
}

//------------------------------------------------------------------------------
// Interpolate for the gradient (y)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime_y(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return mcoeffs[i0 + 2] + mcoeffs[i0 + 3]*x + 2.0*mcoeffs[i0 + 5]*y;
}

//------------------------------------------------------------------------------
// Interpolate for the gradient2 (xx)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime2_xx(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return 2.0*mcoeffs[i0 + 4];
}

//------------------------------------------------------------------------------
// Interpolate for the gradient2 (xy)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime2_xy(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return mcoeffs[i0 + 3];
}

//------------------------------------------------------------------------------
// Interpolate for the gradient2 (yx)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime2_yx(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return mcoeffs[i0 + 3];
}

//------------------------------------------------------------------------------
// Interpolate for the gradient2 (yy)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime2_yy(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return 2.0*mcoeffs[i0 + 5];
}

//------------------------------------------------------------------------------
// Return the lower bound entry in the table for the given x coordinate
//------------------------------------------------------------------------------
inline
size_t
BiQuadraticInterpolator::lowerBound(const double x, const double y) const {
  const auto result = 6u*(mnx1*std::min(mny1 - 1u, size_t(std::max(0.0, y - mymin)/mystep)) +
                               std::min(mnx1 - 1u, size_t(std::max(0.0, x - mxmin)/mxstep)));
  ENSURE(result <= 6u*mnx1*mny1);
  return result;
}

//------------------------------------------------------------------------------
// Data accessors
//------------------------------------------------------------------------------
inline
size_t
BiQuadraticInterpolator::size() const {
  return mcoeffs.size();
}

inline
double
BiQuadraticInterpolator::xmin() const {
  return mxmin;
}

inline
double
BiQuadraticInterpolator::xmax() const {
  return mxmax;
}

inline
double
BiQuadraticInterpolator::ymin() const {
  return mymin;
}

inline
double
BiQuadraticInterpolator::ymax() const {
  return mymax;
}

inline
double
BiQuadraticInterpolator::xstep() const {
  return mxstep;
}

inline
double
BiQuadraticInterpolator::ystep() const {
  return mystep;
}

inline
const std::vector<double>&
BiQuadraticInterpolator::coeffs() const {
  return mcoeffs;
}

}


//***********************************************************
// 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;
}

}


//==================================================================================================
/*!
  @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);
}


// 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


/* 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_


/*
 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


/**
 * Copyright (c) 2022 <Daumantas Kavolis>
 *
 * 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.
 */

#pragma once

#include <array>
#include <vector>

#include "config.hpp"

MSVC_WARNING_DISABLE(4619)
#include <boost/math/quadrature/gauss.hpp>
#include <boost/math/quadrature/gauss_kronrod.hpp>
MSVC_WARNING_POP()

#include "utils.hpp"

namespace poly {

namespace detail {
/// \brief function pointer type to get weights/abscissa
template <class Real>
using quadrature_getter = view<Real const> (*)();

template <Reflection Refl, class Container>
[[nodiscard]] auto full_vector(Container&& points, size_t expected_points [[maybe_unused]])
    -> decltype(auto) {
  using Real = std::remove_cvref_t<typename std::remove_cvref_t<Container>::value_type>;

  std::vector<Real> v = to_vector(points);
  if constexpr (Refl != Reflection::None) {
    reflect_in_place<Refl>(v, 2 * v.size() > expected_points);
  }

  return v;
}

template <class Real, template <class, unsigned> typename Impl, bool Reflect = true>
struct BaseQuadrature {
  template <unsigned Points>
  static auto weights() -> view<Real const> {
    static std::vector<Real> items =
        full_vector<Reflection::Even>(Impl<Real, Points>::weights(), Points);
    return {items.data(), items.size()};
  }

  template <unsigned Points>
  static auto abscissa() -> view<Real const> {
    static std::vector<Real> items =
        full_vector<Reflection::Odd>(Impl<Real, Points>::abscissa(), Points);
    return {items.data(), items.size()};
  }
};

/// \brief Wrapper for boost quadrature to convert compile time points count to runtime
/// \tparam Real floating point type
/// \tparam Impl Type implementing static functions weights() and abscissa()
/// \tparam N Maximum number of points in quadrature
/// \tparam Reflect Whether the values from Impl should be reflected first
template <typename Real, template <class, unsigned> typename Impl, unsigned N = QuadraturePoints,
          bool Reflect = true>
struct Quadrature : BaseQuadrature<Real, Impl, Reflect> {
  using Base = BaseQuadrature<Real, Impl, Reflect>;
  template <bool check>
  static auto weights(unsigned points, bounds_check<check> /* unused */ = no_bounds_check)
      -> view<Real const> {
    if constexpr (check)
      return weights_getters().at(points)();
    else
      return weights_getters()[points]();
  }

  template <bool check = false>
  static auto abscissa(unsigned points, bounds_check<check> /* unused */ = no_bounds_check)
      -> view<Real const> {
    if constexpr (check)
      return abscissa_getters().at(points)();
    else
      return abscissa_getters()[points]();
  }

 private:
  // using array of function pointers so that the values are only generated on first access
  using function_container = std::array<detail::quadrature_getter<Real>, N + 1> const;

  template <unsigned... I>
  static auto make_weights(std::integer_sequence<unsigned, I...> /*unused*/) {
    return function_container{&Base::template weights<I>...};
  }

  template <unsigned... I>
  static auto make_abscissa(std::integer_sequence<unsigned, I...> /*unused*/) {
    return function_container{&Base::template abscissa<I>...};
  }

  static auto weights_getters() -> function_container const& {
    static function_container functions =
        make_weights(std::make_integer_sequence<unsigned, N + 1>{});
    return functions;
  }

  static auto abscissa_getters() -> function_container const& {
    static function_container functions =
        make_abscissa(std::make_integer_sequence<unsigned, N + 1>{});
    return functions;
  }
};

template <typename R, unsigned n>
using GaussQuadrature = boost::math::quadrature::gauss<R, n>;

template <typename R, unsigned n>
using GaussKronrodQuadrature = boost::math::quadrature::gauss_kronrod<R, n>;
}  // namespace detail

template <typename Real, unsigned N = QuadraturePoints>
using GaussQuadrature = detail::Quadrature<Real, detail::GaussQuadrature, N, true>;

template <typename Real, unsigned N = QuadraturePoints>
using GaussKronrodQuadrature = detail::Quadrature<Real, detail::GaussKronrodQuadrature, N, true>;

}  // namespace poly


//==================================================================================================
/*!
  @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_TANH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_TANH_HPP_INCLUDED

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

 /*!

    @ingroup group-hyperbolic
    Function object implementing tanh capabilities

    Returns the hyperbolic tangent: \f$\sinh(x)/\cosh(x)\f$.

    @par Semantic:

    For every parameter of floating type @c T

    @code
    T r = tanh(x);
    @endcode

    @see sinh,  cosh
  **/
  Value tanh(Value const & v0);
} }
#endif

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

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/quantlib.hpp>
#include <boost/timer.hpp>

using namespace QuantLib;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {
Integer sessionId() { return 0; }
}
#endif

class Timer {
    boost::timer timer_;
    double elapsed_;

  public:
    void start() { timer_ = boost::timer(); }
    void stop() { elapsed_ = timer_.elapsed(); }
    double elapsed() const { return elapsed_; }
};

// reusable code snipped to perform one pricing step
#define PROXY_PRICING \
        npvRef = swaptionRef2->NPV(); \
        timer.start(); \
        npvProxy = swaption2->NPV(); \
        /*underlyingProxy = 0.0; swaption2->result<Real>("exerciseValue");*/ \
        timer.stop(); \
        npvProxyTiming = timer.elapsed(); \
        std::clog << "\nPricing results on " \
                  << Settings::instance().evaluationDate() \
                  << ", reference rate " << rateLevelRefQuote->value() \
                  << " with maturity " << maturityRefQuote->value() << "\n"; \
        std::clog << "Integral engine npv = " << npvRef << "\n"; \
        std::clog << "Proxy    engine npv = " << npvProxy \
                  << " (timing: " << npvProxyTiming*1000000.0 \
        << "mus)" /*<< ", underlying npv = " << underlyingProxy*/ << "\n";
// here the main part of the code starts

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

    try {

        std::clog << "Bermudan swaption proxy pricing example\n";

        // Timer seutp

        Timer timer;

        // Original evaluation date and rate level

        Real rateLevelOrig = 0.02;
        Date refDateOrig(12, January, 2015);

        Settings::instance().evaluationDate() = refDateOrig;

        // the yield term structure for the original pricing
        // this must _not_ be floating, see the warning in
        // the proxy engine's header.

        Handle<YieldTermStructure> ytsOrig(boost::make_shared<FlatForward>(
            refDateOrig, rateLevelOrig, Actual365Fixed()));

        // the yield term structure for reference pricings
        // (integral engine) on future dates, this _floating_

        boost::shared_ptr<SimpleQuote> rateLevelRefQuote =
            boost::make_shared<SimpleQuote>(0.02);
        Handle<Quote> rateLevelRef(rateLevelRefQuote);
        Handle<YieldTermStructure> ytsRef(boost::make_shared<FlatForward>(
            0, TARGET(), rateLevelRef, Actual365Fixed()));

        // the euribor index for the swaption's underlying.
        // one should again use the fixed yts ...
        boost::shared_ptr<IborIndex> euribor6m =
            boost::make_shared<Euribor>(6 * Months, ytsOrig);
        // but for the reference pricings in the integral engine
        // we need a floating version as well.
        boost::shared_ptr<IborIndex> euribor6mRef =
            boost::make_shared<Euribor>(6 * Months, ytsRef);

        // the length of the bermudan swaption in years

        Size length = 10;

        // instrument setup

        Real strike = 0.02; // near atm option
        Date effectiveDate = TARGET().advance(refDateOrig, 2 * Days);
        Date startDate = TARGET().advance(effectiveDate, 1 * Years);
        Date maturityDate = TARGET().advance(startDate, length * Years);

        Schedule fixedSchedule(startDate, maturityDate, 1 * Years, TARGET(),
                               ModifiedFollowing, ModifiedFollowing,
                               DateGeneration::Forward, false);
        Schedule floatingSchedule(startDate, maturityDate, 6 * Months, TARGET(),
                                  ModifiedFollowing, ModifiedFollowing,
                                  DateGeneration::Forward, false);

        boost::shared_ptr<VanillaSwap> underlying =
            boost::make_shared<VanillaSwap>(VanillaSwap(
                VanillaSwap::Payer, 1.0, fixedSchedule, strike, Thirty360(),
                floatingSchedule, euribor6m, 0.0, Actual360()));
        boost::shared_ptr<VanillaSwap> underlyingRef =
            boost::make_shared<VanillaSwap>(VanillaSwap(
                VanillaSwap::Payer, 1.0, fixedSchedule, strike, Thirty360(),
                floatingSchedule, euribor6mRef, 0.0, Actual360()));

        std::vector<Date> exerciseDates;
        for (Size i = 0; i < length; ++i) {
            exerciseDates.push_back(
                TARGET().advance(fixedSchedule[i], -2 * Days));
        }

        boost::shared_ptr<Exercise> exercise =
            boost::make_shared<BermudanExercise>(exerciseDates, false);

        boost::shared_ptr<Swaption> swaption =
            boost::make_shared<Swaption>(underlying, exercise);
        boost::shared_ptr<Swaption> swaptionRef =
            boost::make_shared<Swaption>(underlyingRef, exercise);

        // our instrument is a swaption, but the engine is for non standard
        // swaptions
        // so we just convert it

        boost::shared_ptr<NonstandardSwaption> swaption2 =
            boost::make_shared<NonstandardSwaption>(*swaption);
        boost::shared_ptr<NonstandardSwaption> swaptionRef2 =
            boost::make_shared<NonstandardSwaption>(*swaptionRef);

        // just take any model volatility and reversion, we do not calibrate
        // them here. Also they are flat, so no steps needed really.

        std::vector<Date> stepDates;
        std::vector<Real> sigmas(1, 0.0070);
        Real reversion = 0.0030;

        // the gsr model in T-forward measure, T=50 chosen arbitrary here
        // the first model uses the fixed yts, used for the mc pricing
        // generating the proxy

        boost::shared_ptr<Gsr> gsrFixed = boost::make_shared<Gsr>(
            ytsOrig, stepDates, sigmas, reversion, 50.0);

        // the second model is used for the reference pricing, therefore
        // using the floating yts

        boost::shared_ptr<Gsr> gsrFloating =
            boost::make_shared<Gsr>(ytsRef, stepDates, sigmas, reversion, 50.0);

        // the integral engine for reference pricings

        boost::shared_ptr<PricingEngine> integralEngine =
            boost::make_shared<Gaussian1dNonstandardSwaptionEngine>(
                gsrFloating, 64, 7.0, true, false, Handle<Quote>(), ytsRef);

        // compute a reference price for the inital pricing

        timer.start();
        swaption2->setPricingEngine(integralEngine);
        Real npvOrigIntegral = swaption2->NPV();
        timer.stop();
        Real npvOrigIntegralTiming = timer.elapsed();

        // the mc engine, note that we use the fixed model here

        boost::shared_ptr<PricingEngine> mcEngine =
            MakeMcGaussian1dNonstandardSwaptionEngine<>(gsrFixed)
                .withSteps(1) // the gsr model allows for large steps
                .withSamples(10000)
                .withSeed(42)
                .withCalibrationSamples(10000)
                .withProxy(true);

        // compute the mc price

        timer.start();
        swaption2->setPricingEngine(mcEngine);
        Real npvOrigMc = swaption2->NPV();
        Real errorOrigMc = swaption2->errorEstimate();
        timer.stop();
        Real npvOrigMcTiming = timer.elapsed();

        // output the results

        std::clog << "Pricing results on the original reference date ("
                  << refDateOrig << "):\n";
        std::clog << "Integral engine npv = " << npvOrigIntegral
                  << " (timing: " << npvOrigIntegralTiming*1000000.0 << "mus)\n";
        std::clog << "MC       engine npv = " << npvOrigMc << " error estimate "
                  << errorOrigMc << " (timing: " << npvOrigMcTiming*1000000.0 << "mus)\n";

        // proxy pricing, that is what this example is really about

        // reference maturity for the scenario rate

        boost::shared_ptr<SimpleQuote> maturityRefQuote =
            boost::make_shared<SimpleQuote>();
        Handle<Quote> maturityRef(maturityRefQuote);

        boost::shared_ptr<PricingEngine> proxyEngine =
            boost::make_shared<ProxyNonstandardSwaptionEngine>(
                swaption2->proxy(), rateLevelRef, maturityRef, 64, 7.0, false);

        Real npvRef, npvProxy, npvProxyTiming;

        swaptionRef2->setPricingEngine(integralEngine);
        swaption2->setPricingEngine(proxyEngine);

        // move forward by 6 months, to the middle of the first period

        Settings::instance().evaluationDate() = Date(12, June, 2015);
        rateLevelRefQuote->setValue(0.02); // no change
        maturityRefQuote->setValue(10.5);  // maturity of the underlying
        PROXY_PRICING;

        // move somewhere to the middle and check itm, otm, atm

        Settings::instance().evaluationDate() = Date(11, June, 2019);
        rateLevelRefQuote->setValue(0.025); // in the money
        maturityRefQuote->setValue(6.5);
        PROXY_PRICING;
        rateLevelRefQuote->setValue(0.02); // at the money
        PROXY_PRICING;
        rateLevelRefQuote->setValue(0.015); // out of the money
        PROXY_PRICING;

        // move to the beginning of a period

        Settings::instance().evaluationDate() = Date(11, January, 2020);
        rateLevelRefQuote->setValue(0.02);
        maturityRefQuote->setValue(6.0);
        PROXY_PRICING;

        // move to the end of a period

        Settings::instance().evaluationDate() = Date(11, January, 2021);
        rateLevelRefQuote->setValue(0.02);
        maturityRefQuote->setValue(5.0);
        PROXY_PRICING;

        // move to the last period

        Settings::instance().evaluationDate() = Date(11, June, 2024);
        rateLevelRefQuote->setValue(0.02);
        maturityRefQuote->setValue(1.5);
        PROXY_PRICING;

        // check exercise

        // Settings::instance().evaluationDate() = Date(10, January, 2020);
        // rateLevelRefQuote->setValue(0.005);
        // maturityRefQuote->setValue(6.0);
        // npvProxy = swaption2->NPV();
        // underlyingProxy = swaption2->result<Real>("exerciseValue");
        // std::clog << "\nExercise check (" << Settings::instance().evaluationDate() << "):\n";
        // std::clog << "otm option: exercise value=" << underlyingProxy << " npv=" << npvProxy << std::endl;

        // rateLevelRefQuote->setValue(0.04);
        // npvProxy = swaption2->NPV();
        // underlyingProxy = swaption2->result<Real>("exerciseValue");
        // std::clog << "itm option: exercise value=" << underlyingProxy << " npv=" << npvProxy << std::endl;

        return 0;

    } catch (QuantLib::Error e) {
        std::clog << "terminated with a ql exception: " << e.what()
                  << std::endl;
        return 1;
    } catch (std::exception e) {
        std::clog << "terminated with a general exception: " << e.what()
                  << std::endl;
        return 1;
    }
}