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


#include "mps_voxels/octree_utils.h"
#include "mps_voxels/pointcloud_utils.h"
#include <mps_shape_completion_msgs/CompleteShape.h>
//#include <octomap/octomap.h>
//#include <octomap_msgs/conversions.h>
//#include <octomap_msgs/GetOctomap.h>
#include <Eigen/Geometry>

#include <std_msgs/ByteMultiArray.h>

#include <ros/ros.h>
#include <memory>
#include <cassert>

namespace om = octomap;
using namespace mps;

std::shared_ptr<OctreeRetriever> mapClient;
std::shared_ptr<VoxelCompleter> completionClient;
ros::Publisher octreePub;

void publishOctree(std::shared_ptr<octomap::OcTree>& tree, const std::string& globalFrame)
{
	bool publishMarkerArray = (octreePub.getNumSubscribers()>0);

	if (publishMarkerArray)
	{
		visualization_msgs::MarkerArray occupiedNodesVis = visualizeOctree(tree.get(), globalFrame);

		octreePub.publish(occupiedNodesVis);
	}
}

// Point cloud utilities
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <tf_conversions/tf_eigen.h>
//#include <abstract.h>

using PointT = pcl::PointXYZRGB;

std::shared_ptr<tf::TransformListener> listener;

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
{
	// Get Octree
	std::shared_ptr<om::OcTree> octree;
	std::string globalFrame;
	std::tie(octree, globalFrame) = mapClient->getOctree();
	if (!octree)
	{
		ROS_ERROR("Octree lookup failed.");
		return;
	}

	// Transform point cloud to local frame
	if (!listener->waitForTransform(globalFrame, cloudMsg->header.frame_id, ros::Time(0), ros::Duration(5.0)))
	{
		ROS_WARN_STREAM("Failed to look up transform between '" << globalFrame << "' and '" << cloudMsg->header.frame_id << "'.");
		return;
	}
//	sensor_msgs::PointCloud2 transformedCloud;
//	pcl_ros::transformPointCloud(globalFrame, *cloudMsg, transformedCloud, *listener);

	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
//	pcl::fromROSMsg(transformedCloud, *cloud);
	pcl::fromROSMsg(*cloudMsg, *cloud);

	// Filter box
	// Coordinates in table ("global") frame
	Eigen::Vector4f maxExtent(0.5, 1, 1, 1);
	Eigen::Vector4f minExtent(-0.5, -1, -0.25, 1);

	tf::StampedTransform tableFrameInCameraCoordinates;
	listener->lookupTransform(globalFrame, cloudMsg->header.frame_id, ros::Time(0), tableFrameInCameraCoordinates);
	Eigen::Affine3d worldTcamera;
	tf::transformTFToEigen(tableFrameInCameraCoordinates, worldTcamera);

	pcl::PointCloud<PointT>::Ptr cropped_cloud = cropInCameraFrame(cloud, minExtent, maxExtent, worldTcamera);
	if (cropped_cloud->empty())
	{
		ROS_WARN("Filtered cloud contains no points.");
		return;
	}

	// Perform segmentation
	std::vector<pcl::PointCloud<PointT>::Ptr> segments = segment(cropped_cloud);

	std::vector<std::shared_ptr<om::OcTree>> completedClusters;
	for (const pcl::PointCloud<PointT>::Ptr& cluster_cloud : segments)
	{
		// Compute bounding box
		Eigen::Vector3d min, max;
		getBoundingCube(*cluster_cloud, min, max);

		std::shared_ptr<om::OcTree> subtree(octree->create());
		for (const PointT& pt : *cluster_cloud)
		{
			Eigen::Vector3f worldPt = worldTcamera.cast<float>() * pt.getVector3fMap();
			assert(worldPt.cwiseMax(maxExtent.head<3>()) == maxExtent.head<3>());
			assert(worldPt.cwiseMin(minExtent.head<3>()) == minExtent.head<3>());

			om::OcTreeNode* node = subtree->setNodeValue(worldPt.x(), worldPt.y(), worldPt.z(), std::numeric_limits<float>::infinity(), false);
			CV_UNUSED(node);
//			assert(node);
//			assert(node->getOccupancy() > 0.5);
//			assert(octree->search(worldPt.x(), worldPt.y(), worldPt.z())->getOccupancy() > 0.5); // May fail if server is behind
//			assert(subtree->search(worldPt.x(), worldPt.y(), worldPt.z())->getOccupancy() > 0.5);
		}
		completionClient->completeShape(min, max, worldTcamera.cast<float>(), octree.get(), subtree.get());

		completedClusters.push_back(subtree);

		publishOctree(subtree, globalFrame);
	}

}

