/** \file
* ppp.c : precise point positioning
*
* references :
*    [1] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996
*    [2] D.D.McCarthy and G.Petit, IERS Technical Note 32, IERS Conventions
*        2003, November 2003
*    [3] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed,
*        Space Technology Library, 2004
*    [4] J.Kouba, A Guide to using International GNSS Service (IGS) products,
*        May 2009
*    [5] RTCM Paper, April 12, 2010, Proposed SSR Messages for SV Orbit Clock,
*        Code Biases, URA
*    [6] MacMillan et al., Atmospheric gradients and the VLBI terrestrial and
*        celestial reference frames, Geophys. Res. Let., 1997
*    [7] G.Petit and B.Luzum (eds), IERS Technical Note No. 36, IERS
*         Conventions (2010), 2010
*    [8] J.Kouba, A simplified yaw-attitude model for eclipsing GPS satellites,
*        GPS Solutions, 13:1-12, 2009
*    [9] F.Dilssner, GPS IIF-1 satellite antenna phase center and attitude
*        modeling, InsideGNSS, September, 2010
*    [10] F.Dilssner, The GLONASS-M satellite yaw-attitude model, Advances in
*        Space Research, 2010
*    [11] IGS MGEX (http://igs.org/mgex)
*/

#include <boost/log/trivial.hpp>

#include <vector>

using std::vector;

#include "observations.hpp"
#include "streamTrace.hpp"
#include "linearCombo.hpp"
#include "corrections.hpp"
#include "navigation.hpp"
#include "testUtils.hpp"
#include "acsConfig.hpp"
#include "biasSINEX.hpp"
#include "constants.hpp"
#include "satStat.hpp"
#include "preceph.hpp"
#include "station.hpp"
#include "algebra.hpp"
#include "antenna.hpp"
#include "common.hpp"
#include "wancorr.h"
#include "mongo.hpp"
#include "tides.hpp"
#include "enums.h"
#include "ppp.hpp"
#include "vmf3.h"
#include "trop.h"

#include "eigenIncluder.hpp"

#define VAR_IONO    	SQR(60.0)       // init variance iono-delay
#define VAR_IONEX   	SQR(0.0)
#define ERR_BRDCI   	0.5             // broadcast iono model error factor

/** exclude meas of eclipsing satellite (block IIA)
*/
void testeclipse(
	ObsList&	obsList)
{
	double erpv[5] = {0};

	/* unit vector of sun direction (ecef) */
	Vector3d rsun;
	sunmoonpos(gpst2utc(obsList.front().time), erpv, &rsun);
	Vector3d esun = rsun.normalized();

	for (auto& obs : obsList)
	{
		if (obs.exclude)
		{
			continue;
		}

		double r = obs.rSat.norm();
		if (r <= 0)
			continue;

		/* only block IIA */
// 		if (obs.satNav_ptr->pcv.type == "BLOCK IIA")		//todo take from the satSys object
// 			continue;

		/* sun-earth-satellite angle */
		double cosa = obs.rSat.dot(esun) / r;

		if (cosa < -1)		cosa = -1;
		if (cosa > +1)		cosa = +1;

		double ang = acos(cosa);

		/* test eclipse */
		if	( ang < PI / 2
			|| r * sin(ang) > RE_WGS84)
			continue;

// 		trace(3, "eclipsing sat excluded %s sat=%s\n", obs.time.to_string(0).c_str(), obs.Sat.id().c_str());

		obs.excludeEclipse = true;
	}
}

/** Calculate nominal yaw-angle
*/
double yaw_nominal(
	double	beta,
	double	mu)
{
	if	( fabs(beta)	< 1E-12
		&&fabs(mu)		< 1E-12)
		return PI;

	return atan2(-tan(beta), sin(mu)) + PI;
}

/** Satellite attitude model
*/
int sat_yaw(
	GTime		time,		///< Time of calculated yaw
	Vector3d&	rSat,		///< Satellite position (ECEF)
	Vector3d&	vSat,		///< Satellite velocity (ECEF)
	Vector3d&	exs,		///< Output unit vector XS
	Vector3d&	eys)		///< Output unit vector YS
{
	Vector3d	rSun;
	double		erpv[5] = {};
	sunmoonpos(gpst2utc(time), erpv, &rSun);

	Vector3d vSatPrime = vSat;

	/* beta and orbit angle */
	vSatPrime[0] -= OMGE * rSat[1];
	vSatPrime[1] += OMGE * rSat[0];

	Vector3d n = rSat.	cross(vSatPrime);
	Vector3d p = rSun.	cross(n);

	Vector3d es		= rSat.	normalized();
	Vector3d eSun	= rSun.	normalized();
	Vector3d en		= n.	normalized();
	Vector3d ep		= p.	normalized();

	double beta	= PI / 2 - acos(eSun.dot(en));
	double E	= acos(es.dot(ep));
	double mu	= PI / 2 + (es.dot(eSun) <= 0 ? -E : E);
	if      (mu < -PI / 2)		mu += 2 * PI;
	else if (mu >= PI / 2)		mu -= 2 * PI;

	/* yaw-angle of satellite */
	double yaw = yaw_nominal(beta, mu);

	/* satellite fixed x,y-vector */
	Vector3d ex = en.cross(es);

	double cosy = cos(yaw);
	double siny = sin(yaw);

	eys = -cosy * en - siny * ex;
	exs = -siny * en + cosy * ex;

	return 1;
}

/** phase windup model
*/
int model_phw(
	GTime		time,	///< Time
	Obs&		obs,	///< Observation detailing the satellite to apply model to
	Vector3d&	rRec,	///< Position of receiver (ECEF)
	double&		phw)	///< Output of phase windup result
{
	/* satellite yaw attitude model */
	Vector3d exs;
	Vector3d eys;
	if (!sat_yaw(time, obs.rSat, obs.satVel, exs, eys))
		return 0;

	/* non block IIR satellites, to be refined for other constellations */
	/* if ((!strstr(type,"BLOCK IIR"))&&(sys!=SYS_GAL)) { */

	/* all the satellite orientation follow block II-A, 21/03/2019*/
	if (1)
	{
		exs *= -1;
		eys *= -1;
	}

	/* unit vector satellite to receiver */
	Vector3d r = rRec - obs.rSat;
	Vector3d ek = r.normalized();

	/* unit vectors of receiver antenna */
	double E[9];
	double pos[3];
	ecef2pos(rRec.data(), pos);
	xyz2enu(pos, E);
	Vector3d exr;
	Vector3d eyr;
	exr(0) =  E[1];
	exr(1) =  E[4];
	exr(2) =  E[7]; /* x = north */

	eyr(0) = -E[0];
	eyr(1) = -E[3];
	eyr(2) = -E[6]; /* y = west  */

	/* phase windup effect */
	Vector3d eks = ek.cross(eys);
	Vector3d ekr = ek.cross(eyr);

	Vector3d ds = exs - ek * ek.dot(exs) - eks;
	Vector3d dr = exr - ek * ek.dot(exr) + ekr;
	double cosp = ds.dot(dr) / ds.norm() / dr.norm();
	if      (cosp < -1) cosp = -1;
	else if (cosp > +1) cosp = +1;
	double ph = acos(cosp) / 2 / PI;
	Vector3d drs = ds.cross(dr);

	if (ek.dot(drs) < 0)
		ph *= -1;

	phw = ph + floor(phw - ph + 0.5); /* in cycle */

	return 1;
}

/** get meterological parameters
*/
void getmet(
	double	lat,
	double*	met)
{
	const double metprm[][10] = /* lat=15,30,45,60,75 */
	{
		{1013.25, 299.65, 26.31, 6.30E-3, 2.77,  0.00, 0.00, 0.00, 0.00E-3, 0.00},
		{1017.25, 294.15, 21.79, 6.05E-3, 3.15, -3.75, 7.00, 8.85, 0.25E-3, 0.33},
		{1015.75, 283.15, 11.66, 5.58E-3, 2.57, -2.25, 11.00, 7.24, 0.32E-3, 0.46},
		{1011.75, 272.15, 6.78, 5.39E-3, 1.81, -1.75, 15.00, 5.36, 0.81E-3, 0.74},
		{1013.00, 263.65, 4.11, 4.53E-3, 1.55, -0.50, 14.50, 3.39, 0.62E-3, 0.30}
	};

	lat = fabs(lat);

	if      (lat <= 15) for (int i = 0; i < 10; i++) met[i] = metprm[0][i];
	else if (lat >= 75) for (int i = 0; i < 10; i++) met[i] = metprm[4][i];
	else
	{
		int j = (int)(lat / 15);
		double a = (lat - j * 15) / 15.0;

		for (int i = 0; i < 10; i++)
		{
			met[i] = (1 - a) * metprm[j - 1][i] + a * metprm[j][i];
		}
	}
}

/* tropospheric delay correction -----------------------------------------------
* compute sbas tropospheric delay correction (mops model)
* args   : gtime_t time     I   time
*          double   *pos    I   receiver position {lat,lon,height} (rad/m)
*          double   *azel   I   satellite azimuth/elavation (rad)
*          double   *var    O   variance of troposphric error (m^2)
* return : slant tropospheric delay (m)
*-----------------------------------------------------------------------------*/
double sbstropcorr(
	GTime			time,			///< Time
	Vector3d&		rRec,			///< Receiver position (ECEF)
	double			el,				///< Satellite elevation
	double*			var)			///< Optional variance output
{
	double pos[3];
	ecef2pos(rRec.data(), pos);
	const double k1	= 77.604;
	const double k2	= 382000;
	const double rd	= 287.054;
	const double gm	= 9.784;
	const double g	= 9.80665;

// 	trace(4, "sbstropcorr: pos=%.3f %.3f azel=%.3f\n",
// 			pos[0]*R2D,
// 			pos[1]*R2D,
// 			el*R2D);

	if	( pos[2]	< -100
		||pos[2]	> +10000
		||el		<= 0)
	{
		if (var)
			*var = 0;

		return 0;
	}

	double met[10];
	getmet(pos[0] * R2D, met);

	double c = cos(2 * PI * (time2doy(time) - (pos[0] >= 0 ? 28 : 211)) / 365.25);
	for (int i = 0; i < 5; i++)
	{
		met[i] -= met[i + 5] * c;
	}
	double zh = 1E-6 * k1 * rd * met[0] / gm;
	double zw = 1E-6 * k2 * rd / (gm * (met[4] + 1.0) - met[3] * rd) * met[2] / met[1];

	double h = pos[2];
	zh *= pow(1 - met[3] * h / met[1], g / (rd * met[3]));
	zw *= pow(1 - met[3] * h / met[1], (met[4] + 1) * g / (rd * met[3]) - 1);

	double sinel = sin(el);
	double m = 1.001 / sqrt(0.002001 + sinel * sinel);
	if (var)
		*var = SQR(0.12 * m);
	return (zh + zw) * m;
}

