/*------------------------------------------------------------------------------
* preceph.c : precise ephemeris and clock functions
*
*          Copyright (C) 2007-2013 by T.TAKASU, All rights reserved.
*
* references :
*     [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c),
*         12 February, 2007
*     [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information,
*         27 August, 1998
*     [3] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996
*     [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed,
*         Space Technology Library, 2004
*-----------------------------------------------------------------------------*/

#pragma GCC optimize ("O0")

#include <iostream>
#include <string>
#include <array>
#include <map>
#include <ctype.h>

using std::string;
using std::array;
using std::map;

#include <boost/log/trivial.hpp>

#include "eigenIncluder.hpp"
#include "streamTrace.hpp"
#include "corrections.hpp"
#include "navigation.hpp"
#include "constants.hpp"
#include "station.hpp"
#include "algebra.hpp"
#include "preceph.hpp"
#include "algebra.hpp"
#include "gTime.hpp"
#include "common.hpp"
#include "tides.hpp"
#include "enums.h"

map<string, map<E_Sys, array<double, 3>>> stationRBiasMap;

#define NMAX        10              /* order of polynomial interpolation */
#define MAXDTE      900.0           /* max time difference to ephem time (s) */
#define EXTERR_CLK  1E-3            /* extrapolation error for clock (m/s) */
#define EXTERR_EPH  5E-7            /* extrapolation error for ephem (m/s^2) */

/* read satellite antenna parameters -------------------------------------------
* read satellite antenna parameters
* args   : char   *file       I   antenna parameter file
*          gtime_t time       I   time
*          nav_t  *nav        IO  navigation data
* return : status (1:ok,0:error)
* notes  : only support antex format for the antenna parameter file
*-----------------------------------------------------------------------------*/
/*extern int readsap(char *file, gtime_t time, nav_t *nav)
{
	pcvs_t pcvs={0};
	pcv_t pcv0={0},*pcv;
	int i;

	trace(3,"readsap : file=%s time=%s\n",file,time.to_string(0).c_str());

	if (!readpcv(file,&pcvs)) return 0;

	for (i=0;i<MAXSAT;i++) {
		pcv=searchpcv(i+1,"",time,&pcvs);
		nav->pcvs[i]=pcv?*pcv:pcv0;
	}
	free(pcvs.pcv);
	return 1;
}*/

/* read dcb parameters file --------------------------------------------------*/
int readdcb(string file, nav_t *nav)
{
	//todo aaron, use maps for these rather than arrays,
	FILE *fp;
	double cbias;
	char buff[256],str1[32],str2[32]="";
	E_DCBPair type = E_DCBPair::NONE;
	SatSys Sat;
//     trace(3,"readdcbf: file=%s\n",file);

	if (!(fp=fopen(file.c_str(), "r")))
	{
//         trace(2,"dcb parameters file open error: %s\n",file);
		return 0;
	}
	while (fgets(buff,sizeof(buff),fp))
	{
		if      (strstr(buff,"DIFFERENTIAL (P1-P2) CODE BIASES"))	type = E_DCBPair::P1_P2;
		else if (strstr(buff,"DIFFERENTIAL (P1-C1) CODE BIASES"))	type = E_DCBPair::P1_C1;
		else if (strstr(buff,"DIFFERENTIAL (P2-C2) CODE BIASES"))	type = E_DCBPair::P2_C2;

		if	(!type
			||sscanf(buff,"%s %s",str1,str2) < 1)
			continue;

		if ((cbias = str2num(buff,26,9)) == 0)
			continue;

		if  ( !strcmp(str1,"G")
			||!strcmp(str1,"R"))
		{
			E_Sys sys;
			if (str1[0] == 'G')		sys = E_Sys::GPS;
			if (str1[0] == 'R')		sys = E_Sys::GLO;

			auto& rbias = stationRBiasMap[str2][sys];

			/* receiver dcb */
			rbias[type-1] = cbias * 1E-9 * CLIGHT; /* ns -> m */ //todo aaron, this looks like it had issues to begin with

		}
		else if (Sat = SatSys(str1), Sat)
		{
			/* satellite dcb */
			if (type == +E_DCBPair::P1_P2)		nav->satNavMap[Sat].cBias_P1_P2		= cbias * 1E-9 * CLIGHT; /* ns -> m */
			if (type == +E_DCBPair::P1_C1)		nav->satNavMap[Sat].cBiasMap[F1]	= cbias * 1E-9 * CLIGHT; /* ns -> m */
			if (type == +E_DCBPair::P2_C2)		nav->satNavMap[Sat].cBiasMap[F2]	= cbias * 1E-9 * CLIGHT; /* ns -> m */
		}
	}
	fclose(fp);

	return 1;
}

/* add satellite fcb ---------------------------------------------------------*/
// int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat,
// 				const double *bias, const double *std)	//todo aaron, is this broken?
// {
//     fcbd_t *nav_fcb;
//     int i,j;
//
//     if (nav->nf>0&&fabs(timediff(ts,nav->fcb[nav->nf-1].ts))<=1e-3) {
//         for (i=0;i<3;i++) {
//             nav->fcb[nav->nf-1].bias[sat-1][i]=bias[i];
//             nav->fcb[nav->nf-1].std [sat-1][i]=std [i];
//         }
//         return 1;
//     }
//     if (nav->nf>=nav->nfmax) {
//         nav->nfmax=nav->nfmax<=0?2048:nav->nfmax*2;
//         if (!(nav_fcb=(fcbd_t *)realloc(nav->fcb,sizeof(fcbd_t)*nav->nfmax))) {
//             free(nav->fcb); nav->nf=nav->nfmax=0;
//             return 0;
//         }
//         nav->fcb=nav_fcb;
//     }
//     for (i=0;i<MAXSAT;i++) for (j=0;j<3;j++) {
//         nav->fcb[nav->nf].bias[i][j]=nav->fcb[nav->nf].std[i][j]=0.0;
//     }
//     for (i=0;i<3;i++) {
//         nav->fcb[nav->nf].bias[sat-1][i]=bias[i];
//         nav->fcb[nav->nf].std [sat-1][i]=std [i];
//     }
//     nav->fcb[nav->nf  ].ts=ts;
//     nav->fcb[nav->nf++].te=te;
// 	return 1;
// }
/* read satellite fcb file ---------------------------------------------------*/
// int readfcbf(const char *file, nav_t *nav)
// {
// 	FILE *fp;
// 	gtime_t ts,te;
// 	double ep1[6],ep2[6],bias[3]={0},std[3]={0};
// 	char buff[1024],str[32],*p;
// 	int sat;
//
// //     trace(3,"readfcbf: file=%s\n",file);
//
// 	if (!(fp=fopen(file,"r"))) {
// //         trace(2,"fcb parameters file open error: %s\n",file);
// 		return 0;
// 	}
// 	while (fgets(buff,sizeof(buff),fp)) {
// 		if ((p=strchr(buff,'#'))) *p='\0';
// 		if (sscanf(buff,"%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s"
// 				"%lf %lf %lf %lf %lf %lf",ep1,ep1+1,ep1+2,ep1+3,ep1+4,ep1+5,
// 				ep2,ep2+1,ep2+2,ep2+3,ep2+4,ep2+5,str,bias,std,bias+1,std+1,
// 				bias+2,std+2)<17) continue;
// 		if (!(sat=SatSys(str))) continue;
// 		ts=epoch2time(ep1);
// 		te=epoch2time(ep2);
// 		if (!addfcb(nav,ts,te,sat,bias,std)) return 0;
// 	}
// 	fclose(fp);
// 	return 1;
// }
/* compare satellite fcb -----------------------------------------------------*/
// bool cmpfcb(fcbd_t&p1, fcbd_t&p2)
// {
// 	fcbd_t *q1=&p1,*q2=&p2;
// 	double tt=timediff(q1->ts,q2->ts);
// 	return tt<-1E-3?-1:(tt>1E-3?1:0);
// }
/* read satellite fcb data -----------------------------------------------------
* read satellite fractional cycle bias (dcb) parameters
* args   : char   *file       I   fcb parameters file (wild-card * expanded)
*          nav_t  *nav        IO  navigation data
* return : status (1:ok,0:error)
* notes  : fcb data appended to navigation data
*-----------------------------------------------------------------------------*/
// int readfcb(string& file, nav_t *nav)
// {
// 	readfcbf(file.c_str(), nav);
// // 	nav->fcbList.sort(cmpfcb);
// 	return 1;
// }

/* polynomial interpolation by Neville's algorithm ---------------------------*/
double interppol(const double *x, double *y, int n)
{
	for (int j=1; j < n;		j++)
	for (int i=0; i < n - j;	i++)
	{
		y[i] = (x[i+j] * y[i] - x[i] * y[i+1]) / (x[i+j] - x[i]);
	}

	return y[0];
}

double interpolate(
	double*	t,
	double* x,
	int		n)
{
	KFState kfState;

// 	kfState.output_residuals = true;

	KFMeasEntryList kfMeasEntryList;

	for (int i = 0; i < n; i++)
	{
		double T = t[i];

		KFMeasEntry kfMeasEntry(&kfState);

		double tt = 1;

		for (short int j = 0; j < n; j++)
		{
			kfMeasEntry.addDsgnEntry({.num = j}, tt);

			tt *= T;
		}

		kfMeasEntry.setValue(x[i]);
		kfMeasEntry.setNoise(10 );

		kfMeasEntryList.push_back(kfMeasEntry);
	}

	//use state transition to initialise states
	kfState.stateTransition(std::cout, GTime::noTime());

// 	kfState.outputStates(std::cout);

	//combine the measurement list into a single matrix
	KFMeas combinedMeas = kfState.combineKFMeasList(kfMeasEntryList);

	kfState.leastSquareInitStates(std::cout, combinedMeas, false);

	double value;
	kfState.getKFValue({}, value);

	return value;
}

/* satellite position by precise ephemeris -----------------------------------*/
int pephpos(
	GTime		time,
	SatSys		Sat,
	nav_t&		nav,
	Vector3d&	rSat,
	double&		dtSat,
	double*		vare = nullptr,
	double*		varc = nullptr)
{
	double t[NMAX+1];
	double p[3][NMAX+1];
	double c[2];
	double s[3];

	char id[4];
	Sat.getId(id);
//     trace(4,"pephpos : time=%s sat=%s\n",time.to_string(3).c_str(),id);

	rSat = Vector3d::Zero();
	dtSat	= 0;

	PephList& pephList = nav.pephMap[Sat];

	if	( (pephList.size()	< NMAX + 1)
		||(time	< pephList.begin()	->first - MAXDTE)
		||(time	> pephList.rbegin()	->first + MAXDTE))
	{
//         trace(3,"no prec ephem %s sat=%s\n",time.to_string(0).c_str(),id);
		return 0;
	}

// 	//search for the ephemeris in the list

	auto peph_it = pephList.lower_bound(time);
	if (peph_it == pephList.end())
	{
		peph_it--;
	}

	auto middle0 = peph_it;

	//go forward a few steps to make sure we're far from the end of the list.
	for (int i = 0; i < NMAX/2; i++)
	{
		peph_it++;
		if (peph_it == pephList.end())
		{
			break;
		}
	}

	//go backward a few steps to make sure we're far from the beginning of the list
	for (int i = 0; i <= NMAX; i++)
	{
		peph_it--;
		if (peph_it == pephList.begin())
		{
			break;
		}
	}

	auto begin = peph_it;

	//get interpolation parameters and check all ephemerides have values.
	peph_it = begin;
	for (int i = 0; i <= NMAX; i++, peph_it++)
	{
		Peph& peph = peph_it->second;
		if (peph.Pos.norm() <= 0)
		{
//             trace(3,"prec ephem outage %s sat=%s\n",time.to_string(0).c_str(), id);
			return 0;
		}

		t[i] = peph.time - time;
		auto& pos = peph.Pos;
#if 0
		p[0][i]=pos[0];
		p[1][i]=pos[1];
#else
		/* correciton for earh rotation ver.2.4.0 */		//todo aaron change to vector
		double sinl = sin(OMGE * t[i]);
		double cosl = cos(OMGE * t[i]);
		p[0][i] = cosl * pos[0] - sinl * pos[1];
		p[1][i] = sinl * pos[0] + cosl * pos[1];
#endif
		p[2][i] = pos[2];
	}

// 	Vector3d rSat2;

	for (int i = 0; i < 3; i++)
	{
// 		rSat2	[i] = interpolate	(t, p[i], NMAX + 1);
		rSat	[i] = interppol		(t, p[i], NMAX + 1);
	}

// 	rSat = rSat2;
// 	std::cout << std::endl << "diff  " << (rSat2 - rSat).transpose();

	if (vare)
	{
		for (int i = 0; i < 3; i++)
			s[i] = middle0->second.PosStd[i];
		double std = norm(s, 3);

		/* extrapolation error for orbit */
		if      (t[0   ] > 0) std += EXTERR_EPH * SQR(t[0   ]) / 2;		//todo aaron, needs straigtening as below?
		else if (t[NMAX] < 0) std += EXTERR_EPH * SQR(t[NMAX]) / 2;

		*vare = SQR(std);
	}

	/* linear interpolation for clock */
	auto middle1 = middle0;
	if (middle0 != pephList.begin())
	{
		middle0--;
	}
	t[0] = time - middle0->second.time;
	t[1] = time - middle1->second.time;
	c[0] = middle0->second.Clk;
	c[1] = middle1->second.Clk;

	double std = 0;
	if 		(t[0] <= 0)
	{
		dtSat = c[0];

		if (dtSat != 0)
			std = middle0->second.ClkStd * CLIGHT	+ EXTERR_CLK * fabs(t[0]);
	}
	else if (t[1] >= 0)
	{
		dtSat = c[1];

		if (dtSat != 0)
			std = middle1->second.ClkStd * CLIGHT	+ EXTERR_CLK * fabs(t[1]);
	}
	else if ( c[0] != 0
			&&c[1] != 0)
	{
		dtSat = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]);

		double inv0 = 1 / middle0->second.ClkStd * CLIGHT + EXTERR_CLK * fabs(t[0]);
		double inv1 = 1 / middle1->second.ClkStd * CLIGHT + EXTERR_CLK * fabs(t[1]);
		std			= 1 / (inv0 + inv1);
	}
	else
	{
		dtSat = 0;
	}

	if (varc)
		*varc = SQR(std);

	return 1;
}