int main(int argc, char* argv[])
{
	ros::init(argc, argv, "shape_completion_manager");
	ros::NodeHandle nh;

	listener = std::make_shared<tf::TransformListener>();

//	localMapPub = nh.advertise<std_msgs::ByteMultiArray>("local_occupancy", 1, true);
	octreePub = nh.advertise<visualization_msgs::MarkerArray>("predicted_points", 1, true);
	mapClient = std::make_shared<OctreeRetriever>(nh);
	completionClient = std::make_shared<VoxelCompleter>(nh);

	ros::Subscriber sub = nh.subscribe ("kinect2_victor_head/qhd/points", 1, cloud_cb);

//	getOccupancy(om::point3d(-0.1f, -0.1f, -0.1f), om::point3d(0.1f, 0.1f, 0.1f));

//	ros::spinOnce();
	ros::spin();

	return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIVS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_DIVS_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/divs.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/simd/toolbox/arithmetic/functions/divs.hpp>
#include <boost/simd/include/functions/simd/is_eqz.hpp>
#include <boost/simd/include/functions/simd/is_ltz.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/divides.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/if_else_zero.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divs_, tag::cpu_, (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return a0/a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divs_, tag::cpu_, (A0)(X)
                            , ((simd_<uint_<A0>,X>))
                              ((simd_<uint_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      const bA0 iseqza1 = is_eqz(a1);
      const A0 aa1 = a1+if_else_zero(iseqza1, One<A0>());
      const A0 r1 = a0/aa1; //a1!= 0
      const A0 r2 = select(is_eqz(a0),Zero<A0>(),Valmax<A0>());
      return select(iseqza1, r2, r1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::divs_, tag::cpu_, (A0)(X)
                            , ((simd_<int_<A0>,X>))
                              ((simd_<int_<A0>,X>))
                            )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      const bA0 iseqza1 = is_eqz(a1);
      const A0 c = select(logical_and(eq(a0,Valmin<A0>()),eq(a1, Mone<A0>())), One<A0>(), Zero<A0>());
      const A0 aa1 = a1+if_else_zero(iseqza1, One<A0>());
      const A0 r1 = (a0+c)/aa1; //a1!= 0
      const A0 v2 = select(is_ltz(a0),Valmin<A0>(),Valmax<A0>());
      const A0 r2 = select(is_eqz(a0),Zero<A0>(),v2); //a1 == 0
      return select(iseqza1, r2, r1);
    }
  };
} } }

#endif


// Copyright 2002 Paul Mensonides
// Copyright 2019 Peter Dimov
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt

#include <boost/preprocessor/comparison.hpp>
#include "test_macro.h"

/* equality */

BEGIN BOOST_PP_EQUAL(2, 0) == 0 END
BEGIN BOOST_PP_EQUAL(2, 2) == 1 END

/* inequality */

BEGIN BOOST_PP_NOT_EQUAL(2, 0) == 1 END
BEGIN BOOST_PP_NOT_EQUAL(2, 2) == 0 END

/* less */

BEGIN BOOST_PP_LESS(2, 1) == 0 END
BEGIN BOOST_PP_LESS(1, 2) == 1 END

/* less_equal */

BEGIN BOOST_PP_LESS_EQUAL(2, 1) == 0 END
BEGIN BOOST_PP_LESS_EQUAL(1, 2) == 1 END
BEGIN BOOST_PP_LESS_EQUAL(2, 2) == 1 END

/* greater */

BEGIN BOOST_PP_GREATER(2, 1) == 1 END
BEGIN BOOST_PP_GREATER(1, 2) == 0 END

/* greater_equal */

BEGIN BOOST_PP_GREATER_EQUAL(2, 1) == 1 END
BEGIN BOOST_PP_GREATER_EQUAL(1, 2) == 0 END
BEGIN BOOST_PP_GREATER_EQUAL(2, 2) == 1 END