/** Antenna corrected measurement
*/
void corr_meas(
	Trace&		trace,		///< Trace file to output to
	Obs&		obs,		///< Observation to correct measurements of
	E_FType		ft,			///< Frequency type to correct
	double		el,			///< Satellite elevation
	double		dAntRec,	///< Delta for antenna offset of receiver
	double		dAntSat,	///< Delta for antenna offset of satellite
	double		phw,		///< Phase wind up
	ClockJump&	cj,			///< Clock jump
	Station&	rec)		///< Receiver
{
	TestStack ts(__FUNCTION__);

	Sig& sig = obs.Sigs[ft];

	double lam = obs.satNav_ptr->lamMap[ft];

	if	(  lam		== 0
		|| sig.L	== 0
		|| sig.P	== 0)
	{
		return;
	}

	double bias[2] = {};
	double bvar[2] = {};

	bias_io_opt biaopt;
	biaopt.OSB_biases = acsConfig.ambrOpts.readOSB;
	biaopt.DSB_biases = acsConfig.ambrOpts.readDSB;
	biaopt.SSR_biases = acsConfig.ambrOpts.readSSRbias;
	biaopt.SAT_biases = acsConfig.ambrOpts.readSATbias;
	biaopt.REC_biases = acsConfig.ambrOpts.readRecBias;
	biaopt.HYB_biases = acsConfig.ambrOpts.readHYBbias;
	biaopt.COD_biases = true;
	biaopt.PHS_biases = true;

	inpt_hard_bias(trace, obs, sig.code, bias, bvar, biaopt);

	if (acsConfig.ssrOpts.calculate_ssr)
	{
		double dummyVal = 0; // Set dummy code biases L1C & L2W to zero (not currently used)
		double dummyVar = 0; // Set dummy code biases L1C & L2W to zero (not currently used)
		if	( (ft == F1 && sig.code == +E_ObsCode::L1C)
			||(ft == F2 && sig.code == +E_ObsCode::L2W))
		{
			obs.satNav_ptr->ssrOut.ssrCodeBias.canExport					= false;
			obs.satNav_ptr->ssrOut.ssrCodeBias.codeBias_map[sig.code].bias	= dummyVal;
			obs.satNav_ptr->ssrOut.ssrCodeBias.codeBias_map[sig.code].var	= dummyVar;
			obs.satNav_ptr->ssrOut.ssrCodeBias.isSet						= true;
		}
	}

#if 0 /* this should not be done. Once all bias messages follow the SINEX format properly, could be activated temporaly in case Galileo L5 biases are absent */
	if(bias[1]==0.0){
		if(obs.Sat.sys == +E_Sys::GPS) {
			if(ft == F1 && sig.code != +E_ObsCode::L1C) inpt_hard_bias(trace,obs,  1, bs, bsv, station, biaopt);
			if(ft == F1 && sig.code != +E_ObsCode::L2W) inpt_hard_bias(trace,obs, 20, bs, bsv, station, biaopt);
		}

		if( obs.Sat.sys == +E_Sys::GAL ){
			if(ft == F1 && sig.code != +E_ObsCode::L1X) inpt_hard_bias(trace,obs, 12, bs, bsv, station, biaopt);
			if(ft == F5 && sig.code != +E_ObsCode::L5X) inpt_hard_bias(trace,obs, 26, bs, bsv, station, biaopt);
			if(ft == F5 && sig.code != +E_ObsCode::L5Q) inpt_hard_bias(trace,obs, 25, bs, bsv, station, biaopt);
			if(ft == F5 && bs[1]    ==        0.0     )	inpt_hard_bias(trace,obs, 24, bs, bsv, station, biaopt);
		}
		bias[1]=bs[1];
		bvar[1]=bsv[1];
	}
#endif

	tracepdeex(3, trace, "\n %s  Biases for code %3d:   %9.4f %9.4f, vari: %10.4e %10.4e", obs.Sat.id().c_str(), sig.code, bias[0], bias[1], bvar[0], bvar[1]);

	sig.P_corr_m = sig.P       	- dAntSat - dAntRec - bias[0];
	sig.L_corr_m = sig.L * lam	- dAntSat - dAntRec - bias[1] - phw * lam;

#if 0
	double jump = cj.msJump * CLIGHT * 1e-3;
	sig.P_corr_m+=jump;
	sig.L_corr_m+=jump;
#endif

}

/* satellite antenna phase center variation ----------------------------------*/
void satantpcv(
	Vector3d&			rs,
	Vector3d&			rr,
	PhaseCenterData&			pcv,
	map<int, double>&	dAntSat,
	double*				nad)
{
	Vector3d ru = rr - rs;
	Vector3d rz = -rs;
	Vector3d eu = ru.normalized();
	Vector3d ez = rz.normalized();

	double cosa = eu.dot(ez);
	if (cosa < -1)	cosa = -1;
	if (cosa > +1)	cosa = +1;

	double nadir = acos(cosa);
	if (nad)
		*nad = nadir * R2D;

	interp_satantmodel(pcv, nadir, dAntSat);
	//antmodel_s(pcv,nadir,dAntSat);
}

/* precise tropospheric model ------------------------------------------------*/
double trop_model_prec(
	GTime		time,
	double*		pos,
	double*		azel,
	double*		tropStates,
	double*		dTropDx,
	double&		var)
{
	double map[2] = {};

	/* zenith hydrostatic delay */
	double zhd = tropacs(pos, azel, map);

	double zwd = tropStates[0] - zhd;

	if	( acsConfig.process_user
		||acsConfig.process_ppp)
	{
		/* mapping function */
		double m_w;
		double m_h = tropmapf(time, pos, azel, &m_w);

		double& az = azel[0];
		double& el = azel[1];

		double m_az = 0;

		if (el > 0)
		if (el < 0.9999 * PI/2)
		{
			double c = 0.0031;
			m_az = 1 / (sin(el) * tan(el) + c);
		}

		double grad_n = m_az * cos(az);
		double grad_e = m_az * sin(az);

		var			= SQR(0.01);		//todo aaron, move this somewhere else, should use trop state variance?

		double value	= m_h		* zhd
						+ m_w		* zwd
						+ grad_n	* tropStates[1]
						+ grad_e	* tropStates[2];

		dTropDx[0] = m_w;
		dTropDx[1] = grad_n;
		dTropDx[2] = grad_e;

		return value;
	}
	else
	{
		/* wet mapping function */
		dTropDx[0]	= map[1];
		var			= SQR(0.01);

		return map[0] * zhd;
	}
}

/* ionospheric model ---------------------------------------------------------*/
int model_iono(
	GTime		time,
	double*		pos,
	double*		azel,
	double		ionoState,
	double&		dion,
	double&		var)
{
	switch (acsConfig.ionoOpts.corr_mode)
	{
		case E_IonoMode::TOTAL_ELECTRON_CONTENT:
		{
			int res = iontec(time, &nav, pos, azel, 1, dion, var);
			if (res)	var +=	VAR_IONEX;			// adding some extra errors to reflect modelling errors
			else		var =	VAR_IONO;

			return res;
		}
		case E_IonoMode::BROADCAST:
		{
			dion	= ionmodel(time, nav.ion_gps, pos, azel);
			var		= SQR(dion * ERR_BRDCI);

			return 1;
		}
		case E_IonoMode::ESTIMATE:
		{
			dion	= ionoState;
			var		= 0;

			return 1;
		}
		case E_IonoMode::IONO_FREE_LINEAR_COMBO:
		{
			dion	= 0;
			var		= 0;

			return 1;
		}
		case E_IonoMode::OFF:
		{
			return 0;
		}
	}

	return 0;
}

void pppCorrections(
	Trace&		trace,
	ObsList&	obsList,
	Vector3d&	rRec,
	rtk_t&		rtk,
	Station&	rec)
{
	TestStack ts(__FUNCTION__);

	int lv = 3;

	double ep[6];
	time2epoch(obsList.front().time, ep);
	double jd	= ymdhms2jd(ep);
	double mjd	= jd - JD2MJD;

	double pos[3];
	ecef2pos(rRec.data(), pos);

	tracepde(3,trace, "pppCorrections  : n=%d\n", obsList.size());

	for (auto& obs : obsList)
	{
		if (obs.exclude)
		{
			continue;
		}

		TestStack ts(obs.Sat);
		SatStat&	satStat	= *(obs.satStat_ptr);
		auto&		lam		= obs.satNav_ptr->lamMap;

		double r = geodist(obs.rSat, rRec, satStat.e);						TestStack::testMat("r",		r);
		if 	( r <= 0
			||satStat.el < acsConfig.elevation_mask)
		{
			obs.excludeElevation = true;
			continue;
		}

		if	(satexclude(obs.Sat, obs.svh))
		{
			obs.exclude = true;
			continue;
		}

// 		if (acsConfig.antexacs == 0)
// 		{
// 			std::cout << " Using antmodel() " << std::endl;			//todo aaron, broke this.
// 			antmodel(opt->pcvr, opt->antdel, obs.azel, opt->posopt[1], dAntRec);
// 		}

		//satellite and receiver antenna model
		map<int, double> dAntSat;
		if	(acsConfig.sat_pcv)
		{
			PhaseCenterData* pcvsat = findAntenna(obs.Sat.id(), obs.time, nav);
			if (pcvsat)
			{
				satantpcv(obs.rSat, rRec, *pcvsat, dAntSat);
			}
			else
			{
				tracepde(1, trace,	"Warning: no satellite (%s) pcv information\n",	obs.Sat.id().c_str());
				continue;
			}
		}

		// phase windup model
		if (acsConfig.phase_windup)
		{
			bool pass = model_phw(rtk.sol.time, obs, rRec, satStat.phw);
			if (pass == false)
			{
				continue;
			}
		}

		map<int, double> dAntRec;

		for (auto& [ft, sig] : obs.Sigs)
		{
			TestStack ts("F" + std::to_string(ft));

			sig.Range = r;

			double rpcv = 0;

			/* receiver pco correction to the coordinates */
			if (rtk.pcvrec)
			{
				Vector3d pco_r;
				Vector3d dr2;

				recpco(rtk.pcvrec, ft, pco_r);
				enu2ecef(pos, pco_r.data(), dr2.data());    /* convert enu to xyz */

				/* get rec position and geometric distance for each frequency */
				Vector3d rRecFreq = rRec + dr2;

				sig.Range = geodist(obs.rSat, rRecFreq, satStat.e);

				/* calculate pcv */
				double azDeg = satStat.az * R2D;
				double elDeg = satStat.el * R2D;
				recpcv(rtk.pcvrec, ft, elDeg, azDeg, rpcv);
				dAntRec[ft] = rpcv;

												TestStack::testMat("obs.rSat",	obs.rSat);
												TestStack::testMat("rRecFreq",	rRecFreq);
												TestStack::testMat("rRec",	    rRec);
												TestStack::testMat("dr2",	     dr2);
												TestStack::testMat("sig.Range",	sig.Range);
			}
			// corrected phase and code measurements
			ClockJump cj = {};
			corr_meas(trace, obs, ft, satStat.el, dAntRec[ft], dAntSat[ft], satStat.phw, cj, rec);

			tracepde(lv, trace, "*---------------------------------------------------*\n");
			tracepde(lv, trace, " %.6f %sL%d satpcv              = %14.4f\n",                       mjd, obs.Sat.id().c_str(), ft, dAntSat[ft]);
			tracepde(lv, trace, " %.6f %sL%d recpcv              = %14.4f\n",                       mjd, obs.Sat.id().c_str(), ft, dAntRec[ft]);
			tracepde(lv, trace, " %.6f %sL%d az, el              = %14.4f %14.4f\n",                mjd, obs.Sat.id().c_str(), ft, satStat.az*R2D, satStat.el*R2D);
			tracepde(lv, trace, " %.6f %sL%d phw(cycle)          = %14.4f \n",                      mjd, obs.Sat.id().c_str(), ft, satStat.phw);
			tracepde(lv, trace, " %.6f %sL%d satpos+pco          = %14.4f %14.4f %14.4f\n",         mjd, obs.Sat.id().c_str(), ft, obs.rSat[0], obs.rSat[1], obs.rSat[2]);
			tracepde(lv, trace, " %.6f %sL%d dist                = %14.4f\n",                       mjd, obs.Sat.id().c_str(), ft, r);

								TestStack::testMat("dAntRec",	dAntRec[ft]);
								TestStack::testMat("dAntSat",	dAntSat[ft]);
		}

		if (acsConfig.ionoOpts.corr_mode == +E_IonoMode::IONO_FREE_LINEAR_COMBO)
		for (E_FType ft : {F2, F5})
		{
			/* iono-free LC */
			Sig sig1 = obs.Sigs[F1];
			Sig sig2 = obs.Sigs[ft];

			if	( lam[F1] == 0
				||lam[ft] == 0)
				continue;

			if	( (sig1.L_corr_m == 0)
				||(sig1.P_corr_m == 0)
				||(sig2.L_corr_m == 0)
				||(sig2.P_corr_m == 0))
			{
				continue;
			}

			double c1;
			double c2;
			S_LC lc = getLC(sig1.L_corr_m,	sig2.L_corr_m,
							sig1.P_corr_m,	sig2.P_corr_m,
							lam[F1],		lam[ft],
							&c1, 			&c2);

			if (lc.valid == false)
			{
				continue;
			}

			Sig lcSig = {};
			lcSig.L_corr_m = lc.IF_Phas_m;
			lcSig.P_corr_m = lc.IF_Code_m;

			E_FType newType;
			switch (ft)
			{
				case F2: newType = FTYPE_IF12;	break;
				case F5: newType = FTYPE_IF15;	break;
				default: continue;
			}

			//update distance measurement for iflc
			lcSig.Range	= sig1.Range * c1
						- sig2.Range * c2;

			double AA = POW4(CLIGHT/lam[F1]) / POW2(POW2(CLIGHT/lam[F1]) - POW2(CLIGHT/lam[ft]));
			double BB = POW4(CLIGHT/lam[ft]) / POW2(POW2(CLIGHT/lam[F1]) - POW2(CLIGHT/lam[ft]));

			double A = POW4(lam[F1]) / POW2(POW2(lam[F1]) - POW2(lam[ft]));
			double B = POW4(lam[ft]) / POW2(POW2(lam[F1]) - POW2(lam[ft]));
// 			printf("\n%f %f\n", A, B);
// 			printf("\n%f %f\n", AA, BB);
			lcSig.codeVar	= POW4(lam[F1]) * sig1.codeVar / POW2(POW2(lam[F1]) - POW2(lam[ft]))
							+ POW4(lam[ft]) * sig2.codeVar / POW2(POW2(lam[F1]) - POW2(lam[ft]));

			lcSig.phasVar 	= POW4(lam[F1]) * sig1.phasVar / POW2(POW2(lam[F1]) - POW2(lam[ft]))
							+ POW4(lam[ft]) * sig2.phasVar / POW2(POW2(lam[F1]) - POW2(lam[ft]));

			obs.Sigs[newType] = lcSig;

			obs.satStat_ptr->sigStatMap[newType].slip.any	= obs.satStat_ptr->sigStatMap[F1].slip.any
															| obs.satStat_ptr->sigStatMap[ft].slip.any;
		}
	}
}