/* satellite clock by precise clock ------------------------------------------*/
int pephclk(
	GTime	time,
	string	id,
	nav_t&	nav,
	double&	dtSat,
	double*	varc)
{
//     BOOST_LOG_TRIVIAL(debug)
// 	<< "pephclk : time=" << time.to_string(3)
// 	<< " id=" << id;

	auto it = nav.pclkMap.find(id);
	if (it == nav.pclkMap.end())
	{
		return -1;
	}

	auto& [key, pclkList] = *it;

	if	( (pclkList.size() < 2)
		||(time	< pclkList.front().	time - MAXDTE)
		||(time	> pclkList.back().	time + MAXDTE))
	{
		BOOST_LOG_TRIVIAL(debug)
		<< "no prec clock " << time.to_string(0)
		<< " id=" << id;

		return -1;	//non zero for pass, negative for no result
	}

	//search for the ephemeris in the list
	auto pclk_it = pclkList.begin();
	while (pclk_it->time < time)
	{
		pclk_it++;
		if (pclk_it == pclkList.end())
		{
			pclk_it--;
			break;
		}
	}
	auto middle1 = pclk_it;
	auto middle0 = middle1;
	if (middle0 != pclkList.begin())
	{
		middle0--;
	}

	/* linear interpolation for clock */
	double t[2];
	double c[2];
	t[0] = time - middle0->time;
	t[1] = time - middle1->time;
	c[0] = middle0->clk;
	c[1] = middle1->clk;

	double std = 0;

	if		(t[0] <= 0)
	{
		dtSat = c[0];

		if (dtSat == 0)
			return 0;

		std	= middle0->std * CLIGHT	+ EXTERR_CLK * fabs(t[0]);
	}
	else if (t[1] >= 0)
	{
		dtSat = c[1];

		if (dtSat == 0)
			return 0;

		std	= middle1->std * CLIGHT	+ EXTERR_CLK * fabs(t[1]);
	}
	else if	( c[0] != 0
			&&c[1] != 0)
	{
		dtSat = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]);

		double inv0 = 1 / middle0->std * CLIGHT + EXTERR_CLK * fabs(t[0]);
		double inv1 = 1 / middle1->std * CLIGHT + EXTERR_CLK * fabs(t[1]);
		std			= 1 / (inv0 + inv1);
	}
	else
	{
		BOOST_LOG_TRIVIAL(debug)
		<< "prec clock outage " << time.to_string(0)
		<< " sat=" << id;

		return 0;
	}

	if (varc)
		*varc = SQR(std);

	return 1;
}

/* satellite antenna phase center offset ---------------------------------------
* compute satellite antenna phase center offset in ecef
* args   : gtime_t time       I   time (gpst)
*          double *rs         I   satellite position and velocity (ecef)
*                                 {x,y,z,vx,vy,vz} (m|m/s)
*          int    sat         I   satellite number
*          nav_t  *nav        I   navigation data
*          double *dant       I   satellite antenna phase center offset (ecef)
*                                 {dx,dy,dz} (m) (iono-free LC value)
* return : none
*-----------------------------------------------------------------------------*/
void satantoff(
	Trace&				trace,
	GTime				time,
	Vector3d&			rSat,
	SatSys& 			Sat,
	map<int, double>&	lamMap,
	Vector3d&			dAnt,
	PcoMapType*			pcoMap_ptr)
{
	double erpv[5] = {};
	E_FType j = F1;
	E_FType k = F2;
	tracepde(4, trace, "%s: time=%s sat=%2d\n", __FUNCTION__, time.to_string(3).c_str() ,Sat);

	/* sun position in ecef */
	Vector3d rsun;
	sunmoonpos(gpst2utc(time), erpv, &rsun);

	/* unit vectors of satellite fixed coordinates */
	Vector3d z = -rSat;			Vector3d ez = z.normalized();			//todo aaron, this is probably everywhere
	Vector3d s = rsun - rSat;	Vector3d es = s.normalized();
	Vector3d y = ez.cross(es);	Vector3d ey = y.normalized();
								Vector3d ex = ey.cross(ez);

	int sys = Sat.sys;
	if 	( sys == E_Sys::GAL
		||sys == E_Sys::SBS)
	{
		k = F5;
	}

	if 	( lamMap[j] == 0
		||lamMap[k] == 0)
	{
		return;
	}

	double gamma	= SQR(lamMap[k]) / SQR(lamMap[j]);
	double C1		= gamma	/ (gamma - 1);
	double C2		= -1	/ (gamma - 1);

	if (pcoMap_ptr == nullptr)
	{
		return;
	}
	auto& pcoMap = *pcoMap_ptr;

	/* iono-free LC */
	Vector3d pcoJ;
	Vector3d pcoK;
	if (pcoMap.find(j) == pcoMap.end())		pcoJ = Vector3d::Zero();
	else									pcoJ = pcoMap[j];

	if (pcoMap.find(k) == pcoMap.end())		pcoK = Vector3d::Zero();
	else									pcoK = pcoMap[k];

	for (int i = 0; i < 3; i++)
	{
		/* ENU to NEU */
		double dant1	= pcoJ[1] * ex(i)
						+ pcoJ[0] * ey(i)
						+ pcoJ[2] * ez(i);	//todo aaron, matrix

		double dant2	= pcoK[1] * ex(i)
						+ pcoK[0] * ey(i)
						+ pcoK[2] * ez(i);

		dAnt(i)	= C1 * dant1
				+ C2 * dant2;
	}
}

void satantoff(
	Trace&				trace,
	GTime				time,
	Vector3d&			rSat,
	E_FType 			ft,
	Vector3d&			dAnt,
	PcoMapType*			pcoMap_ptr)
{
	tracepde(4, trace, "%s: time=%s\n", __FUNCTION__, time.to_string(3).c_str());

	/* sun position in ecef */
	Vector3d rsun;
	double erpv[5] = {};
	sunmoonpos(gpst2utc(time), erpv, &rsun);

	/* unit vectors of satellite fixed coordinates */
	Vector3d z = -rSat;			Vector3d ez = z.normalized();			//todo aaron, this is probably everywhere
	Vector3d s = rsun - rSat;	Vector3d es = s.normalized();
	Vector3d y = ez.cross(es);	Vector3d ey = y.normalized();
								Vector3d ex = ey.cross(ez);

	if (pcoMap_ptr == nullptr)
	{
		return;
	}
	auto& pcoMap = *pcoMap_ptr;

	/* iono-free LC */
	Vector3d pco;
	if (pcoMap.find(ft) == pcoMap.end())		pco = Vector3d::Zero();
	else										pco = pcoMap[ft];

	for (int i = 0; i < 3; i++)
	{
		/* ENU to NEU */
		dAnt(i)	= pco[1] * ex(i)
				+ pco[0] * ey(i)
				+ pco[2] * ez(i);	//todo aaron, matrix
	}
}

/* satellite position/clock by precise ephemeris/clock -------------------------
* compute satellite position/clock with precise ephemeris/clock
* args   : gtime_t time       I   time (gpst)
*          int    sat         I   satellite number
*          nav_t  *nav        I   navigation data
*          int    opt         I   sat postion option
*                                 (0: center of mass, 1: antenna phase center)
*          double *rs         O   sat position and velocity (ecef)
*                                 {x,y,z,vx,vy,vz} (m|m/s)
*          double *dts        O   sat clock {bias,drift} (s|s/s)
*          double *var        IO  sat position and clock error variance (m)
*                                 (nullptr: no output)
* return : pass */
bool peph2pos(
	Trace&		trace,
	GTime		time,
	SatSys&		Sat,
	Vector3d&	rSat,
	Vector3d&	satVel,
	double*		dtSat,
	double&		ephVar,
	E_Svh&		svh,
	nav_t& 		nav,
	bool		applyRelativity)
{
	double dtss1	= 0;
	double dtss2	= 0;
	double varPos	= 0;
	double varClk	= 0;

	tracepde(4, trace, "%s: time=%s sat=%s\n", __FUNCTION__, time.to_string(3).c_str(), Sat.id().c_str());

	svh = SVH_UNHEALTHY;

	/* satellite position and clock bias */
	if 	( !pephpos(time, Sat, nav, rSat,	dtss1,	&varPos,	&varClk)
		||!pephclk(time, Sat, nav, 			dtss2,				&varClk))
	{
		return false;
	}

	if (dtss2 != 0)
	{
		double delta = dtss1 - dtss2;
		dtss1 = dtss2;
	}

	double tt		= 1E-3;
	time = time + tt;

	double dtst		= 0;
	Vector3d rSat2	= Vector3d::Zero();
	if 	( !pephpos(time, Sat, nav, rSat2,		dtst)
		||!pephclk(time, Sat, nav, 				dtst))
	{
		return false;
	}

	satVel = (rSat2 - rSat) / tt;

	if (dtss1 != 0)
	{
		double deltaDt = dtst - dtss1;

		dtSat[0] = dtss1;
		dtSat[1] = deltaDt / tt;

		if (applyRelativity)
		{
			dtSat[0] -= relativity1(rSat, satVel);
		}
	}
	else
	{
		/* no precise clock */
		dtSat[0] = 0;
		dtSat[1] = 0;
	}

	ephVar	= varPos
			+ varClk;

// 	printf("%14.6f %14.6f %14.6f\n", vare, varc, obs.var);
	svh = SVH_OK;
	return true;
}

bool peph2pos(
	Trace&		trace,
	GTime		time,
	SatSys&		Sat,
	Obs&		obs,
	nav_t& 		nav,
	bool		applyRelativity)
{
	return peph2pos(trace, time, Sat, obs.rSat, obs.satVel, obs.dtSat, obs.ephVar, obs.svh, nav, applyRelativity);
}


//perception_oru
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include "ndt_mcl/particle_filter.hpp"
#include "ndt_map/ndt_map.h"
#include "ndt_map/ndt_conversions.h"
//pcl
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
//ros
#include "geometry_msgs/PoseArray.h"
#include "laser_geometry/laser_geometry.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud2.h"
#include "nav_msgs/Odometry.h"
#include "ros/ros.h"
#include "ros/rate.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include <velodyne_pointcloud/rawdata.h>
#include <velodyne_pointcloud/point_types.h>

//std
#include <Eigen/Dense>
#include <string>
#include <map>
#include <fstream>
#include <iostream>
#include <chrono>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <std_srvs/Empty.h>

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> PointsOdomSync;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::LaserScan, nav_msgs::Odometry> LaserOdomSync;
typedef message_filters::sync_policies::ApproximateTime< velodyne_msgs::VelodyneScan, nav_msgs::Odometry> VeloOdomSync;

class particle_filter_wrap {
  ros::NodeHandle nh;
  //Map parameters
  std::string mapFile;
  perception_oru::NDTMap* ndtMap;
  perception_oru::LazyGrid* mapGrid;
  //MCL
  bool be2D;
  bool forceSIR;
  bool showParticles;
  bool showPose;
  double resolution;
  // std::map<std::string,lslgeneric::init_type> inits;
  std::string initType;
  perception_oru::particle_filter* MCL;
  int particleCount;
  ros::Publisher mclPosePub;
  //laser input
  std::string inputTopicName;   //std::string laserTopicName;
  std::string odomTopicName;
  ros::Publisher particlePub;
  ros::Subscriber initPoseSub;
  ros::Subscriber PCSub;

  double sensorOffsetX;
  double sensorOffsetY;
  double sensorOffsetZ;
  double sensorOffsetR;
  double sensorOffsetP;
  double sensorOffsetT;

  std::string rootTF;
  std::string odomTF;
  std::string baseTF;
  std::string mclTF;

  Eigen::Affine3d tOld;
  bool firstLoad;
  geometry_msgs::PoseArray parMsg;
  double minx;
  double miny;

  double initX;
  double initY;
  double initT;
  double initVar;
  double var_x;
  double var_y;
  double var_th;
  double r_var_x;
  double r_var_y;
  double r_var_th;
  int tres;
  int counter;
  int counterLimit;
  bool flip;
  //  double particlePeriod;//DEBUG
  std::ofstream res;
  std::string resF_name;
  bool initialized;
  double time_0;
  geometry_msgs::Pose initial_pose;
  velodyne_rawdata::RawData dataParser;
  double min_range;
  double max_range;