int main()
{
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_MAXNUMMAG_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_MAXNUMMAG_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
     @brief maxnummag generic tag

     Represents the maxnummag function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct maxnummag_ : ext::elementwise_<maxnummag_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<maxnummag_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_maxnummag_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::maxnummag_, Site> dispatching_maxnummag_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::maxnummag_, Site>();
   }
   template<class... Args>
   struct impl_maxnummag_;
  }
  /*!
    Returns the input value which have the greatest absolute value, ignoring nan.

    @par Semantic:

    @code
    T r = maxnummag(a0,a1);
    @endcode

    is similar to:

    @code
    T r = isnan(a0) ? a1 : (isnan(a1) ? a0 : maxmag(a0, a1));
    @endcode

    @param a0

    @param a1

    @return a value of same type as the inputs
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::maxnummag_, maxnummag, 2)
} }

#endif


/*
 *  testClipTriangle.cpp
 *  Trogdor6
 *
 *  Created by Paul C Hansen on 7/2/11.
 *  Copyright 2011 Stanford University. All rights reserved.
 *
 *  This file is covered by the MIT license.  See LICENSE.txt.
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE testClipTriangle

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <cstdlib>
#include <ctime>
#include "Voxelize/ClipTriangle.h"
#include "SimpleMesh/Search.h"
#include "TimeWrapper.h"

using namespace std;
using namespace TimeWrapper;

//#define CHECK_CLOSE(p0, p1, tol) BOOST_CHECK_SMALL(norm((p0)-(p1)), tol)
#define CHECK_CLOSE(p0, p1) BOOST_CHECK_SMALL(norm((p0)-(p1)), 1e-9)

const void* NULLPTR = 0L;
const int X = 0, Y = 1, Z = 2;

BOOST_AUTO_TEST_CASE( Constructor )
{
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1);
    SimpleMesh::Triangle tri(SimpleMesh::Test::oneTri(o, x, y));
    ClippedTriangle ctri(tri);

    BOOST_CHECK_EQUAL(ctri.vertices().size(), 3);
    BOOST_CHECK_EQUAL(ctri.edges().size(), 3);
    BOOST_CHECK_EQUAL(ctri.triangle(), &tri);

    for (int edge = 0; edge < 3; edge++)
        BOOST_CHECK_EQUAL(ctri.edges()[edge], &tri.edgeSensitivity(edge));

    Rect3d bounds = ctri.bounds();
    BOOST_CHECK(bounds.p1 == o);
    BOOST_CHECK(bounds.p2 == x + y);
}

BOOST_AUTO_TEST_CASE( ClipInPlane )
{
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1);
    SimpleMesh::Triangle tri(SimpleMesh::Test::oneTri(o, x, y));
    ClippedTriangle ctri(tri);
    ClippedTriangle clipped;

    const void* NULLPTR = 0L;

    // Clip below x = 0.5
    clipped = ctri.partBelow(X, 0.5);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 4);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 4);
    CHECK_CLOSE(clipped.vertices()[0], Vector3d(0,0,0));
    CHECK_CLOSE(clipped.vertices()[1], Vector3d(0.5, 0, 0));
    CHECK_CLOSE(clipped.vertices()[2], Vector3d(0.5, 0.5, 0));
    CHECK_CLOSE(clipped.vertices()[3], Vector3d(0, 1, 0));

    BOOST_CHECK_EQUAL(clipped.edges()[0], &tri.edgeSensitivity(0));
    BOOST_CHECK_EQUAL(clipped.edges()[1], NULLPTR);
    BOOST_CHECK_EQUAL(clipped.edges()[2], &tri.edgeSensitivity(1));
    BOOST_CHECK_EQUAL(clipped.edges()[3], &tri.edgeSensitivity(2));

    // Clip above x = 0.5
    clipped = ctri.partAbove(X, 0.5);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 3);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 3);
    CHECK_CLOSE(clipped.vertices()[0], Vector3d(0.5,0,0));
    CHECK_CLOSE(clipped.vertices()[1], Vector3d(1,0,0));
    CHECK_CLOSE(clipped.vertices()[2], Vector3d(0.5,0.5,0));

    BOOST_CHECK_EQUAL(clipped.edges()[0], &tri.edgeSensitivity(0));
    BOOST_CHECK_EQUAL(clipped.edges()[1], &tri.edgeSensitivity(1));
    BOOST_CHECK_EQUAL(clipped.edges()[2], NULLPTR);

    // Clip below y = 0.5
    clipped = ctri.partBelow(Y, 0.5);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 4);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 4);
    CHECK_CLOSE(clipped.vertices()[0], Vector3d(0,0,0));
    CHECK_CLOSE(clipped.vertices()[1], Vector3d(1,0,0));
    CHECK_CLOSE(clipped.vertices()[2], Vector3d(0.5,0.5,0));
    CHECK_CLOSE(clipped.vertices()[3], Vector3d(0,0.5,0));

    BOOST_CHECK_EQUAL(clipped.edges()[0], &tri.edgeSensitivity(0));
    BOOST_CHECK_EQUAL(clipped.edges()[1], &tri.edgeSensitivity(1));
    BOOST_CHECK_EQUAL(clipped.edges()[2], NULLPTR);
    BOOST_CHECK_EQUAL(clipped.edges()[3], &tri.edgeSensitivity(2));

    // Clip above y = 0.5
    clipped = ctri.partAbove(Y, 0.5);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 3);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 3);
    CHECK_CLOSE(clipped.vertices()[0], Vector3d(0.5,0.5,0));
    CHECK_CLOSE(clipped.vertices()[1], Vector3d(0,1,0));
    CHECK_CLOSE(clipped.vertices()[2], Vector3d(0,0.5,0));

    BOOST_CHECK_EQUAL(clipped.edges()[0], &tri.edgeSensitivity(1));
    BOOST_CHECK_EQUAL(clipped.edges()[1], &tri.edgeSensitivity(2));
    BOOST_CHECK_EQUAL(clipped.edges()[2], NULLPTR);
}

// Vertices on the boundary are considered to be outside.  This should prevent
// the existence of degenerate polygons (just boundary segments).
//
// This triangle has an edge at x = 0, an edge at y = 1, and a diagonal.
//
// +------+
// |     /
// |    /
// |   /
// |  /
// | /
// |/
// +
//
// My clipping convention is that partBelow keeps stuff with, e.g., x < 1.0,
// and partAbove keeps stuff with, e.g., x >= 0.0.  All necessary vertices
// will be preserved, but the edge sensitivity will saved for the x = 0 line
// and not for the y = 1 line.  This way the edge of a triangle along a voxel
// boundary will contribute to fill factor sensitivities only on one side at
// a time.
BOOST_AUTO_TEST_CASE( VerticesOnBoundary )
{
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    SimpleMesh::Triangle tri(SimpleMesh::Test::oneTri(o, xy, y));
    ClippedTriangle ctri(tri);
    ClippedTriangle clipped;

    // Clip below x = 0.0.  The vertices on the edge are included but the edge
    // is explicitly not, so the edge pointers are null.
    clipped = ctri.partBelow(X, 0.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 2);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 2);

    CHECK_CLOSE(clipped.vertices()[0], o);
    CHECK_CLOSE(clipped.vertices()[1], y);

    BOOST_CHECK_EQUAL(clipped.edges()[0], NULLPTR);
    BOOST_CHECK_EQUAL(clipped.edges()[1], NULLPTR);

    // Clip above x = 0.0.  All points with x >= 0.0 are included (all of them).
    clipped = ctri.partAbove(X,0.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 3);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 3);

    CHECK_CLOSE(clipped.vertices()[0], o);
    CHECK_CLOSE(clipped.vertices()[1], xy);
    CHECK_CLOSE(clipped.vertices()[2], y);

    BOOST_CHECK_EQUAL(clipped.edges()[0], &tri.edgeSensitivity(0));
    BOOST_CHECK_EQUAL(clipped.edges()[1], &tri.edgeSensitivity(1));
    BOOST_CHECK_EQUAL(clipped.edges()[2], &tri.edgeSensitivity(2));

    // Clip below x = 1.0.  All points with x < 1.0 are included.
    clipped = ctri.partBelow(X, 1.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 3);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 3);

    CHECK_CLOSE(clipped.vertices()[0], o);
    CHECK_CLOSE(clipped.vertices()[1], xy);
    CHECK_CLOSE(clipped.vertices()[2], y);

    BOOST_CHECK_EQUAL(clipped.edges()[0], &tri.edgeSensitivity(0));
    BOOST_CHECK_EQUAL(clipped.edges()[1], &tri.edgeSensitivity(1));
    BOOST_CHECK_EQUAL(clipped.edges()[2], &tri.edgeSensitivity(2));

    // Clip above x = 1.0.  All points with x >= 1.0 are included.  This will
    // initially toss the point at (1,0,0) into the polygon by itself, but the
    // finaly polygon will be empty because degenerate polygons get BALEETED.
    clipped = ctri.partAbove(X, 1.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 0);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 0);

    // Clip below y = 0.0.  Should be empty.
    clipped = ctri.partBelow(Y, 0.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 0);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 0);

    // Clip above y = 0.0.  Should be the whole triangle.
    clipped = ctri.partAbove(Y, 0.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 3);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 3);

    CHECK_CLOSE(clipped.vertices()[0], o);
    CHECK_CLOSE(clipped.vertices()[1], xy);
    CHECK_CLOSE(clipped.vertices()[2], y);

    BOOST_CHECK_EQUAL(clipped.edges()[0], &tri.edgeSensitivity(0));
    BOOST_CHECK_EQUAL(clipped.edges()[1], &tri.edgeSensitivity(1));
    BOOST_CHECK_EQUAL(clipped.edges()[2], &tri.edgeSensitivity(2));

    // Clip below y = 1.0.  Should be the whole triangle but exclude the
    // sensitivity of the top edge.
    clipped = ctri.partBelow(Y, 1.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 3);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 3);

    CHECK_CLOSE(clipped.vertices()[0], o);
    CHECK_CLOSE(clipped.vertices()[1], xy);
    CHECK_CLOSE(clipped.vertices()[2], y);

    BOOST_CHECK_EQUAL(clipped.edges()[0], &tri.edgeSensitivity(0));
    BOOST_CHECK_EQUAL(clipped.edges()[1], NULLPTR);
    BOOST_CHECK_EQUAL(clipped.edges()[2], &tri.edgeSensitivity(2));

    // Clip above y = 1.0.  Should be an isolated edge (i.e. a forward edge
    // and a backward edge).  The forward edge has sensitivity, and the
    // backward edge does not.
    clipped = ctri.partAbove(Y, 1.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 2);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 2);

    CHECK_CLOSE(clipped.vertices()[0], xy);
    CHECK_CLOSE(clipped.vertices()[1], y);

    BOOST_CHECK_EQUAL(clipped.edges()[0], &tri.edgeSensitivity(1));
    BOOST_CHECK_EQUAL(clipped.edges()[1], NULLPTR);
}

BOOST_AUTO_TEST_CASE( TriangleParallelToClippingPlane )
{
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    SimpleMesh::Triangle tri(SimpleMesh::Test::oneTri(o, xy, y));
    ClippedTriangle ctri(tri);
    ClippedTriangle clipped;

    // Clip below z = 0.0.  The vertices are preserved and the edges do NOT
    // save sensitivity information.  I'm not sure if this behavior is good or
    // not—mostly it's a side-effect of the clipping behavior for vertices
    // on clipping planes (it's not by design).  But is it harmful as such?
    // Probably not?  Anyway, this IS what it does:
    clipped = ctri.partBelow(Z, 0.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 3);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 3);

    CHECK_CLOSE(clipped.vertices()[0], o);
    CHECK_CLOSE(clipped.vertices()[1], xy);
    CHECK_CLOSE(clipped.vertices()[2], y);

    BOOST_CHECK_EQUAL(clipped.edges()[0], NULLPTR);
    BOOST_CHECK_EQUAL(clipped.edges()[1], NULLPTR);
    BOOST_CHECK_EQUAL(clipped.edges()[2], NULLPTR);

    // Clip above z = 0.0.  This should be the whole triangle.
    clipped = ctri.partAbove(Z,0.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 3);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 3);

    CHECK_CLOSE(clipped.vertices()[0], o);
    CHECK_CLOSE(clipped.vertices()[1], xy);
    CHECK_CLOSE(clipped.vertices()[2], y);

    BOOST_CHECK_EQUAL(clipped.edges()[0], &tri.edgeSensitivity(0));
    BOOST_CHECK_EQUAL(clipped.edges()[1], &tri.edgeSensitivity(1));
    BOOST_CHECK_EQUAL(clipped.edges()[2], &tri.edgeSensitivity(2));
}

BOOST_AUTO_TEST_CASE( ClipTwice )
{
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    SimpleMesh::Triangle triTwo(SimpleMesh::Test::oneTri(o, 2*xy, 2*y));
    ClippedTriangle ctri(triTwo);
    ClippedTriangle clipped;

    // CLIP X < 1.0
    clipped = ctri.partBelow(X, 1.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 4);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 4);
    CHECK_CLOSE(clipped.vertices()[0], o);
    CHECK_CLOSE(clipped.vertices()[1], xy);
    CHECK_CLOSE(clipped.vertices()[2], xy + y);
    CHECK_CLOSE(clipped.vertices()[3], 2*y);

    BOOST_CHECK_EQUAL(clipped.edges()[0], &triTwo.edgeSensitivity(0));
    BOOST_CHECK_EQUAL(clipped.edges()[1], NULLPTR);
    BOOST_CHECK_EQUAL(clipped.edges()[2], &triTwo.edgeSensitivity(1));
    BOOST_CHECK_EQUAL(clipped.edges()[3], &triTwo.edgeSensitivity(2));

    // CLIP Y < 1.0
    clipped = clipped.partBelow(Y, 1.0);
    BOOST_CHECK_EQUAL(clipped.vertices().size(), 3);
    BOOST_CHECK_EQUAL(clipped.edges().size(), 3);
    CHECK_CLOSE(clipped.vertices()[0], o);
    CHECK_CLOSE(clipped.vertices()[1], xy);
    CHECK_CLOSE(clipped.vertices()[2], y);

    BOOST_CHECK_EQUAL(clipped.edges()[0], &triTwo.edgeSensitivity(0));
    BOOST_CHECK_EQUAL(clipped.edges()[1], NULLPTR);
    BOOST_CHECK_EQUAL(clipped.edges()[2], &triTwo.edgeSensitivity(2));
}

BOOST_AUTO_TEST_CASE( Speed )
{
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    SimpleMesh::Triangle tri(SimpleMesh::Test::oneTri(o, xy, y));
    ClippedTriangle ctri(tri);
    ClippedTriangle clipped;

    int numRepetitions = 100;
    int numPositions = 101;
    double delta = 1.0/(numPositions-1);

    TimePoint t0 = now();
    for (int repetition = 0; repetition < numRepetitions; repetition++)
    for (int axis = X; axis <= Z; axis++)
    for (double planePos = 0.0; planePos <= 1.0; planePos += delta)
    {
        clipped = ctri.partAbove(axis, planePos);
        clipped = ctri.partBelow(axis, planePos);
    }
    double sec = elapsedSeconds(t0, now());

    cout << "Total clips: " << 3*numRepetitions*numPositions*2
        << " in " << sec << " seconds.\n";
    cout << 3*numRepetitions*numPositions*2/sec << " calls/sec.\n";
    cout << sec/(3*numRepetitions*numPositions*2) << " sec/call.\n";
}

BOOST_AUTO_TEST_CASE( AreaAndVolume )
{
    Vector3d o(0,0,0), x(1,0,0), y(0,1,0), z(0,0,1), xy(1,1,0);
    SimpleMesh::Triangle tri(SimpleMesh::Test::oneTri(o + z/2, 2*x + z/2, 2*y + z/2));
    ClippedTriangle ctri(tri);
    ctri = ctri.partBelow(X, 1.0);
    ctri = ctri.partBelow(Y, 1.0);

    // ctri should now be the square [0, 1]x[0, 1], elevated 0.5 along z

    BOOST_CHECK_CLOSE(ctri.area2d(Z), 1.0, 1e-10);
    BOOST_CHECK_CLOSE(ctri.volume(Z), 0.5, 1e-10);
}


//---------------------------------------------------------------------------//
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_SHA3_PADDING_HPP
#define CRYPTO3_SHA3_PADDING_HPP

#include <boost/crypto3/hash/detail/sha3/sha3_policy.hpp>
#include <boost/crypto3/detail/inject.hpp>
#include <boost/crypto3/detail/unbounded_shift.hpp>

namespace boost {
    namespace crypto3 {
        namespace hashes {
            namespace detail {
                template<typename Hash>
                class sha3_padding {
                    typedef Hash policy_type;

                    constexpr static const std::size_t word_bits = policy_type::word_bits;
                    typedef typename policy_type::word_type word_type;

                    constexpr static const std::size_t state_bits = policy_type::state_bits;
                    constexpr static const std::size_t state_words = policy_type::state_words;
                    typedef typename policy_type::state_type state_type;

                    constexpr static const std::size_t block_bits = policy_type::block_bits;
                    constexpr static const std::size_t block_words = policy_type::block_words;
                    typedef typename policy_type::block_type block_type;

                    constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                    typedef typename policy_type::digest_type digest_type;

                    typedef ::boost::crypto3::detail::injector<stream_endian::big_octet_little_bit, word_bits,
                                                             block_words, block_bits>
                        injector_type;

                    bool is_last;

                public:
                    sha3_padding() : is_last(true) {
                    }

                    bool is_last_block() const {
                        return is_last;
                    }

                    void operator()(block_type &block, std::size_t &block_seen) {
                        using namespace boost::crypto3::detail;

                        if ((block_bits - block_seen) > 3) {
                            /*if (block_seen % octet_bits) {
                                pack<stream_endian::big_octet_big_bit, stream_endian::big_octet_little_bit,
                                word_bits, word_bits>(block.begin(), block.end(), block.begin());
                            }*/
                            // pad 011
                            injector_type::inject(unbounded_shr(high_bits<word_bits>(~word_type(), 2), 5), 3, block,
                                                  block_seen);
                            // pad 0*
                            block_type zeros;
                            std::fill(zeros.begin(), zeros.end(), 0);
                            injector_type::inject(zeros, block_bits - 1 - block_seen, block, block_seen);
                            // pad 1
                            injector_type::inject(unbounded_shr(high_bits<word_bits>(~word_type(), 1), 7), 1, block,
                                                  block_seen);
                        }

                        else {
                            is_last = false;
                            std::size_t ind = block_bits - block_seen - 1;
                            block[block_words - 1] &= ~high_bits<word_bits>(~word_type(), ind + 1);
                            block[block_words - 1] |= high_bits<word_bits>(~word_type(), ind);
                        }
                    }

                    void process_last(block_type &block, std::size_t &block_seen) {
                        using namespace boost::crypto3::detail;

                        std::size_t ind = 3 - (block_bits - block_seen);
                        // Insert remaining padding bits
                        injector_type::inject(low_bits<word_bits>(~word_type(), ind), ind, block, block_seen);
                        // pad 0*
                        block_type zeros;
                        std::fill(zeros.begin(), zeros.end(), 0);
                        injector_type::inject(zeros, block_bits - 1 - ind, block, block_seen);
                        // pad 1
                        injector_type::inject(unbounded_shr(high_bits<word_bits>(~word_type(), 1), 7), 1, block,
                                              block_seen);
                    }
                };
            }    // namespace detail
        }        // namespace hashes
    }            // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_SHA3_PADDING_HPP