void outputApriori(
	StationMap& stationMap)
{
	KFState aprioriState;
	for (auto& [id, rec] : stationMap)
	{
		KFKey kfKey;
		kfKey.str	= id + "_0";
		kfKey.type	= KF::REC_POS;

		for (int i = 0; i < 3; i++)
		{
			kfKey.num = i;

			aprioriState.addKFState(kfKey, {.x = rec.aprioriPos[i]});
		}
	}
	for (auto& [id, rec] : stationMap)
	{
		KFKey kfKey;
		kfKey.str	= id + "_0";
		kfKey.type	= KF::REC_CLOCK;

		double precDtRec	= 0;
		pephclk(tsync, id, nav, precDtRec);

		aprioriState.addKFState(kfKey, {.x = CLIGHT * precDtRec});
	}
	aprioriState.stateTransition(nullStream, tsync);

#ifdef ENABLE_MONGODB
	mongoStates(aprioriState);
#endif
}

/** Compare estimated station position with benchmark in SINEX file
 */
void outputPPPSolution(
	Station& rec)
{
	Vector3d snxPos = rec.snx.pos;
	Vector3d estPos = rec.rtk.sol.pppRRec;
	Vector3d diffEcef = snxPos - estPos;

	double latLonHt[3];
	ecef2pos(snxPos, latLonHt); // rad,rad,m

	double diffEcefArr[3];
	Vector3d::Map(diffEcefArr, diffEcef.rows())	= diffEcef; // equiv. to diffEcef = diff

	double diffEnuArr[3];
	ecef2enu(latLonHt, diffEcefArr, diffEnuArr);

	Vector3d diffEnu;
	diffEnu = Vector3d::Map(diffEnuArr, diffEnu.rows());

	std::ofstream fout(acsConfig.ppp_sol_filename, std::ios::out | std::ios::app);
	if (!fout)
	{
		BOOST_LOG_TRIVIAL(error)
		<< "Could not open trace file for PPP solution at " << acsConfig.ppp_sol_filename;
	}
	else
	{
		fout << epoch << " ";
		fout << rec.id << " ";
		fout << snxPos.transpose() << " ";
		fout << estPos.transpose() << " ";
		fout << diffEcef.transpose() << " ";
		fout << diffEnu.transpose() << " ";
		fout << std::endl;
	}
}

void selectAprioriSource(
	Station&	rec,
	bool&		sppUsed)
{
	sppUsed = false;

	if (rec.aprioriPos(2) != 0)
	{
		//already has apriori
		return;
	}

	if (rec.snx.pos(2) != 0)
	{
		rec.aprioriPos		= rec.snx.pos;
		rec.primaryApriori	= rec.snx.primary;

		Vector3d delta = rec.snx.pos - rec.rtk.sol.sppRRec;

		double distance = delta.norm();

		if	( distance > 20
			&&rec.rtk.sol.sppRRec.norm() > 0)
		{
			BOOST_LOG_TRIVIAL(error)
			<< "SINEX apriori for " << rec.id << " is " << distance << "m from SPP estimate";
		}

		return;
	}
	else
	{
		rec.aprioriPos		= rec.rtk.sol.sppRRec;
		sppUsed				= true;
	}
}

/** Deweight worst measurement
 */
bool deweightMeas(
	Trace&		trace,
	KFState&	kfState,
	KFMeas&		kfMeas,
	int			index)
{
	trace << std::endl << "Deweighting " << kfMeas.obsKeys[index] << std::endl;

	kfMeas.R.row(index) *= acsConfig.deweight_factor;
	kfMeas.R.col(index) *= acsConfig.deweight_factor;

	return true;
}

/** Count worst measurement
 */
bool incrementPhaseSignalError(
	Trace&		trace,
	KFState&	kfState,
	KFMeas&		kfMeas,
	int			index)
{
	map<string, void*>& metaDataMap = kfMeas.metaDataMaps[index];

	unsigned int* phaseRejectCount_ptr = (unsigned int*) metaDataMap["phaseRejectCount_ptr"];

	if (phaseRejectCount_ptr == nullptr)
	{
		return true;
	}

	unsigned int&	phaseRejectCount	= *phaseRejectCount_ptr;

	//increment counter, and clear the pointer so it cant be reset to zero in subsequent operations (because this is a failure)
	phaseRejectCount++;
	metaDataMap["phaseRejectCount_ptr"] = nullptr;

	return true;
}

bool countSignalErrors(
	Trace&		trace,
	KFState&	kfState,
	KFMeas&		kfMeas,
	int			index)
{
	map<string, void*>& metaDataMap = kfMeas.metaDataMaps[index];

	Obs* obs_ptr = (Obs*) metaDataMap["obs_ptr"];

	if (obs_ptr == nullptr)
	{
		return true;
	}

	ObsKey&		obsKey	= kfMeas.obsKeys[index];
	Obs&		obs		= *obs_ptr;

	if (obsKey.type == "L")
	{
		//this is a phase observation
		obs.Sigs[(E_FType)obsKey.num].phaseError = true;
	}

	return true;
}

bool resetPhaseSignalError(
	KFMeas&		kfMeas,
	int			index)
{
	map<string, void*>& metaDataMap = kfMeas.metaDataMaps[index];

	//this will have been set to null if there was an error after adding the measurement to the list
	unsigned int* phaseRejectCount_ptr = (unsigned int*) metaDataMap["phaseRejectCount_ptr"];

	if (phaseRejectCount_ptr == nullptr)
	{
		return true;
	}

	unsigned int&	phaseRejectCount	= *phaseRejectCount_ptr;

	phaseRejectCount = 0;

	return true;
}

bool resetPhaseSignalOutage(
	KFMeas&		kfMeas,
	int			index)
{
	map<string, void*>& metaDataMap = kfMeas.metaDataMaps[index];

	unsigned int* phaseOutageCount_ptr = (unsigned int*) metaDataMap["phaseOutageCount_ptr"];

	if (phaseOutageCount_ptr == nullptr)
	{
		return true;
	}

	unsigned int&	phaseOutageCount	= *phaseOutageCount_ptr;

	phaseOutageCount = 0;

	return true;
}


/**
 * gcodetimer
 *
 * Copyright © 2016 Juan Jose Gonzalez
 *
 * 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.
 */

#include "GCodeProcessorBase.h"

#include "Utils.h"
#include "Config.h"

#include <cstdio>
#include <cstring>
#include <string>
#include <boost/tokenizer.hpp>

using namespace std;
using namespace boost;

GCodeProcessorBase::GCodeProcessorBase(ifstream *input) : input(input) {}

void GCodeProcessorBase::process_file() {
    static const float max_jerk_magnitude = Utils::get_euclidean_length(Config::get()->max_jerk);
    string line;
    COORDS pos = {0.0, 0.0, 0.0, 0.0};       // mm
    float rate = 0.0;
    while(!(*input).eof()) {
        float line_duration = 0.0;

        getline(*input, line);
        char_separator<char> sep(" ");
        tokenizer< char_separator<char> > tokens(line, sep);
        tokenizer< char_separator<char> >::iterator token_iter = tokens.begin();
        if (token_iter != tokens.end()) {
            if ((*token_iter).compare("G1") == 0) {     // Linear move
                COORDS target_pos;

                memcpy((void*)&target_pos, (void*)&pos, sizeof(COORDS));
                for(++token_iter; token_iter != tokens.end(); ++token_iter) {
                    char op;
                    float value;

                    sscanf((*token_iter).c_str(), "%c%f", &op, &value);
                    switch(op) {
                        case 'X': target_pos.x = value; break;
                        case 'Y': target_pos.y = value; break;
                        case 'Z': target_pos.z = value; break;
                        case 'E': target_pos.e = value; break;
                        case 'F': rate = value / 60; break;
                    }
                }

                COORDS movement = Utils::get_diff(target_pos, pos);
                float length = Utils::get_euclidean_length(movement);
                if (length > 0) {
                    float rate_speed_factor = Config::get()->speed_multiplier * rate / length;
                    COORDS target_speed_components = Utils::map(movement, [=](float c) { return c * rate_speed_factor; });

                    // Calculate the individual jerk components
                    float jerk_speed_factor = max_jerk_magnitude / length;
                    COORDS jerk_speed = Utils::map(movement, [=](float c) { return abs(c) * jerk_speed_factor; });

                    // Check if the components exceed the max jerk per component. If so, reduce all
                    // components by the required factor to comply with the max jerk settings
                    COORDS jerk_reduce_factor = Utils::map(jerk_speed, Config::get()->max_jerk, [](float jc, float mc) { return jc > mc ? mc / jc : 1.0; });
                    float jerk_multiplier = Utils::reduce(jerk_reduce_factor, [](float c, float factor) { return min (factor, c); }, 1.0);
                    jerk_speed = Utils::map(jerk_speed, [=](float c) { return c * jerk_multiplier * Config::get()->jerk_efficiency; });

                    // Calculate the magnitude of the final jerk vector
                    float jerk_magnitude = Utils::get_euclidean_length(jerk_speed);

                    // Calculate the speed delta for the acceleration and deceleration phase
                    COORDS speed_delta_components = Utils::map(target_speed_components, jerk_speed, [](float sc, float jc) { return Utils::pos(abs(sc) - jc); });

                    // Calculate the time required to complete the acceleration
                    const COORDS &max_accel = movement.e != 0.0 ? Config::get()->max_print_accel : Config::get()->max_move_accel;
                    COORDS accel_time_components = Utils::map(speed_delta_components, max_accel, [] (float sc, float ac) { return sc / ac; });
                    float accel_time = Utils::reduce(accel_time_components, [] (float c, float t) { return max(c, t); }, accel_time_components.x);

                    float accel_magnitude = 0.0;
                    if (accel_time > EPSILON) {
                        // Calculate the actual acceleration per component based on accel_time and speed_delta_components
                        COORDS accel = Utils::map(speed_delta_components, [=] (float c) { return c / accel_time; });

                        // Calculate the magnitude of the acceleration vector
                        accel_magnitude = Utils::get_euclidean_length(accel) * Config::get()->accel_efficiency;
                    } else {
                        accel_time = 0.0;
                    }

                    float speed_magnitude = Utils::get_euclidean_length(target_speed_components);

                    // Full acceleration (a*t^2 / 2) and deceleration (a*t^2 / 2) possible
                    if (length > (2 * jerk_magnitude + accel_magnitude * accel_time) * accel_time) {
                        line_duration = accel_time * 2 + (length - (2 * jerk_magnitude + accel_magnitude * accel_time) * accel_time) / speed_magnitude;

                    } else {
                        // l = 2 * (((t / 2) * a / 2 + js) * (t / 2)) = ((t / 4) * a + js) * t = t^2 * a / 4 + t * js
                        // t^2 * a / 4 + t * js - l = 0 => t = (-js + sqrt(js^2 + a*l)) / 2*(a / 4)
                        line_duration = (sqrt(jerk_magnitude * jerk_magnitude + accel_magnitude * length) - jerk_magnitude) / (accel_magnitude / 2);
                    }

                    memcpy((void*)&pos, (void*)&target_pos, sizeof(COORDS));
                }
            } else if ((*token_iter).compare("G28") == 0) {     // Home
                // We don't know how long this will take. Just set the position to 0 without adding any time
                if (++token_iter == tokens.end()) {
                    pos.x = 0;
                    pos.y = 0;
                    pos.z = 0;
                }
                for(token_iter; token_iter != tokens.end(); ++token_iter) {
                    char op;
                    float value;
                    sscanf((*token_iter).c_str(), "%c%f", &op, &value);
                    switch(op) {
                        case 'X': pos.x = value; break;
                        case 'Y': pos.y = value; break;
                        case 'Z': pos.z = value; break;
                    }
                }
            } else if ((*token_iter).compare("G92") == 0) {     // Reset coords
                if (++token_iter == tokens.end()) {
                    memset((void*)&pos, 0, sizeof(pos));
                }
                for(token_iter; token_iter != tokens.end(); ++token_iter) {
                    char op;
                    float value;
                    sscanf((*token_iter).c_str(), "%c%f", &op, &value);
                    switch(op) {
                        case 'X': pos.x = value; break;
                        case 'Y': pos.y = value; break;
                        case 'Z': pos.z = value; break;
                        case 'E': pos.e = value; break;
                    }
                }
            }
        }
        process_line(line, line_duration);
    }
}