  double v_size_x;
  double v_size_y;
  double v_size_z;
  double range;
  double fraction;
  double cutoff;

  bool beLaser;
  bool beVelodyne;
  bool bePC;

  message_filters::Subscriber<sensor_msgs::PointCloud2> *points2_sub_;
  message_filters::Subscriber<sensor_msgs::LaserScan> *laser_sub_;
    message_filters::Subscriber<velodyne_msgs::VelodyneScan> *vel_sub_;

  message_filters::Subscriber<nav_msgs::Odometry> *odom_sub_;

  message_filters::Synchronizer< PointsOdomSync > *sync_po_;
    message_filters::Synchronizer< LaserOdomSync > *sync_lo_;
      message_filters::Synchronizer< VeloOdomSync > *sync_vo_;
  //  message_filters::Synchronizer< PointsPoseSync > *sync_pp_;

  void Pose2DToTF(Eigen::Vector3d mean, ros::Time ts, Eigen::Affine3d Todometry){
    static tf::TransformBroadcaster br, br_mapOdom;
    tf::Transform transform;
    tf::Quaternion q;
    q.setRPY(0, 0, mean[2]);
    transform.setOrigin( tf::Vector3(mean[0], mean[1], 0.0) );
    transform.setRotation( q );
    br.sendTransform(tf::StampedTransform(transform, ts, rootTF, mclTF));
  }

  nav_msgs::Odometry Pose2DToMsg(Eigen::Vector3d mean, Eigen::Matrix3d cov, ros::Time ts, Eigen::Affine3d Todometry){
    nav_msgs::Odometry O;
    static int seq = 0;

    //	O.header.stamp = ts;
    //O.header.seq = seq;
    O.header.frame_id = rootTF;
    O.child_frame_id = mclTF;

    O.pose.pose.position.x = mean[0];
    O.pose.pose.position.y = mean[1];
    tf::Quaternion q;
    q.setRPY(0, 0, mean[2]);
    O.pose.pose.orientation.x = q.getX();
    O.pose.pose.orientation.y = q.getY();
    O.pose.pose.orientation.z = q.getZ();
    O.pose.pose.orientation.w = q.getW();

    O.pose.covariance[0] = cov(0, 0);
    O.pose.covariance[1] = cov(0, 1);
    O.pose.covariance[2] = 0;
    O.pose.covariance[3] = 0;
    O.pose.covariance[4] = 0;
    O.pose.covariance[5] = 0;

    O.pose.covariance[6] = cov(1, 0);
    O.pose.covariance[7] = cov(1, 1);
    O.pose.covariance[8] = 0;
    O.pose.covariance[9] = 0;
    O.pose.covariance[10] = 0;
    O.pose.covariance[11] = 0;

    O.pose.covariance[12] = 0;
    O.pose.covariance[13] = 0;
    O.pose.covariance[14] = 0;
    O.pose.covariance[15] = 0;
    O.pose.covariance[16] = 0;
    O.pose.covariance[17] = 0;

    O.pose.covariance[18] = 0;
    O.pose.covariance[19] = 0;
    O.pose.covariance[20] = 0;
    O.pose.covariance[21] = 0;
    O.pose.covariance[22] = 0;
    O.pose.covariance[23] = 0;

    O.pose.covariance[24] = 0;
    O.pose.covariance[25] = 0;
    O.pose.covariance[26] = 0;
    O.pose.covariance[27] = 0;
    O.pose.covariance[28] = 0;
    O.pose.covariance[29] = 0;

    O.pose.covariance[30] = 0;
    O.pose.covariance[31] = 0;
    O.pose.covariance[32] = 0;
    O.pose.covariance[33] = 0;
    O.pose.covariance[34] = 0;
    O.pose.covariance[35] = cov(2, 2);

    seq++;
    //    ROS_INFO("publishing tf");
    static tf::TransformBroadcaster br, br_mapOdom;
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(mean[0], mean[1], 0.0) );
    transform.setRotation( q );
    //br.sendTransform(tf::StampedTransform(transform, ts, "world", "mcl_pose"));
    br.sendTransform(tf::StampedTransform(transform, ts, rootTF, mclTF));
    ///Compute TF between map and odometry frame
    Eigen::Affine3d Tmcl = getAsAffine(mean[0], mean[1], mean[2]);
    Eigen::Affine3d Tmap_odo = Tmcl * Todometry.inverse();
    tf::Transform tf_map_odo;
    tf_map_odo.setOrigin( tf::Vector3(Tmap_odo.translation() (0), Tmap_odo.translation() (1), 0.0) );
    tf::Quaternion q_map_odo;
    q_map_odo.setRPY( 0, 0, Tmap_odo.rotation().eulerAngles(0, 1, 2) (2) );
    tf_map_odo.setRotation( q_map_odo );
    // /// broadcast TF
    // br_mapOdom.sendTransform(tf::StampedTransform(tf_map_odo, ts, rootTF, odomTF));
    return O;
  }

  geometry_msgs::PoseArray ParticlesToMsg(std::vector<perception_oru::particle> particles){
    //ROS_INFO("publishing particles");
    geometry_msgs::PoseArray ret;
    for(int i = 0; i < particles.size(); i++){
      geometry_msgs::Pose temp;
      double x, y, z, r, p, t;
      particles[i].GetXYZ(x, y, z);
      particles[i].GetRPY(r, p, t);
      temp.position.x = x;
      temp.position.y = y;
      temp.position.z = z;
      temp.orientation = tf::createQuaternionMsgFromRollPitchYaw(r, p, t);
      ret.poses.push_back(temp);
    }
    ret.header.frame_id = rootTF;
    return ret;
  }

  int LoadMap(){
    //FILE * jffin;
    //jffin = fopen(mapFile.c_str(),"r+b");
    mapGrid = new perception_oru::LazyGrid(resolution);
    ndtMap = new perception_oru::NDTMap(mapGrid);
    if(ndtMap->loadFromJFF(mapFile.c_str()) < 0)
      return -1;
    return 0;
  }
  void initialposeCallback(geometry_msgs::PoseWithCovarianceStamped input_init){
    initialized = false;
    initial_pose = input_init.pose.pose;
    firstLoad = true;
  }
   void VelodyneOdomCallback(const velodyne_msgs::VelodyneScan::ConstPtr& msg,const nav_msgs::Odometry::ConstPtr& odo_in){
    pcl::PointCloud<pcl::PointXYZ> cloud;
    for(size_t next = 0; next < msg->packets.size(); ++next){
      velodyne_rawdata::VPointCloud pnts;
      dataParser.unpack(msg->packets[next], pnts);
      for(size_t i = 0; i < pnts.size(); i++){
  	pcl::PointXYZ p;
  	p.x = pnts.points[i].x;
  	p.y = pnts.points[i].y;
  	p.z = pnts.points[i].z;
  	cloud.push_back(p);
      }
      pnts.clear();
    }
    this->processFrame(cloud,odo_in,msg->header.stamp);
  }
  void PCOdomCallback(const sensor_msgs::PointCloud2::ConstPtr& msg,const nav_msgs::Odometry::ConstPtr& odo_in){
    pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
    pcl::fromROSMsg (*msg, pcl_cloud);
    ROS_INFO("Got points");
    this->processFrame(pcl_cloud,odo_in,msg->header.stamp );

  }

   void LaserOdomCallback(const sensor_msgs::LaserScan::ConstPtr& msg,const nav_msgs::Odometry::ConstPtr& odo_in){
   laser_geometry::LaserProjection projector_;
     sensor_msgs::PointCloud2 cloud;
     pcl::PointCloud<pcl::PointXYZ> pcl_cloud_unfiltered, pcl_cloud;
     //      message_m.lock();
     projector_.projectLaser(*msg, cloud);
     //message_m.unlock();
     pcl::fromROSMsg (cloud, pcl_cloud_unfiltered);

     pcl::PointXYZ pt;
     //add some variance on z
     for(int i=0; i<pcl_cloud_unfiltered.points.size(); i++) {
       pt = pcl_cloud_unfiltered.points[i];
       if(sqrt(pt.x*pt.x+pt.y*pt.y) > min_range) {
   	pt.z += 0.1*((double)rand())/(double)INT_MAX;
   	pcl_cloud.points.push_back(pt);
       }
     }
     ROS_INFO("Got laser points");

     this->processFrame(pcl_cloud,odo_in,msg->header.stamp);

   }

  void processFrame(pcl::PointCloud<pcl::PointXYZ> &cloud, const nav_msgs::Odometry::ConstPtr& odo_in,  ros::Time ts){

    if(!initialized){
      initialized = true;
      MCL->InitializeNormal(initial_pose.position.x, initial_pose.position.y, tf::getYaw(initial_pose.orientation), initVar);
      //MCL->InitializeNormal(0, 0,0, initVar);
      ROS_INFO("Initialized");
    }

    tf::StampedTransform transform;
    double x, y, yaw;
    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
    transform_2.rotate(Eigen::AngleAxisf(sensorOffsetR, Eigen::Vector3f::UnitX())
		       * Eigen::AngleAxisf(sensorOffsetP, Eigen::Vector3f::UnitY())
		       * Eigen::AngleAxisf(sensorOffsetT, Eigen::Vector3f::UnitZ()));                                                                                ////////////////////
    transform_2.translation() << sensorOffsetX, sensorOffsetY, sensorOffsetZ;
    Eigen::Quaterniond qd;

    qd.x() = odo_in->pose.pose.orientation.x;
    qd.y() = odo_in->pose.pose.orientation.y;
    qd.z() = odo_in->pose.pose.orientation.z;
    qd.w() = odo_in->pose.pose.orientation.w;

    Eigen::Affine3d T = Eigen::Translation3d (odo_in->pose.pose.position.x,
					      odo_in->pose.pose.position.y,odo_in->pose.pose.position.z) * qd;

    if(firstLoad){
      tOld = T;
      firstLoad = false;
    }
    Eigen::Affine3d tMotion = tOld.inverse() * T;

    tOld = T;

    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(cloud, *transformed_cloud, transform_2);
    perception_oru::NDTMap *localMap = new perception_oru::NDTMap(new perception_oru::LazyGrid(resolution));
    localMap->loadPointCloud(*transformed_cloud, range);
    localMap->computeNDTCells(CELL_UPDATE_MODE_SAMPLE_VARIANCE);
    cloud.clear();
    transformed_cloud->clear();

    MCL->UpdateAndPredictEff(tMotion, localMap,fraction, cutoff);
    // MCL->UpdateAndPredict(tMotion, localMap);

    delete localMap;
    if(showParticles)
      particlePub.publish(ParticlesToMsg(MCL->particleCloud));
    Eigen::Matrix3d cov;
    Eigen::Vector3d mean;
    nav_msgs::Odometry mcl_pose;
    Eigen::Vector3d mean_pose=MCL->GetMeanPose2D();

    ROS_INFO_STREAM(mean_pose);

    Pose2DToTF(mean_pose,ts,T);
    //ROS_INFO_STREAM(T);
    MCL->GetPoseMeanAndVariance2D(mean, cov);
    mcl_pose = Pose2DToMsg(mean, cov, ts, T);

    if(showPose){
      mcl_pose.header.stamp = ts;
      mclPosePub.publish(mcl_pose);
    }
  }

  Eigen::Affine3d getAsAffine(float x, float y, float yaw ){
    Eigen::Matrix3d m;

    m = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
    Eigen::Translation3d v(x, y, 0);
    Eigen::Affine3d T = v * m;
    return T;
  }