/***********************************************************************/
/**                                                                    */
/** astar.hpp                                                          */
/**                                                                    */
/** Copyright (c) 2016, Service Robotics Lab.                          */
/**                     http://robotics.upo.es                         */
/**                                                                    */
/** All rights reserved.                                               */
/**                                                                    */
/** Authors:                                                           */
/** Ignacio Perez-Hurtado (maintainer)                                 */
/** Jesus Capitan                                                      */
/** Fernando Caballero                                                 */
/** Luis Merino                                                        */
/**                                                                    */
/** This software may be modified and distributed under the terms      */
/** of the BSD license. See the LICENSE file for details.              */
/**                                                                    */
/** http://www.opensource.org/licenses/BSD-3-Clause                    */
/**                                                                    */
/***********************************************************************/

#ifndef _ASTAR_HPP_
#define _ASTAR_HPP_

#include <vector>
#include <list>
#include <unordered_set>
#include <unordered_map>
#include <limits>
#include <tinyxml.h>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>

namespace utils
{
class AStar
{
public:
  AStar(const std::string& file, double threshold = 10);
  AStar(double threshold = 10) : threshold(threshold * threshold)
  {
  }
  void init(const std::string& file);
  void addNode(const std::string& id, double x, double y, double r, bool goal = false);
  void addEdge(const std::string& id0, const std::string& id1);
  std::string& getClosestNode(double x, double y, std::string& id) const;
  void getPos(const std::string& id, double& x, double& y, double& radius) const
  {
    x = nodes.at(id).x;
    y = nodes.at(id).y;
    radius = nodes.at(id).radius;
  }
  std::list<std::string>& getPath(const std::string& start_id, const std::string& goal_id,
                                  std::list<std::string>& path);
  const std::vector<std::string>& getGoals() const
  {
    return goals;
  }

private:
  void init(TiXmlNode* pParent);
  void precompute();