/*======================================================================
Copyright 2019 Google LLC

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

    https://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 <iostream>
#include <cmath>
#include <assert.h>

#include <Eigen/Dense>
#include "PCV_Types.h"

#include "Traj3.h"

using namespace std;
using namespace Eigen;

#define DECEL_WINDOW  0.003
#define POS_WINDOW    0.0002

Traj3::Traj3(double freq)
{
  assert( freq > 0.1 );
  period_=1.0/freq;
  curPos_  = new Vector3d;
  curVel_  = new Vector3d;
  destPos_ = new Vector3d;
  destVel_ = new Vector3d;
  acc_     = new Vector3d;
  maxVel_  = new Vector3d;
};

Traj3::~Traj3()
{
  delete curPos_;
  delete curVel_;
  delete destPos_;
  delete destVel_;
  delete acc_;
  delete maxVel_;
};

void
Traj3::curPos(Vector3d &_curPos)
{
  *curPos_ = _curPos;
}

void
Traj3::curVel(Vector3d &_curVel)
{
  *curVel_ = _curVel;
}

void
Traj3::maxVel(double v_lim, double w_lim)
{
 v_lim_ = fabs(v_lim);
 w_lim_ = fabs(w_lim);

 (*maxVel_)[0] = v_lim_;
 (*maxVel_)[1] = v_lim_;
 (*maxVel_)[2] = w_lim_;
}

void
Traj3::maxVel(Vector3d &_maxVel)
{
  for(int i ; i<_maxVel.size() ; ++i)
  { (*maxVel_)[i] = fabs(_maxVel[i]);
  }
}

void
Traj3::accel(double lin_acc, double rot_acc)
{
  lin_acc_ = lin_acc;
  rot_acc_ = rot_acc;

  (*acc_)[0] = fabs(lin_acc_);
  (*acc_)[1] = fabs(lin_acc_);
  (*acc_)[2] = fabs(rot_acc_);
}

void
Traj3::accel(Vector3d &acc)
{
  for(int i ; i<acc.size() ; ++i)
  { (*acc_)[i] = fabs(acc[i]);
  }
}

void
Traj3::dest2(Vector3d &destPos)
{
  Vector3d errPos;
  double   errLi;

  *destPos_=destPos;

  errPos = *destPos_ - *curPos_;
  errLi  = hypot(errPos[0],errPos[1]);
  if(1.0/errLi < POS_WINDOW )
    errLi = 1;        // SHOULD BE 1/POS_WINDOW ???
  errLi = 1.0/errLi;  // AT 1, IT GIVES A DEADBAND

  (*acc_)[0] = fabs(lin_acc_ * errPos[0]*errLi);
  (*acc_)[1] = fabs(lin_acc_ * errPos[1]*errLi);

  (*acc_)[2] = rot_acc_;

   (*maxVel_)[0] = fabs(v_lim_ * errPos[0]*errLi);
   (*maxVel_)[1] = fabs(v_lim_ * errPos[1]*errLi);

   (*maxVel_)[2] = w_lim_;
}

void
Traj3::get_uV(Vector3d &destUvel, Vector3d &trajPos,
              Vector3d &trajVel,  Vector3d &trajAcc)
{
  Vector3d errVel;
  double   velL;          // LENGTH

  // LIMIT LIN COMMAND SPEEDS TO UNIT MAG LIN
  velL = hypot(destUvel[0],destUvel[1]);  // 'L' IS ALWAYS POSITIVE
  if( velL > 1.0 )  // WON'T DIVIDE BY zero BECAUSE THIS CHECK
  { destUvel[0] = destUvel[0]/velL;
    destUvel[1] = destUvel[1]/velL;
  }
  // LIMIT ROT COMMAND SPEED TO UNIT MAG ROT
  if( destUvel[2] >  1.0 ) destUvel[2] =  1.0;
  if( destUvel[2] < -1.0 ) destUvel[2] = -1.0;

  // SCALE UNIT INPUT TO V_LIMIT AND W_LIMIT
  errVel[0] = destUvel[0]*v_lim_ - (*curVel_)[0];
  errVel[1] = destUvel[1]*v_lim_ - (*curVel_)[1];
  errVel[2] = destUvel[2]*w_lim_ - (*curVel_)[2];

  double   accel;         // ACCELERATION TMP VAR
  double   errL,errLi;    // LENGTHS
  double   v_win =  lin_acc_ * period_; // MAX delta_V MAG.
  double   w_win =  rot_acc_ * period_; // MAX delta_W MAG.

  //LIN ACCEL AT MAX UNLESS ONE STEP TO ZERO ERROR
  errL  = hypot(errVel[0],errVel[1]); // 'L' IS ALWAYS POSITIVE
  if( errL < v_win*0.002 )
    errL = v_win*0.002;
  if( errL < v_win )
    accel = lin_acc_ * errL/v_win;
  else
    accel = lin_acc_;
  errLi = 1.0/errL;
  trajAcc[0] = accel * errVel[0]*errLi;  // x component
  trajAcc[1] = accel * errVel[1]*errLi;  // y component

  //ROT ACCEL AT MAX UNLESS ONE STEP TO ZERO ERROR
  if     ( errVel[2] >  w_win ) trajAcc[2] =  rot_acc_;
  else if( errVel[2] < -w_win ) trajAcc[2] = -rot_acc_;
  else    trajAcc[2] =  rot_acc_ * errVel[2]/w_win;

  // COMPUTE NEW VELOCITIES
  trajVel = *curVel_ + trajAcc*period_;

  // COMPUTE NEW POSITIONS
  trajPos = *curPos_ + trajVel*period_ +
             trajAcc*(0.5*period_*period_);

  // AGE THE VALUES
  *curVel_ = trajVel;
  *curPos_ = trajPos;
}

void
Traj3::get_V_lin(Vector3d &destVel, Vector3d &trajPos,
                 Vector3d &trajVel, Vector3d &trajAcc)
{
  Vector3d errVel;
  double   velL;             // LENGTH
  double   velR;             // RATIO

  // LIMIT COMMAND SPEEDS DUE TO LIN (SCALE ENTIRE VECTOR)
  velL = hypot(destVel[0],destVel[1]);  // 'L' IS ALWAYS POSITIVE
  velR = velL/v_lim_;
  if( velR > 1.0 )  // WON'T DIVIDE BY zero BECAUSE THIS CHECK
    destVel *= (1.0/velR);
  // LIMIT COMMAND SPEEDS DUE TO ROT (SCALE ENTIRE VECTOR)
  velL = fabs( destVel[2] );
  velR = velL/w_lim_;
  if( velR > 1.0 )  // WON'T DIVIDE BY zero BECAUSE THIS CHECK
     destVel *= (1.0/velR);

  errVel = destVel - *curVel_;

  double   accel;      // ACCELERATION TMP VAR
  double   errL;       // LENGTHS
  double   v_win =  lin_acc_ * period_; // MAX delta_V MAG.

  // LIN ACCEL AT MAX UNLESS ONE STEP TO ZERO ERROR
  // ROT ACCEL IS PROPORTIONAL TO LIN ACCEL...
  // ROT ACCEL IS TO ROT V_ERR, AS LIN ACCEL IS TO LIN V_ERR
  // ASSUMES USE OF INPUT WITH LIMITED ROT SPEED
  errL  = hypot(errVel[0],errVel[1]); // 'L' IS ALWAYS POSITIVE
  if( errL < v_win*0.002 )  // AVOID DIV. BY ZERO
    errL = v_win*0.002;
  if( errL < v_win )        // ONE STEP TO ZERO ERROR
    accel = lin_acc_ * errL/v_win;
  else
    accel = lin_acc_;

  trajAcc = errVel * (accel/errL); // RATIO ALWAYS POSITIVE

 // COMPUTE NEW VELOCITIES
  trajVel = *curVel_ + trajAcc*period_;

  // COMPUTE NEW POSITIONS
  trajPos = *curPos_ + trajVel*period_ + trajAcc*(0.5*period_*period_);

  // AGE THE VALUES
  *curVel_ = trajVel;
  *curPos_ = trajPos;
}

int
Traj3::get(Vector3d &trajPos, Vector3d &trajVel, Vector3d &trajAcc)
{
  int i;
  int goalcount = (*destPos_).size();
  double decelDist, decelError, decel, error;
  Vector3d errPos;

  for (i=0; i < (*destPos_).size() ; ++i)
    {
      error = (*destPos_)[i] - (*curPos_)[i];
      if ( ((*acc_)[i] != 0) && (fabs(error) > POS_WINDOW) )
  {
    /* check to see if we can decelerate to *destPos_ */
    decelDist=(*curVel_)[i]*fabs((*curVel_)[i])/(*acc_)[i]/2.0;
    decelError=decelDist - error;
    if ( ((error>0) && (decelError>0) && (decelError <  DECEL_WINDOW)) ||
         ((error<0) && (decelError<0) && (decelError > -DECEL_WINDOW)) )
      {
        /* calculate deceleration and apply it */
        decel = -(*curVel_)[i] * (*curVel_)[i]/error/2.0;
        (*curVel_)[i] += period_ * decel;
        trajAcc[i] = (decel>0 ? (*acc_)[i] : -(*acc_)[i]);
        if ( fabs(error) < POS_WINDOW) // NOT USED CURRENTLY ???
        { (*curVel_)[i] = 0.0;
          (*curPos_)[i] = (*destPos_)[i];
        }
      }
    else
      {
        /* check to see if we need to accelerate, because we don't
     need to decelerate */
        /* positive acceration: */
        if ( (( decelError < 0.0) && ((*curVel_)[i] < (*maxVel_)[i])) ||
             ((*curVel_)[i] < -(*maxVel_)[i]) )
    {
      (*curVel_)[i] += period_ * (*acc_)[i];
      trajAcc[i] =  (*acc_)[i];
      if ((*curVel_)[i] > (*maxVel_)[i])
      { (*curVel_)[i] = (*maxVel_)[i];
      }
    }
        if ( (( decelError > 0.0) && ((*curVel_)[i] > -(*maxVel_)[i])) ||
             ((*curVel_)[i] > (*maxVel_)[i]) )
    {
      (*curVel_)[i] -= period_ * (*acc_)[i];
      trajAcc[i] = -(*acc_)[i];
      if ((*curVel_)[i] < -(*maxVel_)[i])
      { (*curVel_)[i] = -(*maxVel_)[i];
      }
    }
        /* else we don't need to do anything */
        // ie. stay at +/- maxVel
      }
    (*curPos_)[i] += period_ * (*curVel_)[i];
  }
      else
        {
          (*curVel_)[i] = 0.0;
          trajAcc[i] = 0.0;
//    (*curPos_)[i] = (*destPos_)[i];
          --goalcount;
        }

      trajVel[i] = (*curVel_)[i];
      trajPos[i] = (*curPos_)[i];

    }

  return goalcount;
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef NQS_METROPOLISEXCHANGEPT_HPP
#define NQS_METROPOLISEXCHANGEPT_HPP

#include <mpi.h>
#include <Eigen/Dense>
#include <iostream>
#include "Utils/parallel_utils.hpp"
#include "Utils/random_utils.hpp"
#include "abstract_sampler.hpp"

namespace nqs {

// Metropolis sampling generating local exchanges
// Parallel tempering is also used
class MetropolisExchangePt : public AbstractSampler {
  // number of visible units
  const int nv_;