public:
  particle_filter_wrap(ros::NodeHandle param){
    param.param<std::string>("map_file", mapFile, "file.jff");
    param.param<double>("resolution", resolution, 0.2);
    param.param<bool>("be2D", be2D, true);
    param.param<bool>("force_SIR", forceSIR, true);
    param.param<bool>("show_particles", showParticles, false);
    param.param<bool>("show_pose", showPose, true);
    param.param<int>("particle_count", particleCount, 500);

    param.param<std::string>("input_topic_name", inputTopicName, "/velodyne_packets");
    param.param<std::string>("odom_topic_name", odomTopicName, "/odom");
    param.param<bool>("Velodyne", beVelodyne, false);
    param.param<bool>("PointCloud", bePC, false);
    param.param<bool>("Laser", beLaser, false);

    param.param<std::string>("root_tf", rootTF, "/map");
    param.param<std::string>("odom_tf", odomTF, "/odom");
    param.param<std::string>("base_tf", baseTF, "/base_link");
    param.param<std::string>("mcl_tf", mclTF, "/mcl");

    param.param<double>("sensor_pose_x", sensorOffsetX, 0.0);
    param.param<double>("sensor_pose_y", sensorOffsetY, 0.0);
    param.param<double>("sensor_pose_z", sensorOffsetZ, 0.0);
    param.param<double>("sensor_pose_r", sensorOffsetR, 0.0);
    param.param<double>("sensor_pose_p", sensorOffsetP, 0.0);
    param.param<double>("sensor_pose_t", sensorOffsetT, 0.0);

    param.param<double>("initial_x", initX, 0.0);
    param.param<double>("initial_y", initY, 0.0);
    param.param<double>("initial_Th", initT, 0.0);
    param.param<double>("init_var", initVar, 0.5);

    param.param<bool>("flip", flip, false);
    param.param<double>("v_size_x", v_size_x, 200.0);
    param.param<double>("v_size_y", v_size_y, 200.0);
    param.param<double>("v_size_z", v_size_z, 6.0);

    param.param<double>("var_x", var_x, 0.07);
    param.param<double>("var_y", var_y, 0.07);
    param.param<double>("var_th", var_th, 0.035);

    param.param<double>("r_var_x", r_var_x, 1.0);
    param.param<double>("r_var_y", r_var_y, 1.0);
    param.param<double>("r_var_th", r_var_th, 0.001);

    param.param<int>("keep", tres, 400);
    param.param<double>("range", range, 40.0);
    param.param<double>("fraction", fraction, 1.0);
    param.param<double>("cutoff", cutoff, -10);

    param.param("min_range", min_range, 2.0);
    param.param("max_range", max_range, 100.0);
    int retu = dataParser.setup(param);

    dataParser.setParameters(min_range, max_range, 0, 2 * 3.1415);

    /////////
    firstLoad = true;
    initialized = false;
    res.open(resF_name);
    std::cout << "velodyne_ndt_mcl_node will load map\n";
    if(LoadMap() < 0){
      std::cout << "velodyne_ndt_mcl_node load map failed; shutting down\n";
      ros::shutdown();
    }
    std::cout << "velodyne_ndt_mcl_node load map OK\n";
    initial_pose.position.x = initX;
    initial_pose.position.x = initY;
    initial_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0.0, 0.0, initY);

    MCL = new perception_oru::particle_filter(ndtMap, particleCount, be2D, forceSIR);

    if(beVelodyne){
    vel_sub_ = new message_filters::Subscriber< velodyne_msgs::VelodyneScan>(nh,inputTopicName,1);
      odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh,odomTopicName,10);
      sync_vo_ = new message_filters::Synchronizer< VeloOdomSync >(VeloOdomSync(20), *vel_sub_, *odom_sub_);
      sync_vo_->registerCallback(boost::bind(&particle_filter_wrap::VelodyneOdomCallback, this, _1, _2));
    }
    //   PCSub = nh.subscribe(inputTopicName, 1, &particle_filter_wrap::VeloCallback, this);
    if(bePC){
      points2_sub_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh,inputTopicName,1);
      odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh,odomTopicName,10);
      sync_po_ = new message_filters::Synchronizer< PointsOdomSync >(PointsOdomSync(20), *points2_sub_, *odom_sub_);
      sync_po_->registerCallback(boost::bind(&particle_filter_wrap::PCOdomCallback, this, _1, _2));

    }
    // PCSub = nh.subscribe(inputTopicName, 1, &particle_filter_wrap::PCCallback, this);
    if(beLaser){
      laser_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(nh,inputTopicName,1);
      odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(nh,odomTopicName,10);
      sync_lo_ = new message_filters::Synchronizer< LaserOdomSync >(LaserOdomSync(20), *laser_sub_, *odom_sub_);
      sync_lo_->registerCallback(boost::bind(&particle_filter_wrap::LaserOdomCallback, this, _1, _2));
    }
    //   PCSub = nh.subscribe(inputTopicName, 1, &particle_filter_wrap::LaserCallback, this);

    initPoseSub = nh.subscribe("/initialpose", 1000, &particle_filter_wrap::initialposeCallback, this);

    if(showParticles)
      particlePub = nh.advertise<geometry_msgs::PoseArray>("particles", 1);
    if(showPose)
      mclPosePub=nh.advertise<nav_msgs::Odometry>("ndt_mcl_pose", 1);

    std::cout << "velodyne_ndt_mcl_node will now enter spin()\n";
    ros::spin();
  }
};

int main(int argc, char **argv){
  ros::init(argc, argv, "ndt_mcl");
  ros::NodeHandle parameters("~");
  particle_filter_wrap pf(parameters);
}


#include "rb_tracker/StaticEnvironmentFilter.h"
#include "omip_common/OMIPUtils.h"
#include <boost/foreach.hpp>
#include <boost/numeric/ublas/matrix.hpp>

using namespace omip;

using namespace MatrixWrapper;
using namespace BFL;

tf::Transform Eigen2Tf(Eigen::Matrix4f trans)
{
    tf::Matrix3x3 btm;
    btm.setValue(trans(0,0),trans(0,1),trans(0,2),
                 trans(1,0),trans(1,1),trans(1,2),
                 trans(2,0),trans(2,1),trans(2,2));
    tf::Transform ret;
    ret.setOrigin(tf::Vector3(trans(0,3),trans(1,3),trans(2,3)));
    ret.setBasis(btm);
    return ret;
}

tf::Transform Eigen2Tf(Eigen::Matrix4d trans)
{
    tf::Matrix3x3 btm;
    btm.setValue(trans(0,0),trans(0,1),trans(0,2),
                 trans(1,0),trans(1,1),trans(1,2),
                 trans(2,0),trans(2,1),trans(2,2));
    tf::Transform ret;
    ret.setOrigin(tf::Vector3(trans(0,3),trans(1,3),trans(2,3)));
    ret.setBasis(btm);
    return ret;
}

StaticEnvironmentFilter::StaticEnvironmentFilter(double loop_period_ns, FeaturesDataBase::Ptr feats_database,
                                                 double environment_motion_threshold) :
    RBFilter()
{
    this->_loop_period_ns = loop_period_ns;
    this->_id = 0;
    this->_features_database = feats_database;
    this->_estimation_error_threshold = environment_motion_threshold;

    this->_tf_epsilon_linear = 1e-8;
    this->_tf_epsilon_angular = 1.7e-9;
    this->_max_iterations = 100;

    this->_pose = Eigen::Matrix4d::Identity();
    this->_velocity = Eigen::Twistd(0.,0.,0.,0.,0.,0);

    this->_motion_constraint = NO_MOTION;

    this->_computation_type = STATIC_ENVIRONMENT_EKF_TRACKER;

    // To avoid segmentation fault because the features are as old as the rigid body
    this->_trajectory.push_back(this->_pose);
    this->_trajectory.push_back(this->_pose);
    this->_trajectory.push_back(this->_pose);
    this->_trajectory.push_back(this->_pose);
}

void StaticEnvironmentFilter::Init()
{
    RBFilter::Init();
    _pose_covariance.setZero();
    _velocity_covariance.setZero();
}

void StaticEnvironmentFilter::predictState(double time_interval_ns)
{
    if(this->_motion_constraint == NO_MOTION)
    {
        this->_predicted_pose_vh = _pose;
        this->_predicted_pose_bh = _pose;
        return;
    }

    switch(this->_computation_type)
    {

    case STATIC_ENVIRONMENT_ICP_TRACKER:
    {
        Eigen::Twistd predicted_delta_pose_vh = (this->_velocity*time_interval_ns/1e9);
        this->_predicted_pose_vh = predicted_delta_pose_vh.exp(1e-12).toHomogeneousMatrix()*_pose;
        this->_predicted_pose_bh = _pose;
    }
        break;
    case STATIC_ENVIRONMENT_EKF_TRACKER:
    {
        RBFilter::predictState(time_interval_ns);
    }
        break;

    default:
        ROS_ERROR_STREAM_NAMED("StaticEnvironmentFilter.predictState","Wrong StaticEnvironment computation_type!");
        break;
    }
}

void StaticEnvironmentFilter::correctState()
{
    // The problem is that the StaticEnvironmentFilter is created at the beggining when only one location per feature is available
    // There is no way to correct the state with only one location per feature
    static bool first_time = true;
    if(first_time)
    {
        this->_trajectory.push_back(this->_pose);
        first_time = false;
        return;
    }

    if(this->_motion_constraint == NO_MOTION)
    {
        this->_velocity = Eigen::Twistd(0,0,0,0,0,0);
        this->_trajectory.push_back(_pose);
        return;
    }

    switch(this->_computation_type)
    {

    case STATIC_ENVIRONMENT_ICP_TRACKER:
    {
        // I follow the convention of "Introduction to robotics" by Craig
        // The first fram is the reference (left upper index) and the second frame is the referred (left lower index)
        // se -> static environment
        // set -> static environment at time t
        // Example: setnext_set_T is describes the frame "static environment at time t" wrt the frame "static environment at time tnext"
        tf::Transform set_setnext_Tf;
        this->estimateDeltaMotion(this->_supporting_features_ids, set_setnext_Tf);

        Eigen::Matrix4d set_setnext_T;
        set_setnext_Tf.getOpenGLMatrix(set_setnext_T.data());

        Eigen::Twistd set_setnext_Twist;
        TransformMatrix2Twist(set_setnext_T, set_setnext_Twist);

        this->_velocity = set_setnext_Twist/(this->_loop_period_ns/1e9);

        // Accumulate the new delta into the absolute pose of the static environment wrt the camera
        this->_pose = this->_pose*set_setnext_T;

        this->_trajectory.push_back(this->_pose);

        constrainMotion();
    }
        break;
    case STATIC_ENVIRONMENT_EKF_TRACKER:
    {
        RBFilter::correctState();

        constrainMotion();
    }
        break;

    default:
        ROS_ERROR_STREAM_NAMED("StaticEnvironmentFilter.correctState","Wrong StaticEnvironment computation_type!");
        break;
    }
}

void StaticEnvironmentFilter::setMotionConstraint(int motion_constraint)
{
    this->_motion_constraint = (MotionConstraint)motion_constraint;
}

void StaticEnvironmentFilter::setComputationType(static_environment_tracker_t computation_type)
{
    this->_computation_type = computation_type;
}

void StaticEnvironmentFilter::addSupportingFeature(Feature::Id supporting_feat_id)
{
    this->_supporting_features_ids.push_back(supporting_feat_id);

    Feature::Location predicted_location_velocity;
    Feature::Location predicted_location_brake;
    FeaturePCLwc predicted_feature_pcl;
    // Predict feature location based on velocity hypothesis

    predicted_location_velocity = this->_features_database->getFeatureLastLocation(supporting_feat_id);
    LocationAndId2FeaturePCLwc(predicted_location_velocity, supporting_feat_id, predicted_feature_pcl);
    this->_predicted_measurement_pc_vh->points.push_back(predicted_feature_pcl);
    this->_predicted_measurement_map_vh[supporting_feat_id] = predicted_location_velocity;

    // Predict feature location based on brake hypothesis (same as last)
    predicted_location_brake = this->_features_database->getFeatureLastLocation(supporting_feat_id);
    LocationAndId2FeaturePCLwc(predicted_location_brake, supporting_feat_id, predicted_feature_pcl);
    this->_predicted_measurement_pc_bh->points.push_back(predicted_feature_pcl);
    this->_predicted_measurement_map_bh[supporting_feat_id] = predicted_location_brake;
}

void StaticEnvironmentFilter::estimateDeltaMotion(std::vector<Feature::Id>& supporting_features_ids, tf::Transform& previous_current_Tf)
{
    // Prepare the 2 point clouds with the locations of the features in current and previous frames
    pcl::PointCloud<pcl::PointXYZ> previous_locations, current_locations;
    BOOST_FOREACH(Feature::Id supporting_feat_id, supporting_features_ids)
    {
        if (this->_features_database->isFeatureStored(supporting_feat_id) && this->_features_database->getFeatureAge(supporting_feat_id) > 2)
        {
            Feature::Location previous_location = this->_features_database->getFeatureNextToLastLocation(supporting_feat_id);
            Feature::Location current_location = this->_features_database->getFeatureLastLocation(supporting_feat_id);
            previous_locations.push_back(pcl::PointXYZ(previous_location.get<0>(), previous_location.get<1>() ,previous_location.get<2>()));
            current_locations.push_back(pcl::PointXYZ(current_location.get<0>(), current_location.get<1>() ,current_location.get<2>()));
        }
    }

    if (previous_locations.size() ==0)
    {
        ROS_ERROR_STREAM_NAMED("StaticEnvironmentFilter.estimateDeltaMotion","There are no features supporting the static environment!");
        previous_current_Tf.setIdentity();
    }else{
        iterativeICP(previous_locations, current_locations, previous_current_Tf);
    }
}

void StaticEnvironmentFilter::iterativeICP( pcl::PointCloud<pcl::PointXYZ>& previous_locations,
                                            pcl::PointCloud<pcl::PointXYZ>& current_locations, tf::Transform& previous_current_Tf)
{
    // initialize the result transform
    Eigen::Matrix4f previous_current_T;
    previous_current_T.setIdentity();

    // create indices
    std::vector<int> previous_indices, current_indices;
    for(int i=0; i<previous_locations.size();i++)
    {
        previous_indices.push_back(i);
        current_indices.push_back(i);
    }

    for (int iteration = 0; iteration < this->_max_iterations; ++iteration)
    {
        // estimate transformation
        Eigen::Matrix4f previous_current_T_new;
        _svdecomposer.estimateRigidTransformation (previous_locations, previous_indices,
                                                   current_locations, current_indices,
                                                   previous_current_T_new);

        // transform
        pcl::transformPointCloud(previous_locations, previous_locations, previous_current_T_new);

        // accumulate incremental tf
        previous_current_T = previous_current_T_new * previous_current_T;

        // check for convergence
        double linear, angular;

        previous_current_Tf = Eigen2Tf(previous_current_T);
        linear = previous_current_Tf.getOrigin().length();
        double trace = previous_current_Tf.getBasis()[0][0] + previous_current_Tf.getBasis()[1][1] + previous_current_Tf.getBasis()[2][2];
        angular = acos(std::min(1.0, std::max(-1.0, (trace - 1.0)/2.0)));
        if (linear  < this->_tf_epsilon_linear && angular < this->_tf_epsilon_angular)
        {
            break;
        }
    }
}