  struct Node
  {
    std::string id;
    double x;
    double y;
    double radius;
    std::unordered_map<std::string, double> neighbors;
    std::unordered_map<std::string, std::list<std::string> > paths;
    double gScore;
    double fScore;
    Node* cameFrom;
  };

  void addEdge(const std::string& id0, const std::string& id1, double cost);

  double heuristic_cost_estimate(const Node* n0, const Node* n1) const
  {
    return (n0->x - n1->x) * (n0->x - n1->x) + (n0->y - n1->y) * (n0->y - n1->y);
  }

  double threshold;
  std::unordered_map<std::string, Node> nodes;
  std::vector<std::string> goals;
};

inline AStar::AStar(const std::string& file, double threshold)
  : threshold(threshold * threshold)
{
  init(file);
}

inline void AStar::init(const std::string& file)
{
  TiXmlDocument xml(file);
  if (xml.LoadFile())
  {
    init(&xml);
  }
  precompute();
}

inline void AStar::precompute()
{
  for (auto it = nodes.begin(); it != nodes.end(); ++it)
  {
    for (unsigned i = 0; i < goals.size(); i++)
    {
      getPath(it->first, goals[i], it->second.paths[goals[i]]);
    }
  }
}

inline void AStar::init(TiXmlNode* pParent)
{
  if (!pParent)
    return;

  if (pParent->Type() == TiXmlNode::TINYXML_ELEMENT)
  {
    TiXmlElement* pElement = pParent->ToElement();
    TiXmlAttribute* pAttrib = pElement->FirstAttribute();
    if (strcmp(pParent->Value(), "waypoint") == 0 || strcmp(pParent->Value(), "goal") == 0)
    {
      double x, y, r;
      std::string id;
      id.assign(pAttrib->Value());
      pAttrib = pAttrib->Next();
      pAttrib->QueryDoubleValue(&x);
      pAttrib = pAttrib->Next();
      pAttrib->QueryDoubleValue(&y);
      pAttrib = pAttrib->Next();
      pAttrib->QueryDoubleValue(&r);
      addNode(id, x, y, r, strcmp(pParent->Value(), "goal") == 0);
    }
    else if (strcmp(pParent->Value(), "edge") == 0)
    {
      std::string from;
      from.assign(pAttrib->Value());
      pAttrib = pAttrib->Next();
      std::string to;
      to.assign(pAttrib->Value());
      addEdge(from, to);
    }
  }
  for (TiXmlNode* pChild = pParent->FirstChild(); pChild != 0; pChild = pChild->NextSibling())
  {
    init(pChild);
  }
}

inline void AStar::addNode(const std::string& id, double x, double y, double radius, bool goal)
{
  Node& n = nodes[id];
  n.id = id;
  n.x = x;
  n.y = y;
  n.radius = radius;
  if (goal)
  {
    goals.push_back(id);
  }
}

inline std::string& AStar::getClosestNode(double x, double y, std::string& id) const
{
  id = "";
  double min = std::numeric_limits<double>::infinity();
  for (auto it = nodes.begin(); it != nodes.end(); ++it)
  {
    double dis =
        (x - it->second.x) * (x - it->second.x) + (y - it->second.y) * (y - it->second.y);
    if (dis < min)
    {
      min = dis;
      id = it->first;
    }
  }
  return id;
}

inline void AStar::addEdge(const std::string& id0, const std::string& id1)
{
  Node& src = nodes.at(id0);
  Node& dst = nodes.at(id1);
  double cost = heuristic_cost_estimate(&src, &dst);
  if (cost > threshold)
  {
    std::string mid_id = boost::uuids::to_string(boost::uuids::random_generator()());
    addNode(mid_id, (src.x + dst.x) * 0.5, (src.y + dst.y) * 0.5, src.radius, false);
    addEdge(id0, mid_id);
    addEdge(mid_id, id1);
  }
  else
  {
    addEdge(id0, id1, cost);
    addEdge(id1, id0, cost);
  }
}

inline void AStar::addEdge(const std::string& id0, const std::string& id1, double cost)
{
  nodes[id0].neighbors[id1] = cost;
}

inline std::list<std::string>& AStar::getPath(const std::string& start_id,
                                              const std::string& goal_id,
                                              std::list<std::string>& path)
{
  Node* start_node = &nodes.at(start_id);
  Node* goal_node = &nodes.at(goal_id);
  path.clear();
  if (start_node->paths.count(goal_id) > 0 && start_node->paths.at(goal_id).size() > 0)
  {
    path = start_node->paths.at(goal_id);
    return path;
  }
  std::unordered_set<std::string> closedSet;
  std::unordered_map<std::string, Node*> openSet;
  for (auto it = nodes.begin(); it != nodes.end(); ++it)
  {
    it->second.gScore = std::numeric_limits<double>::infinity();
    it->second.fScore = std::numeric_limits<double>::infinity();
    it->second.cameFrom = NULL;
  }

  start_node->gScore = 0;
  start_node->fScore = heuristic_cost_estimate(start_node, goal_node);
  openSet[start_id] = start_node;
  while (!openSet.empty())
  {
    double min = std::numeric_limits<double>::infinity();
    auto current_it = openSet.begin();
    for (auto it = openSet.begin(); it != openSet.end(); ++it)
    {
      if (it->second->fScore < min)
      {
        current_it = it;
        min = it->second->fScore;
      }
    }
    std::string current_id = current_it->first;

    Node* current_node = current_it->second;
    if (current_id == goal_id)
    {
      path.push_front(current_id);
      while (current_node->cameFrom != NULL)
      {
        current_node = current_node->cameFrom;
        path.push_front(current_node->id);
      }
      return path;
    }
    openSet.erase(current_it);
    closedSet.insert(current_id);
    for (auto it = current_node->neighbors.begin(); it != current_node->neighbors.end(); ++it)
    {
      std::string neighbor_id = it->first;
      if (closedSet.count(neighbor_id) > 0)
      {
        continue;
      }
      double neighbor_cost = it->second;
      double tentative_gScore = current_node->gScore + neighbor_cost;
      Node* neighbor_node = &nodes.at(neighbor_id);
      if (openSet.count(neighbor_id) == 0)
      {
        openSet[neighbor_id] = neighbor_node;
      }
      else if (tentative_gScore >= neighbor_node->gScore)
      {
        continue;
      }
      neighbor_node->cameFrom = current_node;
      neighbor_node->gScore = tentative_gScore;
      neighbor_node->fScore =
          tentative_gScore + heuristic_cost_estimate(neighbor_node, goal_node);
    }
  }
  return path;
}
}

#endif