  const int nrep_;
  std::vector<double> beta_;

  // states of visible units
  // for each sampled temperature
  std::vector<Eigen::VectorXd> v_;

  Eigen::VectorXd accept_;
  Eigen::VectorXd moves_;

  int mynode_;
  int totalnodes_;

  // clusters to do updates
  std::vector<std::vector<int>> clusters_;

  // Look-up tables
  std::vector<typename AbstractMachine::LookupType> lt_;

  int sweep_size_;

 public:
  MetropolisExchangePt(const AbstractGraph &graph, AbstractMachine &psi,
                       int dmax = 1, int nreplicas = 1)
      : AbstractSampler(psi), nv_(GetHilbert().Size()), nrep_(nreplicas) {
    Init(graph, dmax);
  }

  void Init(const AbstractGraph &graph, int dmax) {
    MPI_Comm_size(MPI_COMM_WORLD, &totalnodes_);
    MPI_Comm_rank(MPI_COMM_WORLD, &mynode_);

    v_.resize(nrep_);
    for (int i = 0; i < nrep_; i++) {
      v_[i].resize(nv_);
    }

    for (int i = 0; i < nrep_; i++) {
      beta_.push_back(1. - double(i) / double(nrep_));
    }

    lt_.resize(nrep_);

    accept_.resize(2 * nrep_);
    moves_.resize(2 * nrep_);

    GenerateClusters(graph, dmax);

    Reset(true);

    // Always use odd sweep size to avoid possible ergodicity problems
    if (nv_ % 2 == 0) {
      sweep_size_ = nv_ + 1;
    } else {
      sweep_size_ = nv_;
    }

    InfoMessage() << "Metropolis sampler with parallel tempering is ready "
                  << std::endl;
    InfoMessage() << nrep_ << " replicas are being used" << std::endl;
    InfoMessage() << dmax << " is the maximum distance for exchanges"
                  << std::endl;
  }

  template <class Graph>
  void GenerateClusters(Graph &graph, int dmax) {
    auto dist = graph.AllDistances();

    assert(int(dist.size()) == nv_);

    for (int i = 0; i < nv_; i++) {
      for (int j = 0; j < nv_; j++) {
        if (dist[i][j] <= dmax && i != j) {
          clusters_.push_back({i, j});
        }
      }
    }
  }

  void Reset(bool initrandom = false) override {
    if (initrandom) {
      for (int i = 0; i < nrep_; i++) {
        GetHilbert().RandomVals(v_[i], this->GetRandomEngine());
      }
    }

    for (int i = 0; i < nrep_; i++) {
      GetMachine().InitLookup(v_[i], lt_[i]);
    }

    accept_ = Eigen::VectorXd::Zero(2 * nrep_);
    moves_ = Eigen::VectorXd::Zero(2 * nrep_);
  }

  // Exchange sweep at given temperature
  void LocalExchangeSweep(int rep) {
    std::vector<int> tochange(2);
    std::uniform_real_distribution<double> distu;
    std::uniform_int_distribution<int> distcl(0, clusters_.size() - 1);

    std::vector<double> newconf(2);

    for (int i = 0; i < sweep_size_; i++) {
      int rcl = distcl(this->GetRandomEngine());
      assert(rcl < int(clusters_.size()));
      int si = clusters_[rcl][0];
      int sj = clusters_[rcl][1];

      assert(si < nv_ && sj < nv_);

      if (std::abs(v_[rep](si) - v_[rep](sj)) >
          std::numeric_limits<double>::epsilon()) {
        tochange = clusters_[rcl];
        newconf[0] = v_[rep](sj);
        newconf[1] = v_[rep](si);

        auto explo =
            std::exp(beta_[rep] * GetMachine().LogValDiff(v_[rep], tochange,
                                                          newconf, lt_[rep]));
        double ratio = this->GetMachineFunc()(explo);

        if (ratio > distu(this->GetRandomEngine())) {
          accept_(rep) += 1;
          GetMachine().UpdateLookup(v_[rep], tochange, newconf, lt_[rep]);
          GetHilbert().UpdateConf(v_[rep], tochange, newconf);
        }
      }

      moves_(rep) += 1;
    }
  }

  void Sweep() override {
    // First we do local exchange sweeps
    for (int i = 0; i < nrep_; i++) {
      LocalExchangeSweep(i);
    }

    // Tempearture exchanges
    std::uniform_real_distribution<double> distribution(0, 1);

    for (int r = 1; r < nrep_; r += 2) {
      if (ExchangeProb(r, r - 1) > distribution(this->GetRandomEngine())) {
        Exchange(r, r - 1);
        accept_(nrep_ + r) += 1.;
        accept_(nrep_ + r - 1) += 1;
      }
      moves_(nrep_ + r) += 1.;
      moves_(nrep_ + r - 1) += 1;
    }

    for (int r = 2; r < nrep_; r += 2) {
      if (ExchangeProb(r, r - 1) > distribution(this->GetRandomEngine())) {
        Exchange(r, r - 1);
        accept_(nrep_ + r) += 1.;
        accept_(nrep_ + r - 1) += 1;
      }
      moves_(nrep_ + r) += 1.;
      moves_(nrep_ + r - 1) += 1;
    }
  }

  // computes the probability to exchange two replicas
  double ExchangeProb(int r1, int r2) {
    const double lf1 = 2 * std::real(GetMachine().LogVal(v_[r1], lt_[r1]));
    const double lf2 = 2 * std::real(GetMachine().LogVal(v_[r2], lt_[r2]));

    return std::exp((beta_[r1] - beta_[r2]) * (lf2 - lf1));
  }

  void Exchange(int r1, int r2) {
    std::swap(v_[r1], v_[r2]);
    std::swap(lt_[r1], lt_[r2]);
  }

  const Eigen::VectorXd &Visible() const noexcept override { return v_[0]; }

  void SetVisible(const Eigen::VectorXd &v) override { v_[0] = v; }

  AbstractMachine::VectorType DerLogVisible() override {
    return GetMachine().DerLog(v_[0], lt_[0]);
  }

  Eigen::VectorXd Acceptance() const override {
    Eigen::VectorXd acc = accept_;
    for (int i = 0; i < acc.size(); i++) {
      acc(i) /= moves_(i);
    }
    return acc;
  }
};

}  // namespace nqs

#endif


#pragma once
#include <vector>
#include <queue>
#include <Eigen/Geometry>
#include <boost/optional.hpp>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <OpenMesh/Core/Mesh/PolyConnectivity.hh>
#include "../vector_convert.hh"
#include "../FaceBaryCoordT.hh"
#include "../../eigen_util.hh"
#include "../../util.hh"
#include "../DerivedPtrHolder.hh"

namespace kt84 {

/*
    Implementation of a surface mesh parameterization algorithm descibed in the following articles:
        Ryan Schmidt, Cindy Grimm, and Brian Wyvill.
        Interactive decal compositing with discrete exponential maps.
        ACM SIGGRAPH 2006.

        Ryan Schmidt and Karan Singh.
        Drag, Drop, and Clone: An Interactive Interface for Surface Composition.
        Technical Report CSRG-611, Department of Computer Science, University of Toronto, 2010.

    NOTE: The algorithm is implemented in a slightly different way; the uv coordiante of currently processed vertex q is estimated by rotating
    its local frame to that of its adjacent vertex p that has been processed already, which is in contrast to what is claimed in the original
    SIGGRAPH paper (see sentences below equation (3)). This modification seems to provide better robustness especially when parameterizing highly
    curved surface regions (probably because normals at distant locations are very different from the seed's normal).

    parameters:
        seed(s)                 seed vertex id
        seed(s)_basis_u         3D vector in object space corresponding to unit basis vector of u-coordinate.
                                (its length determines the overall scaling of the parameterization)
        seed(s)_uv              seed uv coordinate
        dist_max                termination criteria based on object-space distance
        uv_min, uv_max          termination criteria based on parameter-space bounding box
*/

struct ExpMap_VertexTraits {
    struct Data {
        bool            paramed;
        double          dist;
        double          weight;
        Eigen::Vector2d uv;
        Eigen::Vector2d basis_u;              // represents u-coordiante basis vector in object space.
                                            // (i.e., its length determines scaling of parameterization.)
                                            // living on the tangent plane defined by intrinsic bases

        Data()
            : paramed()
            , dist   (util::dbl_max())
            , weight ()
            , uv     (Eigen::Vector2d::Zero())
            , basis_u(Eigen::Vector2d::Zero())
        {}
    } expmap;
};

struct ExpMap_FaceTraits {
    bool expmap_paramed;

    ExpMap_FaceTraits()
        : expmap_paramed()
    {}
};

namespace internal {
    template <class TMeshBase>
    void expmap_compute_common(TMeshBase* mesh,
                               const std::vector<OpenMesh::PolyConnectivity::VHandle>& seeds,
                               const std::vector<Eigen::Vector3d                    >& seeds_basis_u,
                               const std::vector<Eigen::Vector2d                    >& seeds_uv,
                               double dist_max,
                               const Eigen::Vector2d& uv_min,
                               const Eigen::Vector2d& uv_max,
                               std::vector<OpenMesh::PolyConnectivity::FHandle>& expmap_paramed_faces)
    {
        // helper functions ==============================================================================================
        auto get_intr_basis = [] (const Eigen::Vector3d& normal) -> Eigen::Vector3d {
            // get unit basis vector orthogonal to input vector in intrinsical manner
            double nx = std::abs(normal.x());
            double ny = std::abs(normal.y());
            double nz = std::abs(normal.z());
            Eigen::Vector3d basis =
                nx < ny && ny < nz ? Eigen::Vector3d::UnitX() :
                ny < nz            ? Eigen::Vector3d::UnitY() :
                                     Eigen::Vector3d::UnitZ();
            eigen_util::orthonormalize(normal, basis);
            return basis;
        };

        auto compute_rotation = [get_intr_basis] (const Eigen::Vector3d& p_normal,
                                                  const Eigen::Vector3d& q_normal) -> Eigen::Rotation2Dd
            // compute 2D rotation which will be used to rotate basis_u of p to estimate that of q
        {
            // get intrinsic basis vectors of p, q
            auto p_intr_basis = get_intr_basis(p_normal);
            auto q_intr_basis = get_intr_basis(q_normal);

            // step 1: compute 3D rotation that aligns q_normal with p_normal. apply it to q_basis.
            auto rot1_axis = q_normal.cross(p_normal);
            if (!rot1_axis.isZero()) {
                rot1_axis.normalize();
                double rot1_angle = util::acos_clamped(q_normal.dot(p_normal));
                q_intr_basis = Eigen::AngleAxisd(rot1_angle, rot1_axis) * q_intr_basis;
            }

            // step 2: compute 2D rotation that makes (rotated) q_basis aligned with p_basis.
            double rot2_angle = util::acos_clamped(q_intr_basis.dot(p_intr_basis));
            if (q_intr_basis.cross(p_intr_basis).dot(p_normal) < 0)     // negative rotation
                rot2_angle *= -1.0f;

            return Eigen::Rotation2Dd(rot2_angle);
        };

        auto compute_uv = [get_intr_basis] (const Eigen::Vector3d& p_point ,
                                            const Eigen::Vector3d& p_normal,
                                            const Eigen::Vector2d& p_uv,
                                            const Eigen::Vector2d& p_basis_u,
                                            const Eigen::Vector3d& q_point ) -> Eigen::Vector2d
            // computes uv for point q using p's point, normal, uv, basis_u
        {
            auto p_intr_basis_x = get_intr_basis(p_normal);
            auto p_intr_basis_y = p_normal.cross(p_intr_basis_x);

            // project (rotate) pq into tangent plane
            Eigen::Vector3d pq = q_point - p_point;
            Eigen::Vector2d local_uv(pq.dot(p_intr_basis_x), pq.dot(p_intr_basis_y));
            local_uv *= pq.norm() / local_uv.norm();

            // encode 2D local_uv using propagated (non-unit) basis vectors
            auto p_basis_v = eigen_util::rotate90(p_basis_u);
            local_uv = Eigen::Vector2d(p_basis_u.dot(local_uv), p_basis_v.dot(local_uv));

            // account for scale difference
            local_uv /= p_basis_u.squaredNorm();

            Eigen::Vector2d q_uv = p_uv + local_uv;

            return q_uv;
        };
        // ============================================================================================== helper functions

        // clear all vertex data
        for (auto v : mesh->vertices())
            mesh->data(v).expmap = ExpMap_VertexTraits::Data();

        // priority queue for propagation front
        struct QueueElement {
            OpenMesh::PolyConnectivity::VHandle vhandle;
            double dist;

            QueueElement() : dist(){}
            QueueElement(OpenMesh::PolyConnectivity::VHandle vhandle_, double dist_) : vhandle(vhandle_) , dist(dist_) {}
            bool operator<(const QueueElement& rhs) const { return dist > rhs.dist; }       // note: smaller distance gets higher priority
        };
        std::priority_queue<QueueElement> candidates;

        // init
        for (size_t i = 0; i < seeds.size(); ++i) {
            auto seed = seeds[i];
            auto& seed_data = mesh->data(seed).expmap;

            Eigen::Vector3d seed_normal       = o2e(mesh->normal(seed));
            Eigen::Vector3d seed_intr_basis_x = get_intr_basis(seed_normal);
            Eigen::Vector3d seed_intr_basis_y = seed_normal.cross(seed_intr_basis_x);
            Eigen::Vector3d seed_basis_u      = seeds_basis_u[i];

            seed_data.weight  += 1;
            seed_data.dist     = 0;
            seed_data.uv      += seeds_uv[i];
            seed_data.basis_u += Eigen::Vector2d(seed_intr_basis_x.dot(seed_basis_u), seed_intr_basis_y.dot(seed_basis_u));

            candidates.push(QueueElement(seed, 0));
        }

        while (!candidates.empty()) {
            auto p = candidates.top().vhandle;
            candidates.pop();

            auto& p_data   = mesh->data(p).expmap;
            auto  p_point  = o2e(mesh->point (p));
            auto  p_normal = o2e(mesh->normal(p));

            if (p_data.paramed) continue;
            p_data.paramed = true;

            p_data.uv      /= p_data.weight;
            p_data.basis_u /= p_data.weight;

            if (dist_max < p_data.dist) continue;

            if (!Eigen::AlignedBox2d(uv_min, uv_max).contains(p_data.uv)) continue;

            for (auto q = mesh->vv_iter(p); q.is_valid(); ++q) {
                auto& q_data   = mesh->data(*q).expmap;
                auto  q_point  = o2e(mesh->point (*q));
                auto  q_normal = o2e(mesh->normal(*q));

                if (q_data.paramed) continue;

                // distance update
                double dist_pq = (p_point - q_point).norm();
                if (p_data.dist + dist_pq < q_data.dist)
                    q_data.dist = p_data.dist + dist_pq;

                // weight based on distance between p and q
                double weight = 1.0f / std::pow(dist_pq, 0.25);
                q_data.weight += weight;

                // estimate of data by weighted averaging
                q_data.uv      += weight * compute_uv(p_point, p_normal, p_data.uv, p_data.basis_u, q_point);
                q_data.basis_u += weight * (compute_rotation(p_normal, q_normal) * p_data.basis_u);            // be sure to use parentheses! otherwise Eigen will happily go funny:)

                candidates.push(QueueElement(*q, q_data.dist));
            }
        }

        // collect faces with its vertices all paramed
        expmap_paramed_faces.clear();
        expmap_paramed_faces.reserve(mesh->n_faces());
        for (auto f : mesh->faces()) {
            mesh->data(f).expmap_paramed = true;
            for (auto v = mesh->fv_iter(f); v.is_valid(); ++v) {
                if (!mesh->data(*v).expmap.paramed) {
                    mesh->data(f).expmap_paramed = false;
                    break;
                }
            }
            if (mesh->data(f).expmap_paramed)
                expmap_paramed_faces.push_back(f);
        }
    }
}

template <class TMeshBase, class TMesh>
struct ExpMap : public DerivedPtrHolder<TMesh, ExpMap<TMeshBase, TMesh>> {
    std::vector<typename TMeshBase::FHandle> expmap_paramed_faces;