void StaticEnvironmentFilter::constrainMotion()
{
    // The last estimated and unconstrained pose is contained in the variable _pose = cam_setnext_T

//    // The previous pose is contained in the vector _trajectory
//    Eigen::Matrix4d cam_set_T = _trajectory.at(_trajectory.size() - 2);

//    switch(this->_motion_constraint)
//    {
//    case NO_ROLL_PITCH:
//    {
//        this->_pose.rz() = cam_set_Twist.rz();
//        this->_pose.ry() = cam_set_Twist.ry();

//        this->_velocity.rz() = 0;
//        this->_velocity.ry() = 0;
//    }
//        break;
//    case NO_ROLL_PITCH_TZ:
//    {
//        this->_pose.rz() = cam_set_Twist.rz();
//        this->_pose.ry() = cam_set_Twist.ry();
//        this->_pose.vz() = cam_set_Twist.vz();

//        this->_velocity.rz() = 0;
//        this->_velocity.ry() = 0;
//        this->_velocity.vz() = 0;
//    }
//        break;
//    case NO_TRANSLATION:
//    {
//        this->_pose.vx() = cam_set_Twist.vx();
//        this->_pose.vy() = cam_set_Twist.vy();
//        this->_pose.vz() = cam_set_Twist.vz();

//        this->_velocity.vx() = 0;
//        this->_velocity.vy() = 0;
//        this->_velocity.vz() = 0;
//    }
//        break;
//    case NO_TRANSLATION_ROLL_YAW:
//    {
//        this->_pose.vx() = cam_set_Twist.vx();
//        this->_pose.vy() = cam_set_Twist.vy();
//        this->_pose.vz() = cam_set_Twist.vz();
//        this->_pose.rz() = cam_set_Twist.rz();
//        this->_pose.rx() = cam_set_Twist.rx();

//        this->_velocity.vx() = 0;
//        this->_velocity.vy() = 0;
//        this->_velocity.vz() = 0;
//        this->_velocity.rz() = 0;
//        this->_velocity.rx() = 0;
//    }
//        break;
//    case NO_ROTATION:
//    {
//        this->_pose.rx() = cam_set_Twist.rx();
//        this->_pose.ry() = cam_set_Twist.ry();
//        this->_pose.rz() = cam_set_Twist.rz();\
//        this->_velocity.rx() = 0;
//        this->_velocity.ry() = 0;
//        this->_velocity.rz() = 0;
//    }
//        break;
//    case NO_MOTION:
//    {
//        this->_pose.rx() = cam_set_Twist.rx();
//        this->_pose.ry() = cam_set_Twist.ry();
//        this->_pose.rz() = cam_set_Twist.rz();
//        this->_pose.vx() = cam_set_Twist.vx();
//        this->_pose.vy() = cam_set_Twist.vy();
//        this->_pose.vz() = cam_set_Twist.vz();
//        this->_velocity.rx() = 0;
//        this->_velocity.ry() = 0;
//        this->_velocity.rz() = 0;
//        this->_velocity.vx() = 0;
//        this->_velocity.vy() = 0;
//        this->_velocity.vz() = 0;
//    }
//        break;
//    case ROBOT_XY_BASELINK_PLANE:
//    {
//        // Query the current pose of the camera wrt the baselink frame
//        tf::StampedTransform bl_cam_TF;
//        Eigen::Matrix4d bl_cam_T;

//        try{

//            this->_tf_listener.lookupTransform("/base_link", "/camera_rgb_optical_frame", ros::Time(0), bl_cam_TF);
//            bl_cam_TF.getOpenGLMatrix(bl_cam_T.data());
//        }
//        catch (tf::TransformException ex)
//        {
//            ROS_ERROR("waitForTransform in StaticEnvironmentFilter failed!");
//            ROS_ERROR("%s",ex.what());
//            bl_cam_T = Eigen::Matrix4d::Identity();
//        }

//        // The matrix of the pose of the static environment to the camera in the next time step
//        Eigen::Matrix4d cam_se_T;
//        Twist2TransformMatrix(_pose, cam_se_T);

//        Eigen::Matrix4d bl_se_T = bl_cam_T * cam_se_T;

//        Eigen::Matrix4d bl_se_T_constrained = bl_se_T;

//        // Eliminate translation along the z axis of the base link frame
//        bl_se_T_constrained(2,3) = bl_cam_T(2,3);

//        // Restrict the rotation to be only around the z axis of the base link frame
//        Eigen::Quaterniond bl_se_R(bl_se_T.block<3,3>(0,0));
//        Eigen::Vector3d bl_se_RPY = bl_se_R.toRotationMatrix().eulerAngles(2, 1, 0);

//        Eigen::Quaterniond bl_cam_R(bl_cam_T.block<3,3>(0,0));
//        Eigen::Vector3d bl_cam_RPY = bl_cam_R.toRotationMatrix().eulerAngles(2, 1, 0);

//        Eigen::Vector3d bl_se_RPY_constrained;

//        bl_se_RPY_constrained[0] = bl_se_RPY[0];    //Rotation around the baselink_z is the estimated one (unconstrained)
//        bl_se_RPY_constrained[1] = bl_cam_RPY[1];   //Rotation around the baselink_y is constrained, must be the same as for the cam
//        bl_se_RPY_constrained[2] = bl_cam_RPY[2];   //Rotation around the baselink_x is constrained, must be the same as for the cam

//        // Build the constrained rotation matrix
//        Eigen::Matrix3f bl_se_R_constrained;
//        bl_se_R_constrained = Eigen::AngleAxisf(bl_se_RPY_constrained[0], Eigen::Vector3f::UnitZ())
//                *Eigen::AngleAxisf(bl_se_RPY_constrained[1], Eigen::Vector3f::UnitY())
//                *Eigen::AngleAxisf(bl_se_RPY_constrained[2], Eigen::Vector3f::UnitX());

//        // Assign the rotation part to the constrained transformation matrix
//        bl_se_T_constrained.block<3,3>(0,0) = bl_se_R_constrained.cast<double>();

//        // Calculate the constrained pose of the static environment to the camera
//        Eigen::Matrix4d cam_se_T_constrained = bl_cam_T.inverse() * bl_se_T_constrained;
//        TransformMatrix2Twist(cam_se_T_constrained, _pose);

//        // Calculate the constrained velocity of the static environment to the camera
//        Eigen::Matrix4d cam_set_T = cam_set_Twist.exp(1e-12).toHomogeneousMatrix();
//        Eigen::Matrix4d set_setnext_T_constrained = cam_set_T.inverse()*cam_se_T_constrained;
//        Eigen::Twistd set_setnext_Twist_constrained;
//        TransformMatrix2Twist(set_setnext_T_constrained, set_setnext_Twist_constrained);

//        _velocity = set_setnext_Twist_constrained / _loop_period_ns/1e9;
//    }
//        break;
//    default:
//        break;

//    }

//    // Reconfigure the variables in the filters
//    ColumnVector constrained_state(12);
//    constrained_state(1) = this->_pose.vx();
//    constrained_state(2) = this->_pose.vy();
//    constrained_state(3) = this->_pose.vz();
//    constrained_state(4) = this->_pose.rx();
//    constrained_state(5) = this->_pose.ry();
//    constrained_state(6) = this->_pose.rz();
//    constrained_state(7) = this->_velocity.vx();
//    constrained_state(8) = this->_velocity.vy();
//    constrained_state(9) = this->_velocity.vz();
//    constrained_state(10) = this->_velocity.rx();
//    constrained_state(11) = this->_velocity.ry();
//    constrained_state(12) = this->_velocity.rz();

//    this->_ekf_velocity->PostGet()->ExpectedValueSet(constrained_state);
//    this->_ekf_brake->PostGet()->ExpectedValueSet(constrained_state);

//    // Remove the last estimated pose (unconstrained) from the trajectory vector and replace it with the constrained one
//    this->_trajectory.pop_back();
//    this->_trajectory.push_back(this->_pose);
}

//void StaticEnvironmentFilter::constrainMotion(tf::Transform& motion)
//{
//    // **** degree-of-freedom constraints

//    switch(this->_motion_constraint)
//    {
//    case NO_ROLL_PITCH:
//    {
//        tf::Quaternion q;
//        q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation()));

//        motion.setRotation(q);

//    }
//        break;
//    case NO_ROLL_PITCH_TZ:
//    {
//        tf::Quaternion q;
//        q.setRPY(0.0, 0.0, tf::getYaw(motion.getRotation()));

//        tf::Vector3 p(motion.getOrigin().getX(), motion.getOrigin().getY(), 0.0);

//        motion.setOrigin(p);
//        motion.setRotation(q);
//    }
//        break;
//    case NO_TRANSLATION:
//    {
//        tf::Vector3 p(0.0,0.0, 0.0);
//        motion.setOrigin(p);
//    }
//        break;
//    case NO_TRANSLATION_ROLL_YAW:
//    {
//        tf::Quaternion q_set;
//        tf::Quaternion q(motion.getRotation().x(),
//                         motion.getRotation().y(),
//                         motion.getRotation().z(),
//                         motion.getRotation().w());
//        double roll, pitch, yaw;
//        tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
//        q_set.setRPY(0.0, pitch, 0.0);
//        tf::Vector3 p(0.0,0.0, 0.0);
//        motion.setOrigin(p);
//        motion.setRotation(q_set);
//    }
//        break;
//    case NO_ROTATION:
//    {
//        tf::Quaternion q;
//        q.setRPY(0.0, 0.0 , 0.0);
//        motion.setRotation(q);
//    }
//        break;
//    case NO_MOTION:
//    {
//        tf::Vector3 p(0.,0., 0.0);
//        tf::Quaternion q;
//        q.setRPY(0.0, 0.0 , 0.0);

//        motion.setOrigin(p);
//        motion.setRotation(q);
//    }
//        break;
//    case ROBOT_XY_BASELINK_PLANE:
//    {
//        // Query the current pose of the camera wrt the baselink frame
//        tf::StampedTransform Tf_camt_bl;
//        Eigen::Matrix4d T_camt_bl;
//        try{

//            this->_tf_listener.lookupTransform("/base_link", "/camera_rgb_optical_frame", ros::Time(0), Tf_camt_bl);
//            Tf_camt_bl.getOpenGLMatrix(T_camt_bl.data());
//        }
//        catch (tf::TransformException ex)
//        {
//            ROS_ERROR("waitForTransform in StaticEnvironmentICPFilter failed!");
//            ROS_ERROR("%s",ex.what());
//            T_camt_bl = Eigen::Matrix4d::Identity();
//            getchar();
//        }

//        // Apply the estimated delta motion to the pose
//        // Verbose!

//        Eigen::Matrix4d T_camtnext_camt;
//        motion.inverse().getOpenGLMatrix(T_camtnext_camt.data());

//        Eigen::Matrix4d T_camt_camtnext = T_camtnext_camt.inverse();

//        Eigen::Matrix4d T_camtnext_bl = T_camt_bl*T_camt_camtnext.inverse();

//        Eigen::Matrix4d T_camtnext_bl_constrained = T_camtnext_bl;

//        // Eliminate translation along the z axis
//        T_camtnext_bl_constrained(2,3) = T_camt_bl(2,3);

//        // Restrict the rotation to be only around the z axis
//        Eigen::Quaterniond R_camtnext_bl(T_camtnext_bl.block<3,3>(0,0));
//        Eigen::Vector3d RPY_camtnext_bl = R_camtnext_bl.toRotationMatrix().eulerAngles(2, 1, 0);

//        Eigen::Quaterniond R_camt_bl(T_camt_bl.block<3,3>(0,0));
//        Eigen::Vector3d RPY_camt_bl = R_camt_bl.toRotationMatrix().eulerAngles(2, 1, 0);

//        Eigen::Vector3d RPY_camtnext_bl_constrained;
//        RPY_camtnext_bl_constrained.x() = RPY_camtnext_bl.x();  //Unconstrained rotation around z
//        RPY_camtnext_bl_constrained.y() = RPY_camt_bl.y();  // Constrained rotation around y
//        RPY_camtnext_bl_constrained.z() = RPY_camt_bl.z();  // Constrained rotation around x

//        // Build the constrained pose
//        Eigen::Matrix3f R_camtnext_bl_constrained;
//        R_camtnext_bl_constrained = Eigen::AngleAxisf(RPY_camtnext_bl_constrained[0], Eigen::Vector3f::UnitZ())
//                *Eigen::AngleAxisf(RPY_camtnext_bl_constrained[1], Eigen::Vector3f::UnitY())
//                *Eigen::AngleAxisf(RPY_camtnext_bl_constrained[2], Eigen::Vector3f::UnitX());