    void expmap_compute(
        const std::vector<typename TMeshBase::VHandle>& seeds,
        const std::vector<Eigen::Vector3d            >& seeds_basis_u,
        const std::vector<Eigen::Vector2d            >& seeds_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        TMesh* mesh = DerivedPtrHolder<TMesh, ExpMap<TMeshBase, TMesh>>::derived_ptr;
        internal::expmap_compute_common(mesh, seeds, seeds_basis_u, seeds_uv, dist_max, uv_min, uv_max, expmap_paramed_faces);
    }
    void expmap_compute(
        typename TMeshBase::VHandle seed,
        Eigen::Vector3d             seed_basis_u,
        Eigen::Vector2d             seed_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        expmap_compute(
            std::vector<typename TMeshBase::VHandle>(1, seed        ),
            std::vector<Eigen::Vector3d            >(1, seed_basis_u),
            std::vector<Eigen::Vector2d            >(1, seed_uv     ),
            dist_max,
            uv_min,
            uv_max);
    }
};

// specialization for triangle meshes, with functionality to convert on-surface position (face id + barycentric coord) into uv coordinates.
template <class TTrait, class TMesh>
struct ExpMap<OpenMesh::TriMesh_ArrayKernelT<TTrait>, TMesh> : public DerivedPtrHolder<TMesh, ExpMap<OpenMesh::TriMesh_ArrayKernelT<TTrait>, TMesh>> {
    typedef OpenMesh::TriMesh_ArrayKernelT<TTrait> MeshBase;

    std::vector<typename MeshBase::FHandle> expmap_paramed_faces;

    void expmap_compute(
        const std::vector<typename MeshBase::VHandle>& seeds,
        const std::vector<Eigen::Vector3d           >& seeds_basis_u,
        const std::vector<Eigen::Vector2d           >& seeds_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        TMesh* mesh = get_mesh();
        internal::expmap_compute_common(mesh, seeds, seeds_basis_u, seeds_uv, dist_max, uv_min, uv_max, expmap_paramed_faces);
    }
    void expmap_compute(
        typename MeshBase::VHandle seed,
        Eigen::Vector3d            seed_basis_u,
        Eigen::Vector2d            seed_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        expmap_compute(
            std::vector<typename MeshBase::VHandle>(1, seed        ),
            std::vector<Eigen::Vector3d           >(1, seed_basis_u),
            std::vector<Eigen::Vector2d           >(1, seed_uv     ),
            dist_max,
            uv_min,
            uv_max);
    }
    void expmap_compute(
        const std::vector<FaceBaryCoord  >& seeds,
        const std::vector<Eigen::Vector3d>& seeds_basis_u,
        const std::vector<Eigen::Vector2d>& seeds_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        TMesh* mesh = get_mesh();

        std::vector<typename MeshBase::VHandle> seeds2;
        std::vector<Eigen::Vector3d           > seeds2_basis_u;
        std::vector<Eigen::Vector2d           > seeds2_uv;

        for (size_t i = 0; i < seeds.size(); ++i) {
            auto& seed         = seeds        [i];
            auto& seed_basis_u = seeds_basis_u[i];
            auto& seed_uv      = seeds_uv     [i];

            Eigen::Vector3d p = o2e(seed.blend_point (*mesh));
            Eigen::Vector3d n = o2e(seed.blend_normal(*mesh));

            Eigen::Vector3d seed_basis_v = n.cross(seed_basis_u);

            double offset_scaling = 1 / seed_basis_u.squaredNorm();

            for (auto v = mesh->fv_iter(seed.f); v; ++v) {
                Eigen::Vector3d pq = o2e(mesh->point(v)) - p;
                Eigen::Vector2d uv_offset(pq.dot(seed_basis_u), pq.dot(seed_basis_v));
                uv_offset *= offset_scaling;

                seeds2        .push_back(v);
                seeds2_basis_u.push_back(seed_basis_u);
                seeds2_uv     .push_back(seed_uv + uv_offset);
            }
        }

        expmap_compute(seeds2, seeds2_basis_u, seeds2_uv, dist_max, uv_min, uv_max);
    }
    void expmap_compute(
        FaceBaryCoord   seed,
        Eigen::Vector3d seed_basis_u,
        Eigen::Vector2d seed_uv,
        double dist_max,
        const Eigen::Vector2d& uv_min = Eigen::Vector2d::Constant(-std::numeric_limits<double>::max()),
        const Eigen::Vector2d& uv_max = Eigen::Vector2d::Constant( std::numeric_limits<double>::max()))
    {
        expmap_compute(
            std::vector<FaceBaryCoord  >(1, seed        ),
            std::vector<Eigen::Vector3d>(1, seed_basis_u),
            std::vector<Eigen::Vector2d>(1, seed_uv     ),
            dist_max,
            uv_min,
            uv_max);
    }
    // conversion from on-surface position to uv coordinates
    boost::optional<Eigen::Vector2d> fbc_to_uv(const FaceBaryCoord& fbc) const {
        TMesh* mesh = get_mesh();

        if (!mesh->data(fbc.f).expmap_paramed)
            return boost::none;

        return fbc.blend_value<MeshBase, Eigen::Vector2d>(
            *mesh,
            [](const MeshBase& mesh, typename MeshBase::VHandle v) { return mesh.data(v).expmap.uv; }
        );
    }
    boost::optional<Eigen::Vector2d> fbc_to_uv(typename MeshBase::FHandle f, const BaryCoord& bc) const {
        return fbc_to_uv(FaceBaryCoord(f, bc));
    }
    FaceBaryCoord uv_to_fbc(Eigen::Vector2d uv) const {
        TMesh* mesh = get_mesh();

        for (auto f : expmap_paramed_faces) {
            // compute bounding box of this face
            Eigen::AlignedBox2d box;
            std::vector<Eigen::Vector2d> face_uv;
            for (auto v = mesh->cfv_iter(f); v.is_valid(); ++v) {
                auto expmap_uv = mesh->data(*v).expmap.uv;
                face_uv.push_back(expmap_uv);
                box.extend(expmap_uv);
            }

            // skip if uv is outside the bounding box
            if (!box.contains(uv)) continue;

            // compute barycentric coordinate
            // (1 - hit.u - hit.v) * face_uv[0] + hit.u * face_uv[1] + hit.v * face_uv[2] = uv
            // | face_uv[1] - face_uv[0], face_uv[2] - face_uv[0] | * |hit.u| = | uv - face_uv[0] |
            // |                                                  |   |hit.v|   |                 |
            Eigen::Matrix2d A;
            A << face_uv[1] - face_uv[0], face_uv[2] - face_uv[0];
            Eigen::Vector2d hit_uv = A.inverse() * (uv - face_uv[0]);

            // skip if barycentric coordinate is negative
            BaryCoord bc(1 - hit_uv[0] - hit_uv[1], hit_uv[0], hit_uv[1]);
            if (!bc.is_all_positive()) continue;

            // return result
            return FaceBaryCoord(f, bc);
        }

        return FaceBaryCoord();
    }
private:
    TMesh* get_mesh() const { return DerivedPtrHolder<TMesh, ExpMap<OpenMesh::TriMesh_ArrayKernelT<TTrait>, TMesh>>::derived_ptr; }
};

}


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

#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "determinant.hpp"
#include "SiconosVector.hpp"
#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"

#include "SiconosAlgebra.hpp"

using namespace Siconos;

//=======================
//       get norm
//=======================

double SimpleMatrix::normInf() const
{
  if (_num == 1)
    return norm_inf(*mat.Dense);
  else if (_num == 2)
    return norm_inf(*mat.Triang);
  else if (_num == 3)
    return norm_inf(*mat.Sym);
  else if (_num == 4)
    return norm_inf(*mat.Sparse);
  else if (_num == 5)
    return norm_inf(*mat.Banded);
  else if (_num == 6)
    return 0;
  else // if(_num==7)
    return 1;
}

void SimpleMatrix::normInfByColumn(SP::SiconosVector vIn) const
{
  if (_num == 1)
  {
    if (vIn->size() != size(1))
      RuntimeException::selfThrow("SimpleMatrix::normInfByColumn: the given vector does not have the right length");
    DenseVect tmpV = DenseVect(size(0));
    for (unsigned int i = 0; i < size(1); i++)
    {
       ublas::noalias(tmpV) = ublas::column(*mat.Dense, i);
       (*vIn)(i) = norm_inf(tmpV);
    }
  }
  else
    RuntimeException::selfThrow("SimpleMatrix::normInfByColumn: not implemented for data other than DenseMat");
}
//=======================
//       determinant
//=======================