//        T_camtnext_bl_constrained.block<3,3>(0,0) = R_camtnext_bl_constrained.cast<double>();

//        Eigen::Matrix4d T_camt_camtnext_constrained = ((T_camt_bl.inverse())*T_camtnext_bl_constrained).inverse();

//        Eigen::Matrix4d T_camtnext_camt_constrained = T_camt_camtnext_constrained.inverse();

//        motion = Eigen2Tf(T_camtnext_camt_constrained);
//    }
//        break;
//    default:
//        break;

//    }
//}


/**
 *-----------------------------------------------------------------------------
 * Title         : SMuRF Data Processor
 * ----------------------------------------------------------------------------
 * File          : SmurfProcessor.cpp
 * Created       : 2019-09-27
 *-----------------------------------------------------------------------------
 * Description :
 *    SMuRF Data Processor Class.
 *-----------------------------------------------------------------------------
 * This file is part of the smurf software platform. It is subject to
 * the license terms in the LICENSE.txt file found in the top-level directory
 * of this distribution and at:
    * https://confluence.slac.stanford.edu/display/ppareg/LICENSE.html.
 * No part of the smurf software platform, including this file, may be
 * copied, modified, propagated, or distributed except according to the terms
 * contained in the LICENSE.txt file.
 *-----------------------------------------------------------------------------
**/

#include <boost/python.hpp>
#include "smurf/core/processors/SmurfProcessor.h"

namespace scp = smurf::core::processors;

scp::SmurfProcessor::SmurfProcessor()
:
    ris::Slave(),
    ris::Master(),
    numCh(maxNumCh),
    payloadSize(0),
    mask(numCh,0),
    disableUnwrapper(false),
    currentData(numCh, 0),
    previousData(numCh, 0),
    wrapCounter(numCh, 0),
    inputData(numCh, 0),
    disableFilter(false),
    order(4),
    gain(1),
    a( order + 1 ,1 ),
    b( order + 1, 1 ),
    currentIndex(0),
    x( ( order + 1 ) * numCh ),
    y( ( order + 1 ) * numCh ),
    outData(numCh,0),
    disableDownsampler(false),
    factor(20),
    sampleCnt(0),
    headerCopy(SmurfHeader<std::vector<uint8_t>::iterator>::SmurfHeaderSize, 0),
    runTxThread(true),
    txDataReady(false),
    pktTransmitterThread(std::thread( &SmurfProcessor::pktTansmitter, this )),
    eLog_(rogue::Logging::create("pysmurf.SmurfProcessor"))
{
    if( pthread_setname_np( pktTransmitterThread.native_handle(), "pktTransmitter" ) )
        perror( "pthread_setname_np failed for pktTransmitterThread thread" );
}

scp::SmurfProcessorPtr scp::SmurfProcessor::create()
{
    return std::make_shared<SmurfProcessor>();
}

void scp::SmurfProcessor::setup_python()
{
    bp::class_< scp::SmurfProcessor,
                scp::SmurfProcessorPtr,
                bp::bases<ris::Slave,ris::Master>,
                boost::noncopyable >
                ("SmurfProcessor",bp::init<>())
        // Channel mapping variables
        .def("getNumCh",                &SmurfProcessor::getNumCh)
        .def("setPayloadSize",          &SmurfProcessor::setPayloadSize)
        .def("getPayloadSize",          &SmurfProcessor::getPayloadSize)
        .def("setMask",                 &SmurfProcessor::setMask)
        .def("getMask",                 &SmurfProcessor::getMask)
        // Unwrapper variables
        .def("setUnwrapperDisable",     &SmurfProcessor::setUnwrapperDisable)
        .def("getUnwrapperDisable",     &SmurfProcessor::getUnwrapperDisable)
        .def("resetUnwrapper",          &SmurfProcessor::resetUnwrapper)
        // Filter variables
        .def("setFilterDisable",        &SmurfProcessor::setFilterDisable)
        .def("getFilterDisable",        &SmurfProcessor::getFilterDisable)
        .def("setOrder",                &SmurfProcessor::setOrder)
        .def("getOrder",                &SmurfProcessor::getOrder)
        .def("setA",                    &SmurfProcessor::setA)
        .def("getA",                    &SmurfProcessor::getA)
        .def("setB",                    &SmurfProcessor::setB)
        .def("getB",                    &SmurfProcessor::getB)
        .def("setGain",                 &SmurfProcessor::setGain)
        .def("getGain",                 &SmurfProcessor::getGain)
        .def("resetFilter",             &SmurfProcessor::resetFilterWithMutex)
        // Downsampler variables
        .def("setDownsamplerDisable",   &SmurfProcessor::setDownsamplerDisable)
        .def("getDownsamplerDisable",   &SmurfProcessor::getDownsamplerDisable)
        .def("setFactor",               &SmurfProcessor::setFactor)
        .def("getFactor",               &SmurfProcessor::getFactor)
    ;
    bp::implicitly_convertible< scp::SmurfProcessorPtr, ris::SlavePtr  >();
    bp::implicitly_convertible< scp::SmurfProcessorPtr, ris::MasterPtr >();
}

const std::size_t scp::SmurfProcessor::getNumCh() const
{
    return numCh;
}

const std::size_t scp::SmurfProcessor::getPayloadSize() const
{
    return payloadSize;
}

void scp::SmurfProcessor::setPayloadSize(std::size_t s)
{
    payloadSize = s;
}

void scp::SmurfProcessor::setMask(bp::list m)
{
    std::size_t listSize = len(m);

    // Check if the size of the list, is not greater than
    // the number of channels we can have in the output packet.
    if ( listSize > maxNumCh )
    {
        // This should go to a logger instead
        std::cerr << "ERROR: Trying to set a mask list of length = " << listSize \
                  << ", which is larger that the number of channel in a SMuRF packet = " \
                  <<  maxNumCh << std::endl;

        // Do not update the mask vector.
        return;
    }

    // We will use a temporal vector to hold the new data.
    // New data will be check as it is pushed to this vector. If there
    // are not error, this vector will be swap with 'mask'.
    std::vector<std::size_t> temp;

    for (std::size_t i{0}; i < listSize; ++i)
    {
        std::size_t val = bp::extract<std::size_t>(m[i]);

        // Check if the mask value is not greater than
        // the number of channel we received in the incoming frame
        if (val > maxNumCh)
        {
            // This should go to a logger instead
            std::cerr << "ERROR: mask value at index " << i << " is " << val \
                      << ", which is greater the maximum number of channel we expect from an input frame = " \
                      << maxNumCh << std::endl;

            // Do not update the mask vector.
            return;
        }

        // A valid number was found. Add it to the temporal vector
        temp.push_back(val);
    }

    // Take the mutex before changing the mask vector
    std::lock_guard<std::mutex> lockMap(mutChMapper);

    // At this point, all element in the mask list are valid.
    // Update the mask vector
    mask.swap(temp);

    // Update the number of mapped channels
    updateNumCh();
}

const bp::list scp::SmurfProcessor::getMask() const
{
    bp::list temp;

    for (auto const &v : mask)
        temp.append(v);

    return temp;
}

void scp::SmurfProcessor::updateNumCh()
{
    // Start with the size of the mask vector as the new size
    std::size_t newNumCh = mask.size();

    // If the new size if different from the current size, update it
    if (numCh != newNumCh)
    {
        numCh = newNumCh;

        // Reset the Unwrapper
        resetUnwrapper();

        // Reset the filter
        // Take the mutex before changing the filter parameters
        // This make sure that the new order value is not used before
        // the a and b array are resized.
        std::lock_guard<std::mutex> lockFilter(mutFilter);
        resetFilter();
    }
}

void scp::SmurfProcessor::setUnwrapperDisable(bool d)
{
    disableUnwrapper = d;

    // Reset the unwrapper when it is re-enable.
    if (!disableUnwrapper)
        resetUnwrapper();
}

const bool scp::SmurfProcessor::getUnwrapperDisable() const
{
    return disableUnwrapper;
}

void scp::SmurfProcessor::resetUnwrapper()
{
    // Take the mutex before reseting the unwrapper as the
    // This makes sure that the new vectors are not used while
    // they are potentially being resized.
    std::lock_guard<std::mutex> lockUnwrapper(mutUnwrapper);

    std::vector<fw_t>(numCh).swap(currentData);
    std::vector<fw_t>(numCh).swap(previousData);
    std::vector<unwrap_t>(numCh).swap(wrapCounter);
    std::vector<unwrap_t>(numCh).swap(inputData);
}

void scp::SmurfProcessor::setFilterDisable(bool d)
{
    // Take the mutex before changing the filter parameters
    // This make sure that the new order value is not used before
    // the a and b array are resized.
    std::lock_guard<std::mutex> lock(mutFilter);

    disableFilter = d;

    // Reset the filter when is enable flag changes.
    resetFilter();
}

const bool scp::SmurfProcessor::getFilterDisable() const
{
    return disableFilter;
}

void scp::SmurfProcessor::setOrder(std::size_t o)
{
    // Check if the new order is different from the current one
    if ( o != order )
    {
        // Take the mutex before changing the filter parameters
        // This make sure that the new order value is not used before
        // the a and b array are resized.
        std::lock_guard<std::mutex> lock(mutFilter);

        order = o;

        // When the order change, reset the filter
        resetFilter();
    }
}

const std::size_t scp::SmurfProcessor::getOrder() const
{
    return order;
}

void scp::SmurfProcessor::setA(bp::list l)
{
    std::vector<double> temp;

    // Take the mutex before changing the filter parameters
    // This make sure that the 'a' array is not used before it has
    // beem resized, if necessary.
    std::lock_guard<std::mutex> lock(mutFilter);

    std::size_t listSize = len(l);

    if (listSize == 0)
    {
        // Verify that the input list is not empty.
        // If empty, set the coefficients vector to a = [1.0].
        eLog_->error("Trying to set an empty set of a coefficients. Defaulting to 'a = [1.0]'");
        temp.push_back(1.0);
    }
    else if (l[0] == 0)
    {
        // Verify that the first coefficient is not zero.
        // if it is, set the coefficients vector to a = [1.0].
        eLog_->error("The first a coefficient can not be zero. Defaulting to 'a = [1.0]'");
        temp.push_back(1.0);
    }
    else
    {
        // Extract the coefficients coming from python into a temporal vector
        for (std::size_t i{0}; i < listSize; ++i)
            temp.push_back(bp::extract<double>(l[i]));
    }

    // Update the a vector with the new values
    a.swap(temp);

    // When the coefficients change, reset the filter
    resetFilter();
}

const bp::list scp::SmurfProcessor::getA() const
{
    bp::list temp;

    for (auto const &v : a)
        temp.append(v);

    return temp;
}

void scp::SmurfProcessor::setB(bp::list l)
{
    std::vector<double> temp;

    // Take the mutex before changing the filter parameters
    // This make sure that the 'b' array is not used before it has
    // beem resized, if necessary.
    std::lock_guard<std::mutex> lock(mutFilter);

    std::size_t listSize = len(l);

    if (listSize == 0)
    {
        // Verify that the input list is not empty.
        // If empty, set the coefficients vector to a = [0.0].
        eLog_->error("ERROR: Trying to set an empty set of a coefficients. Defaulting to 'b = [0.0]'");
        temp.push_back(0.0);
    }
    else
    {
        // Extract the coefficients coming from python into a temporal vector
        for (std::size_t i{0}; i < len(l); ++i)
            temp.push_back(bp::extract<double>(l[i]));
    }

    // Update the a vector with the new values
    b.swap(temp);

    // When the coefficients change, reset the filter
    resetFilter();
}

const bp::list scp::SmurfProcessor::getB() const
{
    bp::list temp;

    for (auto const &v : b)
        temp.append(v);

    return temp;
}

void scp::SmurfProcessor::setGain(double g)
{
    gain = g;
}

const double scp::SmurfProcessor::getGain() const
{
    return gain;
}

// Reset the filter. Resize and Zero-initialize the data buffer, and
// check if the coefficient vectors have the correct size, and expand
// if necessary, padding with zeros.
void scp::SmurfProcessor::resetFilter()
{
    // Resize and re-initialize the data buffer
    std::vector<double>( ( order + 1 ) * numCh ).swap(x);
    std::vector<double>( ( order + 1 ) * numCh ).swap(y);

    // Use the mutex to update the outData buffer
    {
        std::lock_guard<std::mutex> lock(outDataMutex);
        std::vector<filter_t>( numCh ).swap(outData);
    }

    // Check that a coefficient vector size is at least 'order + 1'.
    // If not, add expand it with zeros.
    if ( a.size() < (order + 1) )
        a.resize(order +  1, 0);

    // Check that b coefficient vector size is at least 'order + 1'.
    // If not, add expand it with zeros.
    if ( b.size() < (order + 1) )
        b.resize(order +  1, 0);

    // Reset the index of the older point in the buffer
    currentIndex = 0;
}

// Reset the filter but holding the mutex. This is need when reseting the
// filter from python
void scp::SmurfProcessor::resetFilterWithMutex()
{
    // Take the mutex before changing the filter parameters
    // This make sure that the new order value is not used before
    // the a and b array are resized.
    std::lock_guard<std::mutex> lockFilter(mutFilter);
    resetFilter();
}

void scp::SmurfProcessor::setDownsamplerDisable(bool d)
{
    disableDownsampler = d;
}

const bool scp::SmurfProcessor::getDownsamplerDisable() const
{
    return disableDownsampler;
}

void scp::SmurfProcessor::setFactor(std::size_t f)
{
    // Check if the factor is 0
    if (0 == f)
    {
        // This should go to a logger instead
        std::cerr << "ERROR: Trying to set factor = 0."<< std::endl;
        return;
    }

    factor = f;

    // When the factor is changed, reset the counter.
    resetDownsampler();
}

const std::size_t scp::SmurfProcessor::getFactor() const
{
    return factor;
}

void scp::SmurfProcessor::resetDownsampler()
{
    sampleCnt = 0;
}

void scp::SmurfProcessor::acceptFrame(ris::FramePtr frame)
{
    // Release the GIL
    rogue::GilRelease noGil;

    std::size_t frameSize;

    // Hold the frame lock
    ris::FrameLockPtr lockFrame{frame->lock()};

    // Check for frames with errors or flags
    if ( frame->getError() || ( frame->getFlags() & 0x100 ) )
    {
        eLog_->error("Received frame with errors and/or flags");
        return;
    }

    // Get the frame size
    frameSize = frame->getPayload();

    // Check if the frame size is lower than the header size
    if ( frameSize < SmurfHeader<std::vector<uint8_t>::iterator>::SmurfHeaderSize )
    {
        eLog_->error("Received frame with size lower than the header size. Received frame size=%zu, expected header size=%zu",
            frameSize, SmurfHeader<std::vector<uint8_t>::iterator>::SmurfHeaderSize);
        return;
    }

    // Create a FrameAccessor object to access the frame data.
    ris::FrameIterator frameIt { frame->begin() };
    ris::FrameAccessor<uint8_t> frameAccessor { frameIt, static_cast<uint32_t>(frameSize) };

    // Do sanity checks on the incoming frame
    // - The frame has at least the header, so we can construct a (smart) pointer to it
    SmurfHeaderPtr<ris::FrameIterator> header { SmurfHeader<ris::FrameIterator>::create(frame) };

    // - Read the number of channel from the header
    uint32_t numChannels { header->getNumberChannels() };

    // - The incoming frame should have at least the supported maximum number of channels
    if ( numChannels < maxNumCh )
    {
         eLog_->error("Received frame with less channels that the maximum supported. Number of channels in received frame=%zu, supported maximum number of channels=%zu",
            numChannels, maxNumCh);
        return;
    }

    // - Check if the frame size is correct. The frame should have at least enough room to
    //   hold the number of channels defined in its header. Padded frames are allowed.
    if ( header->SmurfHeaderSize + (numChannels * sizeof(fw_t)) > frameSize )
    {
        eLog_->error("Received frame does not match expected size. Received frame size=%zu. Minimum expected sizes: header=%zu, payload=%i",
            frameSize, header->SmurfHeaderSize, numChannels * sizeof(fw_t));

        return;
    }

    // Acquire the channel mapper lock. We acquired here, so that is hold during the hold frame processing chain,
    // to avoid the 'numCh' parameter to be changed during that time.
    std::lock_guard<std::mutex> lockParam(mutChMapper);

    // Map and unwrap data at the same time
    {
        // Move the current data to the previous data
        previousData.swap(currentData);

        // Beginning of the data area in the frame, using the FrameAccessor
        std::vector<uint8_t>::const_iterator inIt(frameAccessor.begin() + SmurfHeader<std::vector<uint8_t>::iterator>::SmurfHeaderSize);

        // Build iterator to the data vectors
        std::vector<fw_t>::iterator     currentIt  { currentData.begin()  };
        std::vector<fw_t>::iterator     previousIt { previousData.begin() };
        std::vector<unwrap_t>::iterator inputIt    { inputData.begin()    };
        std::vector<unwrap_t>::iterator wrapIt     { wrapCounter.begin()  };

        // Acquire the lock while the unwrapper vectors are used.
        // Acquire this lock outside the loop, to increase performance.
        std::lock_guard<std::mutex> lock(mutUnwrapper);

        // Map and unwrap data in a single loop
        for(auto const& m : mask)
        {
            // Get the mapped value from the framweBuffer and cast it
            // Reinterpret the bytes from the frame to 'fw_t' values. And the cast that value to 'unwrap_t' values
            *currentIt = *(reinterpret_cast<const fw_t*>(&(*( inIt + m * sizeof(fw_t) ))));
            *inputIt = static_cast<unwrap_t>(*currentIt);

            // Unwrap the value is the unwrapper is not disabled.
            // If it is disabled, don't do anything to the data
            if (!disableUnwrapper)
            {
                // Check if the value wrapped
                if ((*currentIt > upperUnwrap) && (*previousIt < lowerUnwrap))
                {
                    // Decrement wrap counter
                    *wrapIt -= stepUnwrap;
                }
                else if ((*currentIt < lowerUnwrap) && (*previousIt > upperUnwrap))
                {
                    // Increment wrap counter
                    *wrapIt += stepUnwrap;
                }

                // Add the wrap counter to the value
                *inputIt += *wrapIt;
            }

            // increase output channel index
            ++currentIt;
            ++previousIt;
            ++inputIt;
            ++wrapIt;
        }

        // Update the number of channels in the header
        header->setNumberChannels(numCh);
    }

    // Filter data
    { // filter parameter lock scope
        // Filter the data, if the filter is not disabled.
        if (!disableFilter)
        {
            // Acquire the lock while the filter parameters are used.
            std::lock_guard<std::mutex> lockParam(mutFilter);

            // The pass sample buffer x and y have the following structure:
            //
            //  |     .....    |     .....    |      ...     |     .....    |
            //  ^              ^              ^              ^              ^
            //  |--------------|--------------|              |--------------|
            //     (order + 1)    (order + 1)                  (order + 1)
            //    Pass samples   Pass samples                  Pass samples
            //    of channel 0   of channel 1                  of channel (numCh-1)
            //  ^                                                           ^
            //  |-----------------------------------------------------------|
            //                    ( ( order + 1 ) * numCh )
            //
            // Each block of (order + 1) values container the current values plus the pass
            // (order) values of each channel, as a circular buffer.
            // The 'currentIndex' variable contains the index of the current sample (time = 0).
            // The value at index (currentIndex - 1) will be the sample at time = -1.
            // The value at index (currentIndex + 1) will be the sample at time = -order.

            // Copy the 'order' value to a local variable. This improve the performance of this function.
            std::size_t order_copy { order };

            // Move the 'currentIndex' index to the oldest slot in the buffer
            if (currentIndex == order_copy)
                currentIndex = 0;
            else
                ++currentIndex;

            // Create iterators
            std::vector<double>::iterator         xCh    { x.begin() + currentIndex };  // Current X value, first channel
            std::vector<double>::iterator         yCh    { y.begin() + currentIndex };  // Current Y value, first channel
            std::vector<double>::const_iterator   aIt    { a.begin()                };  // A coefficients
            std::vector<double>::const_iterator   bIt    { b.begin()                };  // B coefficients
            std::vector<unwrap_t>::const_iterator dataIt { inputData.begin()        };  // Input data

            // First, update the first points in the x and y buffers for all channels.

            // On each cycle, move the iterators to the next channel, which is in the
            // next block of size (order + 1)
            for (std::size_t ch{0}; ch < numCh; ++ch, xCh += order_copy + 1, yCh += order_copy + 1)
            {
                // New input value
                *xCh = static_cast<double>(*dataIt++);

                // Stat computing the output value
                *yCh= *bIt * *xCh;
            }

            // Now calculate the output value based on previous values from time -1 to -order
            // for all channels.

            // Index to the previous sample. It will be updated inside the loop
            std::size_t previousIndex { currentIndex };

            // Create iterator
            std::vector<double>::const_iterator xChPrev; // Previous X value, first channel
            std::vector<double>::const_iterator yChPrev; // Previous Y value, first channel

            // We first iterate over all the previous samples.
            for (std::size_t t{1}; t < order + 1; ++t)
            {
                // Get the index to the previous sample
                if (previousIndex == 0)
                    previousIndex = order_copy;
                else
                    --previousIndex;

                // Update the iterators each time we move to a previous sample
                yCh     = y.begin() + currentIndex;  // Go back to first channel
                xChPrev = x.begin() + previousIndex; // Go back to first channel
                yChPrev = y.begin() + previousIndex; // Go back to first channel
                ++aIt;                               // Go the next coefficient
                ++bIt;                               // Go the next coefficient

                // Update the output value for all channels. On each cycle, move the iterators
                // to the next channel, which is in the next block of size (order + 1)
                for (std::size_t ch{0}; ch < numCh; ++ch, yCh += order_copy + 1, xChPrev += order_copy + 1, yChPrev += order_copy + 1)
                {
                    *yCh += *bIt * *xChPrev - *aIt * *yChPrev;
                }
            }

            // Finally, divide the results for all channels by the coefficient a[0].

            // On each cycle, move the iterator to the next channel, which is in
            // the next block of size (order + 1)
            yCh = y.begin() + currentIndex;     // Go back to the first channel
            for (std::size_t ch{0}; ch < numCh; ++ch, yCh += order_copy + 1)
            {
                *yCh /= a[0];
            }
        }
    } // filter parameter lock scope

    // Downsample the data, if the downsampler is not disabled.
    // Otherwise, the data will be send on each cycle.
    if (!disableDownsampler)
    {
        // Downsampler. If we haven't reached the factor counter, we don't do anything
        // When we reach the factor counter, we send the resulting frame.
        if (++sampleCnt < factor)
            return;

        // Reset the downsampler
        resetDownsampler();
    }

    // Give the data to the Tx thread to be sent to the next slave.
    {
        // Copy the header
        std::copy(frameAccessor.begin(), frameAccessor.begin() + SmurfHeader<std::vector<uint8_t>::iterator>::SmurfHeaderSize, headerCopy.begin());

        // Copy the data
        {
            // Hold the mutex while we copy the data
            std::lock_guard<std::mutex> lock(outDataMutex);

            // Check if the filter was disabled. If it was disabled, use the 'inputData' vector as source.
            // Otherwise, use the 'y' vector, applying the gain and casting.
            if (disableFilter)
            {
                // Just cast the data type
                std::transform( inputData.begin(), inputData.end(), outData.begin(),
                    [this](const unwrap_t& v) -> filter_t { return static_cast<filter_t>(v); });
            }
            else
            {
                // Create iterators
                std::vector<double>::const_iterator yCh(y.begin() + currentIndex); // Filtered value, first channel
                std::vector<filter_t>::iterator     outIt(outData.begin());        // Destination

                // Multiply the values by the gain, and cast the result
                // to 'filter_t' into he outData buffer.
                // The current filtered values for each channel will be located at steps of (order+1) in the 'y' buffer.
                // On each cycle, move the iterator to the next channel, which is in
                // the next block of size (order + 1)
                for (std::size_t ch{0}; ch < numCh; ++ch, yCh += order + 1)
                {
                    *outIt++ = static_cast<filter_t>(*yCh * gain);
                }
            }
        }

        // Notify the Tx thread that new data is ready
        txDataReady = true;
        std::unique_lock<std::mutex> lock(txMutex);
        txCV.notify_all();
    }
}

void scp::SmurfProcessor::pktTansmitter()
{
    std::cout << "Transmitter thread started..." << std::endl;

    // Infinite loop
    for(;;)
    {
        // Check if new data is ready
        if ( !txDataReady )
        {
            // Wait until data is ready, with a 10s timeout
            std::unique_lock<std::mutex> lock(txMutex);
            txCV.wait_for( lock, std::chrono::seconds(10) );
        }
        else
        {
            // Output frame size. Start with the size of the header
            std::size_t outFrameSize = SmurfHeaderRO<std::vector<uint8_t>::iterator>::SmurfHeaderSize;

            // Extract the number of channels from the passed header
            SmurfHeaderROPtr<std::vector<uint8_t>::iterator> header { SmurfHeaderRO<std::vector<uint8_t>::iterator>::create(headerCopy) };
            std::size_t numChannels { header->getNumberChannels() };

            if (payloadSize > numChannels)
                // If the payload size is greater that the number of channels, then reserved
                // that number of channels in the output frame.
                outFrameSize += payloadSize * sizeof(filter_t);
            else
                // Otherwise, the size of the frame will only hold the number of channels
                outFrameSize += numChannels * sizeof(filter_t);

            // Request a new frame
            ris::FramePtr outFrame = reqFrame(outFrameSize, true);
            outFrame->setPayload(outFrameSize);
            ris::FrameIterator outFrameIt = outFrame->beginWrite();

            // Copy the header from the input frame to the output frame
            outFrameIt = std::copy(headerCopy.begin(), headerCopy.end(), outFrameIt);

            // Copy the data to the output frame
            {
                std::lock_guard<std::mutex> lock(outDataMutex);
                std::size_t i{0};
                for(auto it = outData.begin(); it != outData.end(); ++it)
                    helpers::setWord<filter_t>(outFrameIt, i++, *it);
            }

            // Send the frame to the next slave.
            sendFrame(outFrame);

            // Clear the flag
            txDataReady = false;
        }

        // Check if we should stop the loop
        if (!runTxThread)
        {
            std::cout << "pktTansmitter interrupted." << std::endl;
            return;
        }
    }
}