double SimpleMatrix::det() const
{
  if (_num == 1)
    return determinant(*mat.Dense);
  else if (_num == 2)
    return determinant(*mat.Triang);
  else if (_num == 3)
    return determinant(*mat.Sym);
  else if (_num == 4)
    return determinant(*mat.Sparse);
  else if (_num == 5)
    return determinant(*mat.Banded);
  else if (_num == 6)
    return 0;
  else // if(_num==7)
    return 1;
}

void SimpleMatrix::trans()
{
  switch (_num)
  {
  case 1:
    *mat.Dense = ublas::trans(*mat.Dense);
    break;
  case 2:
    SiconosMatrixException::selfThrow("SimpleMatrix::trans() failed, the matrix is triangular matrix and can not be transposed in place.");
    break;
  case 3:
    break;
  case 4:
    *mat.Sparse = ublas::trans(*mat.Sparse);
    break;
  case 5:
    *mat.Banded = ublas::trans(*mat.Banded);
    break;
  case 6:
    break;
  case 7:
    break;
  }
  resetLU();
}

void SimpleMatrix::trans(const SiconosMatrix &m)
{
  if (m.isBlock())
    SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, not yet implemented for m being a BlockMatrix.");

  if (&m == this)
    trans();//SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, m = this, use this->trans().");
  else
  {
    unsigned int numM = m.num();
    switch (numM)
    {
    case 1:
      if (_num != 1)
        SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, try to transpose a dense matrix into another type.");
      noalias(*mat.Dense) = ublas::trans(*m.dense());
      break;
    case 2:
      if (_num != 1)
        SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, try to transpose a triangular matrix into a non-dense one.");
      noalias(*mat.Dense) = ublas::trans(*m.triang());
      break;
    case 3:
      *this = m;
      break;
    case 4:
      if (_num == 1)
        noalias(*mat.Dense) = ublas::trans(*m.sparse());
      else if (_num == 4)
        noalias(*mat.Sparse) = ublas::trans(*m.sparse());
      else
        SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, try to transpose a sparse matrix into a forbidden type (not dense nor sparse).");
      break;
    case 5:
      if (_num == 1)
        noalias(*mat.Dense) = ublas::trans(*m.banded());
      else if (_num == 5)
        noalias(*mat.Banded) = ublas::trans(*m.banded());
      else
        SiconosMatrixException::selfThrow("SimpleMatrix::trans(m) failed, try to transpose a banded matrix into a forbidden type (not dense nor banded).");
      break;
    case 6:
      *this = m;
      break;
    case 7:
      *this = m;
    }
    // unsigned int tmp = _dimRow;
    // _dimRow = _dimCol;
    // _dimCol = tmp;
    resetLU();
  }
}

const SimpleMatrix matrix_pow(const SimpleMatrix& m, unsigned int power)
{
  if (m.isBlock())
    SiconosMatrixException::selfThrow("Matrix, pow function: not yet implemented for BlockMatrix.");
  if ( m.size(0) != m.size(1))
    SiconosMatrixException::selfThrow("matrix_pow(SimpleMatrix), matrix is not square.");

  if (power > 0)
  {
    unsigned int num = m.num();
    if (num == 1)
    {
      DenseMat p = *m.dense();
      for (unsigned int i = 1; i < power; i++)
        p = prod(p, *m.dense());
      return p;
    }
    else if (num == 2)
    {
      TriangMat t = *m.triang();
      for (unsigned int i = 1; i < power; i++)
        t = prod(t, *m.triang());
      return t;
    }
    else if (num == 3)
    {
      SymMat s = *m.sym();
      for (unsigned int i = 1; i < power; i++)
        s = prod(s, *m.sym());
      return s;
    }
    else if (num == 4)
    {
      SparseMat sp = *m.sparse();
      for (unsigned int i = 1; i < power; i++)
        sp = prod(sp, *m.sparse());
      return sp;
    }
    else if (num == 5)
    {
      DenseMat b = *m.banded();
      for (unsigned int i = 1; i < power; i++)
        b = prod(b, *m.banded());
      return b;
    }
    else if (num == 6)
    {
      ZeroMat z(m.size(0), m.size(1));
      return z;
    }
    else // if (num==7)
    {
      IdentityMat I(m.size(0), m.size(1));;
      return I;
    }
  }
  else// if(power == 0)
  {
    IdentityMat I = ublas::identity_matrix<double>(m.size(0), m.size(1));
    return I;
  }
}

/*
The following code inverts the matrix input using LU-decomposition with backsubstitution of unit vectors. Reference: Numerical Recipies in C, 2nd ed., by Press, Teukolsky, Vetterling & Flannery.

you can solve Ax=b using three lines of ublas code:

permutation_matrix<> piv;
lu_factorize(A, piv);
lu_substitute(A, piv, x);

*/
#ifndef INVERT_MATRIX_HPP
#define INVERT_MATRIX_HPP

// REMEMBER to update "lu.hpp" header includes from boost-CVS
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

/* Matrix inversion routine.
Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
template<class T, class U, class V>
bool InvertMatrix(const ublas::matrix<T, U, V>& input, ublas::matrix<T, U, V>& inverse)
{
  using namespace boost::numeric::ublas;
  typedef permutation_matrix<std::size_t> pmatrix;
// create a working copy of the input
  matrix<T, U, V> A(input);
// create a permutation matrix for the LU-factorization
  pmatrix pm(A.size1());

// perform LU-factorization
  int res = lu_factorize(A,pm);
  if (res != 0) return false;

// create identity matrix of "inverse"
  inverse.assign(ublas::identity_matrix<T>(A.size1()));

// backsubstitute to get the inverse
  lu_substitute(A, pm, inverse);

  return true;
}

#endif //INVERT_MATRIX_HPP

void invertMatrix(const SimpleMatrix& input, SimpleMatrix& output)
{
  InvertMatrix(*input.dense(), *output.dense());
}

/* XXX Find out if we can use an elementwise ublas operation */
SP::SiconosVector compareMatrices(const SimpleMatrix& data, const SimpleMatrix& ref)
{
  SimpleMatrix diff(data.size(0), data.size(1));
  SP::SiconosVector res(new SiconosVector(data.size(1)));
  diff = data - ref;
  for (unsigned int i = 0; i < data.size(0); ++i)
  {
    for (unsigned int j = 0; j < data.size(1); ++j)
      diff(i, j) /= 1 + fabs(ref(i, j));
  }
  diff.normInfByColumn(res);
  return res;

}


#include <iostream>
#include <memory>
#include <tuple>
#include <stdlib.h>

#include <boost/optional.hpp>

// lvr2 includes
#include "lvr2/registration/TransformUtils.hpp"
#include "lvr2/types/MatrixTypes.hpp"

int main(int argc, char** argv)
{
    std::cout << "Coordinate Example" << std::endl;
    // Overview Coordinate Systems

    /** LVR / ROS
    *        z  x
    *        | /
    *   y ___|/
    *
    *  - x: front
    *  - y: left
    *  - z: up
    *  - scale: m
    */

    /** OpenCV
     *
     *        z
     *       /
     *      /___ x
     *     |
     *     |
     *     y
     *
     * - x: right
     * - y: down
     * - z: front
     * - scale: m
     */

    // x: 2.0, y: 0.5, z: 1.0
    // front: 1.0, right: 2.0, down: 0.5
    lvr2::Vector3d cv_point = {2.0, 0.5, 1.0};
    std::cout << "cv point: " << cv_point.transpose() << std::endl;

    // convert to lvr
    // should be x(front): 1.0, y(left): -2.0, z(up): -0.5
    lvr2::Vector3d lvr_point = lvr2::openCvToLvr(cv_point);
    std::cout << "lvr point: " << lvr_point.transpose() << std::endl;

    if(lvr2::lvrToOpenCv(lvr_point) == cv_point)
    {
        std::cout << "LVR <-> OpenCV - Point: Success" << std::endl;
    }

    // check opencv transformation

    lvr2::Rotationd cv_rot, lvr_rot;

    double roll = -0.25*M_PI; // cv: z, lvr: x
    double pitch = 1.6*M_PI; // cv: -x, lvr: y
    double yaw = -0.06*M_PI; // cv: -y, lvr: z

    // cv rotate x: pitch
    cv_rot = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitZ())
        * Eigen::AngleAxisd(-pitch, Eigen::Vector3d::UnitX())
        * Eigen::AngleAxisd(-yaw, Eigen::Vector3d::UnitY());

    // cv -> lvr
    lvr_rot = lvr2::openCvToLvr(cv_rot);

    // cv_rot

    lvr2::Vector3d cv_point_rotated = cv_rot * cv_point;
    lvr2::Vector3d lvr_point_rotated = lvr_rot * lvr_point;

    // lvr2::openCvToLvr(cv_point_rotated) - lvr_point_rotated
    if((lvr2::openCvToLvr(cv_point_rotated) - lvr_point_rotated).norm() < 0.000001)
    {
        std::cout << "LVR <-> OpenCV - Rotation Matrix: Success" << std::endl;
    } else {
        std::cout << "LVR <-> OpenCV - Rotation Matrix: Wrong" << std::endl;
    }

    // transformation
    lvr2::Transformd cv_transform, lvr_transform;

    cv_transform = lvr2::Transformd::Identity();
    cv_transform.block<3,3>(0,0) = cv_rot;
    cv_transform(0,2) = 2.0;
    cv_transform(1,2) = 5.0;
    cv_transform(2,2) = -1.0;

    lvr_transform = lvr2::openCvToLvr(cv_transform);

    lvr2::Vector3d cv_point_transformed = cv_transform * cv_point;
    lvr2::Vector3d lvr_point_transformed = lvr_transform * lvr_point;

    if((lvr2::openCvToLvr(cv_point_transformed)-lvr_point_transformed).norm() < 0.000001)
    {
        std::cout << "LVR <-> OpenCV - Transformation Matrix: Success" << std::endl;
    } else {
        std::cout << "LVR <-> OpenCV - Transformation Matrix: Wrong" << std::endl;
    }

    return 0;
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__MPC__MPC_TRACKING_HPP_
#define CBR_CONTROL__MPC__MPC_TRACKING_HPP_

#include <Eigen/Dense>
#include <cbr_utils/thread_pool.hpp>
#include <cbr_utils/utils.hpp>
#include <cbr_math/interp.hpp>

#include <utility>
#include <future>
#include <mutex>
#include <thread>

#include "cltv_ocp.hpp"
#include "cltv_ocp_lie.hpp"
#include "dltv_ocp.hpp"
#include "dltv_ocp_solver.hpp"
#include "ocp_common.hpp"

using std::chrono::duration_cast, std::chrono::duration, std::chrono::nanoseconds;

namespace cbr
{

/**
 * @brief Helper type for MPC trajectory tracking
 *
 * CltvOcp requires an interface with get_xxx(tau) methods where tau is
 * the time elapsed from the initial condition of the problem.
 *
 * This class takes functions of absolute time t and implements the required
 * interfaces as functions of relative time.
 */
template<typename _problem_t>
struct MPCTrackingProblem : public _problem_t
{
public:
  using typename _problem_t::state_t;
  using typename _problem_t::deriv_t;
  using typename _problem_t::input_t;

  explicit MPCTrackingProblem(double T, const _problem_t & p)
  : _problem_t(p), T_(T) {}

  explicit MPCTrackingProblem(double T, _problem_t && p)
  : _problem_t(std::move(p)), T_(T) {}

  /**
   * @brief Return the time horizon of the problem
   * @param[out] T result
   */
  double get_T() const {return T_;}

  /**
   * @brief Return the problem initial condition
   * @param[out] x0 result
   */
  const state_t & get_x0() const {return x0;}

  /**
   * @brief Get the desired state
   * @param tau relative problem time tau = t - t0
   * @return xd(t0 + tau)
   */
  state_t get_xd(double tau) const
  {
    return xd(t0 + duration_cast<nanoseconds>(duration<double>(tau)));
  }
  /**
   * @brief Get the desired input
   * @param tau relative problem time tau = t - t0
   * @return ud(t0 + tau)
   */
  input_t get_ud(double tau) const
  {
    return ud(t0 + duration_cast<nanoseconds>(duration<double>(tau)));
  }

  /**
   * @brief Get the state linearization
   * @param tau relative problem time tau = t - t0
   * @return xl(t0 + tau)
   */
  state_t get_xl(double tau) const
  {
    return xl(t0 + duration_cast<nanoseconds>(duration<double>(tau)));
  }
  /**
   * @brief Get the state derivative linearization
   * @param tau relative problem time tau = t - t0
   * @return xldot(t0 + tau)
   */
  deriv_t get_xldot(double tau) const
  {
    return xldot(t0 + duration_cast<nanoseconds>(duration<double>(tau)));
  }
  /**
   * @brief Get the input linearization
   * @param tau relative problem time tau = t - t0
   * @return ul(t0 + tau)
   */
  input_t get_ul(double tau) const
  {
    return ul(t0 + duration_cast<nanoseconds>(duration<double>(tau)));
  }

private:
  // These functions return are defined in absolute time
  std::function<state_t(nanoseconds)> xd = [](nanoseconds) {return state_t{};};
  std::function<input_t(nanoseconds)> ud = [](nanoseconds) {return input_t{};};
  std::function<state_t(nanoseconds)> xl = [](nanoseconds) {return state_t{};};
  std::function<deriv_t(nanoseconds)> xldot = [](nanoseconds) {return deriv_t{};};
  std::function<input_t(nanoseconds)> ul = [](nanoseconds) {return input_t{};};

  nanoseconds t0 = nanoseconds(0);
  state_t x0{};
  double T_{0};

  // MPCTracking modifies the private variables from outside (except T_)
  template<typename problem_t__, std::size_t nPts__>
  friend class MPCTracking;
};

struct MPCTrackingParams
{
  double T{10};  // MPC horizon
  DltvOcpSolverParams solver_params{};
};

// Trait to detect whether a problem is defined on a lie group
template<typename, typename = void>
struct has_tangent : std::false_type {};

template<typename T>
struct has_tangent<T, std::void_t<typename T::Tangent>>: std::true_type {};

/**
 * @brief Trajectory tracking with nonlinear MPC
 *
 * @tparam problem_t stationary problem definition with dynamics, weights, and state bounds
 * @tparam nPts_ number of time discretization points
 *
 * NOTE: all set_xxx methods must be called before solving for the first time
 */
template<typename _problem_t, std::size_t nPts_>
class MPCTracking
{
public:
  // Linearization order used for system linearization
  static constexpr std::size_t LinOrder = 4;

  static constexpr bool is_lie = has_tangent<typename _problem_t::state_t>::value;

  using problem_t = _problem_t;
  using state_t = typename problem_t::state_t;
  using deriv_t = typename problem_t::deriv_t;
  using input_t = typename problem_t::input_t;  // must be eigen type

  // Use CltvOcpLie or CltvOcp depending on the type of problem we have
  using cltv_t = typename std::conditional<is_lie,
      CltvOcpLie<MPCTrackingProblem<_problem_t>>,
      CltvOcp<MPCTrackingProblem<_problem_t>>
    >::type;

  using dltv_t = DltvOcp<cltv_t, nPts_, LinOrder>;
  using solver_t = DltvOcpSolver<dltv_t>;

  using state_traj_t = typename std::conditional<is_lie,
      std::array<state_t, nPts_>,
      Eigen::Matrix<double, problem_t::nx, nPts_>
    >::type;

  struct Solution
  {
    DltvOcpSolverCode rc;
    Eigen::Matrix<double, 1, nPts_> t;
    state_traj_t x;
    Eigen::Matrix<double, input_t::SizeAtCompileTime, nPts_> u;
  };

  MPCTracking() = delete;
  MPCTracking(const MPCTracking &) = default;
  MPCTracking(MPCTracking &&) = default;
  MPCTracking & operator=(const MPCTracking &) = default;
  MPCTracking & operator=(MPCTracking &&) = default;

  explicit MPCTracking(const _problem_t & problem, MPCTrackingParams param = MPCTrackingParams{})
  : solver_(
      DltvOcp<cltv_t, nPts_, LinOrder>(cltv_t(MPCTrackingProblem<_problem_t>(param.T, problem))),
      param.solver_params
  ),
    uspline_(
      (Eigen::Matrix<double, 1, 2>() << 0., 1.).finished(),
      input_t::Zero()
    ),
    tp_(1)
  {
    mpcSol_.rc = DltvOcpSolverCode::no_run;

    lock_problem();

    if constexpr (is_lie) {
      // lie types default-initialize to identity
      set_xd([](std::chrono::nanoseconds) {return state_t{};});
      set_xl([](std::chrono::nanoseconds) {return state_t{};});
      set_xldot([](std::chrono::nanoseconds) {return deriv_t{};});
    } else {
      // Eigen types must be explicitly zero-initialized
      set_xd([](std::chrono::nanoseconds) {return state_t::Zero();});
      set_xl([](std::chrono::nanoseconds) {return state_t::Zero();});
      set_xldot([](std::chrono::nanoseconds) {return deriv_t::Zero();});
    }

    // zero-initialize linearization and desired inputs
    set_ud([](std::chrono::nanoseconds) {return input_t::Zero();});
    set_ul([](std::chrono::nanoseconds) {return input_t::Zero();});

    unlock_problem();
  }

  /**
   * @brief Block solving to safely update xd/ud/xl/xldot/ul
   *
   * Use before calling the set_xxx functions, then call unlock_problem()
   */
  void lock_problem() {problemsMtx_.lock();}

  /**
   * @brief Unblock solving
   *
   * Use after calling lock_problem()
   */
  void unlock_problem() {problemsMtx_.unlock();}

  /**
   * @brief Specify desired state trajectory as a function t -> xd(t) [absolute time]
   *
   * Only use after calling lock_problem()
   *
   * @param f xd as a function of t
   */
  template<typename T>
  void set_xd(T && f) {solver_.problem().problem().problem().xd = std::forward<T>(f);}

  /**
   * @brief Specify desired input trajectory as a function t -> ud(t) [absolute time]
   *
   * Only use after calling lock_problem()
   *
   * @param f ud as a function of t
   */

  template<typename T>
  void set_ud(T && f) {solver_.problem().problem().problem().ud = std::forward<T>(f);}

  /**
   * @brief Specify state linearization trajectory as a function t -> xd(t) [absolute time]
   *
   * Only use after calling lock_problem()
   *
   * May be required if xd/ud is changed to be far from current linearization
   */
  template<typename T>
  void set_xl(T && f) {solver_.problem().problem().problem().xl = std::forward<T>(f);}

  /**
   * @brief Specify state linearization trajectory as a function t -> \dot xd(t) [absolute time]
   *
   * Only use after calling lock_problem()
   *
   * May be required if xd/ud is changed to be far from current linearization
   */
  template<typename T>
  void set_xldot(T && f) {solver_.problem().problem().problem().xldot = std::forward<T>(f);}

  /**
   * @brief Specify state linearization trajectory as a function t -> xd(t) [absolute time]
   *
   * Only use after calling lock_problem()
   *
   * May be required if xd/ud is changed to be far from current linearization
   */
  template<typename T>
  void set_ul(T && f) {solver_.problem().problem().problem().ul = std::forward<T>(f);}

  /**
   * @brief Asynchronous update of MPC
   *
   * @param t current absolute time
   * @param xt state at time t
   * @return future to solver status
   *
   * If optimization is already running this function returns without without effect.
   */
  std::shared_future<DltvOcpSolverCode>
  update(const nanoseconds t, const state_t & xt)
  {
    std::lock_guard lock(futMtx_);
    if (fut_.valid()) {
      if (fut_.wait_for(nanoseconds(0)) != std::future_status::ready) {
        return {};
      }
    }

    fut_ = tp_.enqueue(&MPCTracking::update_, this, t, xt, 1);
    return fut_.share();
  }

  /**
   * @brief Blocking update of MPC
   *
   * @param t current absolute time
   * @param xt state at time t
   * @param iter number of solve/linearaze iterations to run
   * @return solver status
   */
  DltvOcpSolverCode update_sync(const nanoseconds t, const state_t & xt, std::size_t iter = 1)
  {
    std::lock_guard lock(futMtx_);
    if (fut_.valid()) {
      fut_.wait();
    }
    return update_(t, xt, iter);
  }

  /**
   * @brief Obtain input at given time for most recent solution
   */
  input_t get_u(nanoseconds t)
  {
    std::lock_guard lock(solutionMtx_);
    return uspline_.val(duration_cast<duration<double>>(t - uspline_t0_).count());
  }

  /**
   * @brief Obtain most recent solution
   *
   * NOTE: solution times are defined on [0, T]
   */
  Solution solution()
  {
    std::lock_guard lock(solutionMtx_);
    return mpcSol_;
  }

private:
  /**
   * @brief Internal helper function to update mpc solution
   */
  DltvOcpSolverCode update_(std::chrono::nanoseconds t0, const state_t & x0, std::size_t iter)
  {
    std::lock_guard lock(problemsMtx_);  // lock for the duration to ensure consistency
    DltvOcpSolverCode rc = DltvOcpSolverCode::no_run;

    solver_.problem().problem().problem().t0 = t0;
    solver_.problem().problem().problem().x0 = x0;

    for (std::size_t i = 0; i != iter; ++i) {
      solver_.init();

      auto sol = solver_.solve();
      rc = sol.rc;

      if (sol.rc != DltvOcpSolverCode::success) {
        break;
      } else {
        Solution abs_sol;
        abs_sol.rc = sol.rc;

        for (auto i = 0u; i < nPts_; i++) {
          // solution is a trajectory around the linearization, we add back
          // the linearization to get a trajectory in global coordinates
          const double tt = solver_.problem().indexToTime(i);
          abs_sol.t[i] = tt;
          if constexpr (is_lie) {
            abs_sol.x[i] =
              solver_.problem().problem().problem().get_xl(tt) * state_t::exp(sol.x.col(i));
          } else {
            abs_sol.x.col(i) = solver_.problem().problem().problem().get_xl(tt) + sol.x.col(i);
          }
          abs_sol.u.col(i) = solver_.problem().problem().problem().get_ul(tt) + sol.u.col(i);
        }

        auto ulin = cbr::PiecewiseLinear::fitND(abs_sol.t, abs_sol.u);

        {
          // store solution so that get_u() can be called
          std::lock_guard lock(solutionMtx_);
          mpcSol_ = abs_sol;

          uspline_t0_ = t0;
          uspline_ = ulin;
        }

        // Set desired input for future solving to current input
        solver_.problem().problem().problem().ud =
          [u0 = abs_sol.u.col(0).eval()](nanoseconds) -> input_t {
            return u0;
          };

        // Update linearization points (functions defined in absolute time)
        solver_.problem().problem().problem().ul =
          [t0 = t0, ulin = std::move(ulin)](nanoseconds t) -> input_t {
            return ulin.val(duration_cast<duration<double>>(t - t0).count());
          };

        if constexpr (is_lie) {
          // fit splines: first need to copy to a vector
          vector_aligned<state_t> sol_vec(abs_sol.x.begin(), abs_sol.x.end());
          auto xlin = cbr::Spline::fitLie(abs_sol.t, std::move(sol_vec));
          xlin.set_extrap(PiecewisePoly::EXTRAP::CLAMP);

          solver_.problem().problem().problem().xl =
            [t0 = t0, xlin = xlin](nanoseconds t) -> state_t {
              const auto t_spline = duration_cast<duration<double>>(t - t0).count();
              return xlin.val(t_spline);
            };
          solver_.problem().problem().problem().xldot =
            [t0 = t0, xlin = std::move(xlin)](nanoseconds t) -> deriv_t {
              const auto t_spline = duration_cast<duration<double>>(t - t0).count();
              return xlin.der(t_spline);
            };
        } else {
          // fit splines
          auto xlin = cbr::PiecewiseLinear::fitND(abs_sol.t, abs_sol.x);

          solver_.problem().problem().problem().xl =
            [t0 = t0, xlin = xlin](nanoseconds t) -> state_t {
              return xlin.val(duration_cast<duration<double>>(t - t0).count());
            };
          solver_.problem().problem().problem().xldot =
            [t0 = t0, xlin = xlin](nanoseconds t) -> deriv_t {
              return xlin.der(duration_cast<duration<double>>(t - t0).count());
            };
        }

        // reset warm-start solution to zeros
        solver_.set_ic(decltype(sol.x)::Zero(), decltype(sol.u)::Zero());
      }
    }

    return rc;
  }

private:
  solver_t solver_{};

  Solution mpcSol_;
  std::mutex solutionMtx_, problemsMtx_, futMtx_;
  std::future<DltvOcpSolverCode> fut_;

  nanoseconds uspline_t0_;
  cbr::PiecewisePolyND uspline_;

  ThreadPool tp_;
};

}  // namespace cbr

#endif  // CBR_CONTROL__MPC__MPC_TRACKING_HPP_