#ifndef MBGL_TEXT_TYPES
#define MBGL_TEXT_TYPES

#include <mbgl/util/vec.hpp>
#include <mbgl/util/rect.hpp>
#include <array>
#include <vector>

#include <boost/optional.hpp>

namespace mbgl {

typedef vec2<float> CollisionPoint;
typedef vec2<float> CollisionAnchor;

typedef std::array<float, 2> PlacementRange;
typedef float CollisionAngle;
typedef std::vector<CollisionAngle> CollisionAngles;
typedef std::array<CollisionAngle, 2> CollisionRange;
typedef std::vector<CollisionRange> CollisionList;
typedef std::array<CollisionPoint, 4> CollisionCorners;

struct CollisionRect {
    CollisionPoint tl;
    CollisionPoint br;
    inline explicit CollisionRect() {}
    inline explicit CollisionRect(CollisionPoint::Type ax,
                                  CollisionPoint::Type ay,
                                  CollisionPoint::Type bx,
                                  CollisionPoint::Type by)
        : tl(ax, ay), br(bx, by) {}
    inline explicit CollisionRect(const CollisionPoint &tl,
                                  const CollisionPoint &br)
        : tl(tl), br(br) {}
};

// These are the glyph boxes that we want to have placed.
struct GlyphBox {
    explicit GlyphBox() {}
    explicit GlyphBox(const CollisionRect &box,
                      const CollisionAnchor &anchor,
                      float minScale,
                      float maxScale,
                      float padding)
        : box(box), anchor(anchor), minScale(minScale), maxScale(maxScale), padding(padding) {}
    explicit GlyphBox(const CollisionRect &box,
                      float minScale,
                      float padding)
        : box(box), minScale(minScale), padding(padding) {}

    CollisionRect box;
    CollisionAnchor anchor;
    float minScale = 0.0f;
    float maxScale = std::numeric_limits<float>::infinity();
    float padding = 0.0f;
    boost::optional<CollisionRect> hBox;
};

typedef std::vector<GlyphBox> GlyphBoxes;

// TODO: Transform the vec2<float>s to vec2<int16_t> to save bytes
struct PlacedGlyph {
    explicit PlacedGlyph(const vec2<float> &tl, const vec2<float> &tr,
                      const vec2<float> &bl, const vec2<float> &br,
                      const Rect<uint16_t> &tex, float angle, const vec2<float> &anchor,
                      float minScale, float maxScale)
        : tl(tl),
          tr(tr),
          bl(bl),
          br(br),
          tex(tex),
          angle(angle),
          anchor(anchor),
          minScale(minScale),
          maxScale(maxScale) {}

    vec2<float> tl, tr, bl, br;
    Rect<uint16_t> tex;
    float angle;
    vec2<float> anchor;
    float minScale, maxScale;
};

typedef std::vector<PlacedGlyph> PlacedGlyphs;

// These are the placed boxes contained in the rtree.
struct PlacementBox {
    CollisionAnchor anchor;
    CollisionRect box;
    boost::optional<CollisionRect> hBox;
    PlacementRange placementRange = {{0.0f, 0.0f}};
    float placementScale = 0.0f;
    float maxScale = std::numeric_limits<float>::infinity();
    float padding = 0.0f;
};

struct PlacementProperty {
    explicit PlacementProperty() {}
    explicit PlacementProperty(float zoom, const PlacementRange &rotationRange)
        : zoom(zoom), rotationRange(rotationRange) {}

    inline operator bool() const {
        return !std::isnan(zoom) && zoom != std::numeric_limits<float>::infinity() &&
               rotationRange[0] != rotationRange[1];
    }

    float zoom = std::numeric_limits<float>::infinity();
    PlacementRange rotationRange = {{0.0f, 0.0f}};
};

}

#endif


/***************************************************************************
 * Copyright 1998-2020 by authors (see AUTHORS.txt)                        *
 *                                                                         *
 *   This file is part of LuxCoreRender.                                   *
 *                                                                         *
 * 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/format.hpp>

#include "slg/bsdf/bsdf.h"
#include "slg/lights/mappointlight.h"

using namespace std;
using namespace luxrays;
using namespace slg;

//------------------------------------------------------------------------------
// MapPointLight
//------------------------------------------------------------------------------

MapPointLight::MapPointLight() : imageMap(NULL), func(NULL) {
}

MapPointLight::~MapPointLight() {
	delete func;
}

void MapPointLight::Preprocess() {
	PointLight::Preprocess();

	delete func;
	func = new SampleableSphericalFunction(new ImageMapSphericalFunction(imageMap));
}

void MapPointLight::GetPreprocessedData(float *localPosData, float *absolutePosData,
		float *emittedFactorData, const SampleableSphericalFunction **funcData) const {
	PointLight::GetPreprocessedData(localPosData, absolutePosData, emittedFactorData);

	if (funcData)
		*funcData = func;
}

float MapPointLight::GetPower(const Scene &scene) const {
	return imageMap->GetSpectrumMeanY() * PointLight::GetPower(scene);
}

Spectrum MapPointLight::Emit(const Scene &scene,
		const float time, const float u0, const float u1,
		const float u2, const float u3, const float passThroughEvent,
		Ray &ray, float &emissionPdfW,
		float *directPdfA, float *cosThetaAtLight) const {
	const Point rayOrig = absolutePos;

	Vector localFromLight;
	func->Sample(u0, u1, &localFromLight, &emissionPdfW);
	if (emissionPdfW == 0.f)
		return Spectrum();

	const Vector rayDir = Normalize(lightToWorld * localFromLight);

	if (directPdfA)
		*directPdfA = 1.f;
	if (cosThetaAtLight)
		*cosThetaAtLight = 1.f;

	ray.Update(rayOrig, rayDir, time);

	return emittedFactor * ((SphericalFunction *)func)->Evaluate(localFromLight) /
			(4.f * M_PI * func->Average());
}

Spectrum MapPointLight::Illuminate(const Scene &scene, const BSDF &bsdf,
		const float time, const float u0, const float u1, const float passThroughEvent,
        Ray &shadowRay, float &directPdfW,
		float *emissionPdfW, float *cosThetaAtLight) const {
	const Point shadowRayOrig = bsdf.GetRayOrigin(absolutePos - bsdf.hitPoint.p);
	const Vector localFromLight = Normalize(Inverse(lightToWorld) * shadowRayOrig - localPos);
	const float funcPdf = func->Pdf(localFromLight);
	if (funcPdf == 0.f)
		return Spectrum();

	const Vector toLight(absolutePos - shadowRayOrig);
	const float centerDistanceSquared = toLight.LengthSquared();
	const float shadowRayDistance = sqrtf(centerDistanceSquared);
	const Vector shadowRayDir = toLight / shadowRayDistance;

	if (cosThetaAtLight)
		*cosThetaAtLight = 1.f;

	directPdfW = centerDistanceSquared;

	if (emissionPdfW)
		*emissionPdfW = funcPdf;

	shadowRay = Ray(shadowRayOrig, shadowRayDir, 0.f, shadowRayDistance, time);

	return emittedFactor * ((SphericalFunction *)func)->Evaluate(localFromLight) /
			(4.f * M_PI * func->Average());
}

Properties MapPointLight::ToProperties(const ImageMapCache &imgMapCache, const bool useRealFileName) const {
	const string prefix = "scene.lights." + GetName();
	Properties props = PointLight::ToProperties(imgMapCache, useRealFileName);

	props.Set(Property(prefix + ".type")("mappoint"));
	const string fileName = useRealFileName ?
		imageMap->GetName() : imgMapCache.GetSequenceFileName(imageMap);
	props.Set(Property(prefix + ".mapfile")(fileName));
	props.Set(imageMap->ToProperties(prefix, false));

	return props;
}


// Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

// 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 UUID_8AC84A2217C411E0A7AF3A1BDFD72085
#define UUID_8AC84A2217C411E0A7AF3A1BDFD72085

#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/enable_if.hpp>
#include <boost/qvm/inline.hpp>
#include <boost/qvm/quat_traits.hpp>
#include <boost/qvm/static_assert.hpp>

namespace boost {
namespace qvm {
////////////////////////////////////////////////

namespace qvm_detail {
template <class Q> struct quat_v_ {
  template <class R> operator R() const {
    R r;
    assign(r, *this);
    return r;
  }

private:
  quat_v_(quat_v_ const &);
  quat_v_ const &operator=(quat_v_ const &);
  ~quat_v_();
};
} // namespace qvm_detail

template <class V> struct vec_traits;

template <class Q> struct vec_traits<qvm_detail::quat_v_<Q>> {
  typedef qvm_detail::quat_v_<Q> this_vector;
  typedef typename quat_traits<Q>::scalar_type scalar_type;
  static int const dim = 3;

  template <int I>
  BOOST_QVM_INLINE_CRITICAL static scalar_type
  read_element(this_vector const &q) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return quat_traits<Q>::template read_element<I + 1>(
        reinterpret_cast<Q const &>(q));
  }

  template <int I>
  BOOST_QVM_INLINE_CRITICAL static scalar_type &write_element(this_vector &q) {
    BOOST_QVM_STATIC_ASSERT(I >= 0);
    BOOST_QVM_STATIC_ASSERT(I < dim);
    return quat_traits<Q>::template write_element<I + 1>(
        reinterpret_cast<Q &>(q));
  }
};

template <class Q, int D> struct deduce_vec<qvm_detail::quat_v_<Q>, D> {
  typedef vec<typename quat_traits<Q>::scalar_type, D> type;
};

template <class Q, int D>
struct deduce_vec2<qvm_detail::quat_v_<Q>, qvm_detail::quat_v_<Q>, D> {
  typedef vec<typename quat_traits<Q>::scalar_type, D> type;
};

template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value,
                         qvm_detail::quat_v_<Q> const &>::type
    V(Q const &a) {
  return reinterpret_cast<qvm_detail::quat_v_<Q> const &>(a);
}

template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value, qvm_detail::quat_v_<Q> &>::type
    V(Q &a) {
  return reinterpret_cast<qvm_detail::quat_v_<Q> &>(a);
}

template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value,
                         typename quat_traits<Q>::scalar_type>::type
    S(Q const &a) {
  return quat_traits<Q>::template read_element<0>(a);
}
template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value,
                         typename quat_traits<Q>::scalar_type>::type
    X(Q const &a) {
  return quat_traits<Q>::template read_element<1>(a);
}
template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value,
                         typename quat_traits<Q>::scalar_type>::type
    Y(Q const &a) {
  return quat_traits<Q>::template read_element<2>(a);
}
template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value,
                         typename quat_traits<Q>::scalar_type>::type
    Z(Q const &a) {
  return quat_traits<Q>::template read_element<3>(a);
}

template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value,
                         typename quat_traits<Q>::scalar_type &>::type
    S(Q &a) {
  return quat_traits<Q>::template write_element<0>(a);
}
template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value,
                         typename quat_traits<Q>::scalar_type &>::type
    X(Q &a) {
  return quat_traits<Q>::template write_element<1>(a);
}
template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value,
                         typename quat_traits<Q>::scalar_type &>::type
    Y(Q &a) {
  return quat_traits<Q>::template write_element<2>(a);
}
template <class Q>
BOOST_QVM_INLINE_TRIVIAL
    typename enable_if_c<is_quat<Q>::value,
                         typename quat_traits<Q>::scalar_type &>::type
    Z(Q &a) {
  return quat_traits<Q>::template write_element<3>(a);
}

////////////////////////////////////////////////
} // namespace qvm
} // namespace boost

#endif


/**
 * @file image.hpp
 * @copyright Copyright (c) 2020 Nicolas Pope, MIT License
 * @author Nicolas Pope
 */

#pragma once

#include "../defines.hpp"

#include <voltu/types/channel.hpp>
#include <voltu/types/intrinsics.hpp>
#include <memory>

#include <Eigen/Eigen>

namespace voltu
{

enum class ImageFormat
{
	kInvalid = 0,
	kFloat32 = 1,
	kBGRA8 = 2,
	kFloat16_4 = 3
};

PY_NO_SHARED_PTR struct ImageData
{
	ImageFormat format = ImageFormat::kInvalid;
	unsigned char* data = nullptr;
	unsigned int pitch = 0;
	unsigned int width = 0;
	unsigned int height = 0;
};

class Image
{
public:
	virtual ~Image() = default;

	PY_API PY_RV_LIFETIME_PARENT virtual ImageData getHost() = 0;

	virtual ImageData getDevice() = 0;

	virtual bool isDevice() = 0;

	PY_API virtual voltu::Channel getChannel() = 0;

	PY_API virtual std::string getName() = 0;

	PY_API virtual voltu::Intrinsics getIntrinsics() = 0;

	PY_API virtual Eigen::Matrix4d getPose() = 0;

	PY_API virtual voltu::StereoIntrinsics getStereoIntrinsics() = 0;

	PY_API virtual int64_t getTimestamp() = 0;

	//virtual voltu::RoomId getRoomId() = 0;

	PY_API virtual int getCameraNumber() = 0;

	PY_API virtual uint32_t getUniqueId() = 0;
};

typedef std::shared_ptr<Image> ImagePtr;

}