#include <unordered_map>
#include <iostream>
#include <string>
using std::string;

#include <boost/log/utility/setup/console.hpp>
#include <boost/log/trivial.hpp>

#include "streamTrace.hpp"
#include "navigation.hpp"
#include "constants.hpp"
#include "station.hpp"
#include "antenna.hpp"
#include "satSys.hpp"
#include "tides.hpp"
#include "rinex.hpp"
#include "gTime.hpp"

#define J2_GLO   1.0826257E-3     /* 2nd zonal harmonic of geopot   ref [2] */

#define OMGE_GLO 7.292115E-5      /* earth angular velocity (rad/s) ref [2] */
#define OMGE_GAL 7.2921151467E-5  /* earth angular velocity (rad/s) ref [7] */
#define OMGE_CMP 7.292115E-5      /* earth angular velocity (rad/s) ref [9] */

#define RTOL_KEPLER 	1E-14         /* relative tolerance for Kepler equation */
#define MAX_ITER_KEPLER 30        /* max number of iteration of Kelpler */
#define GLOSTEP   		60.0

#define SIN_5 -0.0871557427476582 /* sin(-5.0 deg) */
#define COS_5  0.9961946980917456 /* cos(-5.0 deg) */

nav_t	nav = {};
string			GNSSinp="GE";
map<SatSys,int> BS_Satel_List;
int 			BS_Start_Week		= 0;
double			BS_Start_TOW		= 0.0;
double			BS_Start_Epoc[6]	= {0};
int 			BS_Epoch_Num		= 0;
double			BS_Epoch_Inter		= 900.0;
map<E_Sys,double> BS_Max_Dtime;

void helpstring(void)
{
	fprintf(stdout, " brdc2sp3 -ant <ATX file> -inp <RINEX NAV file> [options]\n");
	fprintf(stdout, " Options [default]:\n");
	fprintf(stdout, "     -gns string   GNSS to be processed: G: GPS, R:GLONASS, E:Galileo, C: Beidou, J: QZSS, A: all [GE]\n");
	fprintf(stdout, "     -int float    Time spacing of SP3 entries [900.0]\n");
	fprintf(stdout, "     -out string   File name for SP3 output [BS_orbits.sp3]\n");
	fprintf(stdout, "     -*dt max_dt   set max_dt for constellation (*: 'G','R','E','J' or 'C') [86400.0]\n");
	fprintf(stdout, "     -clst         Select ephemeris with closest Toe (within max_dt seconds) [Latest valid ephemeris]\n");
	fprintf(stdout, "     -inav         I/NAV only for Galileo [use both F/NAV and I/NAV]\n");
	fprintf(stdout, "     -fnav         F/NAV only for Galileo [use both F/NAV and I/NAV]\n");
	fprintf(stdout, "     -frst         Filter out satellites with no/bad data in first epoc [not filtered]\n");
}

int BS_satantoff( GTime time, SatSys Sat, Vector3d& rs, Vector3d& dant, int gloind = 0)
{
	double lam1, lam2;

	char id[5];
	Sat.getId(id);

	PhaseCenterData* pcsat = findAntenna(Sat.id(), time, nav);

	if (pcsat == nullptr) return -1;
	auto& pcoMap = pcsat->pcoMap;

	E_FType j = F1;
	E_FType k = F2;
	double glonfrq1 = FREQ1_GLO + DFRQ1_GLO*gloind;
	double glonfrq2 = FREQ2_GLO + DFRQ2_GLO*gloind;
	switch (Sat.sys)
	{
		case E_Sys::GPS: lam1 = CLIGHT/FREQ1; 		lam2 = CLIGHT/FREQ2;			break;
		case E_Sys::GLO: lam1 = CLIGHT/glonfrq1; 	lam2 = CLIGHT/glonfrq2;			break;
		case E_Sys::QZS: lam1 = CLIGHT/FREQ1; 		lam2 = CLIGHT/FREQ2;			break;
		case E_Sys::GAL: lam1 = CLIGHT/FREQ1; 		lam2 = CLIGHT/FREQ5;	k=F5;	break;
		case E_Sys::BDS: lam1 = CLIGHT/FREQ1_CMP; 	lam2 = CLIGHT/FREQ2_CMP; 		break;
		default:     	 return -2;
	}

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

	/* unit vectors of satellite fixed coordinates */
	Vector3d r = -rs;
	Vector3d ez = r.normalized();
	r = rsun - rs;
	Vector3d es = r.normalized();
	r = ez.cross(es);
	Vector3d ey = r.normalized();
	Vector3d ex = ey.cross(ez);

	double gamma	= lam2*lam2/lam1/lam1;
	double C1		= gamma	/ (gamma - 1);
	double C2		= -1	/ (gamma - 1);

	/* iono-free LC */
	for (int i = 0; i < 3; i++)
	{
		/* ENU to NEU */
		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];
		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);

		fprintf( stdout,"\n  ATX: %s %11.6f %11.6f %11.6f  %11.6f %11.6f %11.6f", Sat.id().c_str(), pcoJ[0], pcoJ[1], pcoJ[2], pcoK[0], pcoK[1], pcoK[2]);

		//dant(i)	= C1 * dant1
		//		+ C2 * dant2;
		dant(i) = dant1;
	}
	return 0;
}

Eph* BS_seleph( GTime time, SatSys Sat, int iode, nav_t& nav_, int opt=0)
{
	double valid = 0;

	switch (Sat.sys)
	{
		case E_Sys::QZS:	valid = MAXDTOE_QZS	+ 1; break;
		case E_Sys::GAL:	valid = MAXDTOE_GAL	+ 1; break;
		case E_Sys::BDS:	valid = MAXDTOE_CMP	+ 1; break;
		default: 			valid = MAXDTOE		+ 1; break;
	}

	auto& ephList = nav_.ephMap[Sat];
	Eph* chosen = nullptr;
	GTime latestToe = GTime::noTime();
	double max_dtime = BS_Max_Dtime[Sat.sys];
	for (auto& [dummy, eph] : ephList)
	{
		if 	( iode >= 0 )
		{
			if(iode == eph.iode) return &eph;
			else continue;
		}
		//fprintf(stdout, "    %s %s\n",Sat.id().c_str(),eph.toe.to_string(0).c_str());
		if(opt == 0)
		{
			if (fabs(eph.toe - time) > valid)
				continue;
			if (eph.toe > latestToe)
			{
				chosen	= &eph;
				latestToe = eph.toe;
			}
		}
		if(opt==1)
		{
			double dtime=max_dtime;
			if (Sat.sys == +E_Sys::GPS)	dtime = fabs(eph.toe - (time + 3600));
			else						dtime = fabs(eph.toe - time);

			if ( max_dtime > dtime)
			{
				chosen	= &eph;
				latestToe = eph.toe;
				max_dtime = dtime;
			}
		}

	}

	return chosen;
}

Geph* BS_selgeph( GTime time, SatSys Sat, int iode, nav_t& nav_, int opt=0)
{
	double valid = MAXDTOE_GLO	+ 1;

	auto& gephList = nav_.gephMap[Sat];
	Geph* chosen = nullptr;
	GTime latestToe = GTime::noTime();
	double max_dtime = BS_Max_Dtime[E_Sys::GLO];
	for (auto& [dummy, geph] : gephList)
	{
		if 	( iode >= 0 )
		{
			if(iode == geph.iode) 	return &geph;
			else 					continue;
		}
		//fprintf(stdout, "    %s %s\n",Sat.id().c_str(),geph.toe.to_string(0).c_str());
		if(opt==0)
		{
			if (fabs(geph.toe - time) > valid)
				continue;
			if (geph.toe > latestToe)
			{
				chosen	= &geph;
				latestToe = geph.toe;
			}
		}
		if(opt==1)
		{
			double dtime = fabs(geph.toe - time);
			if ( max_dtime > dtime)
			{
				chosen	= &geph;
				latestToe = geph.toe;
				max_dtime = dtime;
			}
		}
	}

	return chosen;
}

/* glonass orbit differential equations --------------------------------------*/
void deq(const double* x, double* xdot, Vector3d& acc)
{
	double a, b, c, r2 = dot(x, x, 3), r3 = r2 * sqrt(r2), omg2 = OMGE_GLO*OMGE_GLO;

	if (r2 <= 0.0)
	{
		xdot[0] = xdot[1] = xdot[2] = xdot[3] = xdot[4] = xdot[5] = 0.0;
		return;
	}

	/* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */
	a = 1.5 * J2_GLO * MU_GLO * RE_GLO*RE_GLO / r2 / r3; /* 3/2*J2*mu*Ae^2/r^5 */
	b = 5.0 * x[2] * x[2] / r2;            /* 5*z^2/r^2 */
	c = -MU_GLO / r3 - a * (1.0 - b);      /* -mu/r^3-a(1-b) */
	xdot[0] = x[3];
	xdot[1] = x[4];
	xdot[2] = x[5];
	xdot[3] = (c + omg2) * x[0] + 2.0 * OMGE_GLO * x[4] + acc[0];
	xdot[4] = (c + omg2) * x[1] - 2.0 * OMGE_GLO * x[3] + acc[1];
	xdot[5] = (c - 2.0 * a) * x[2] + acc[2];
}

/* glonass position and velocity by numerical integration --------------------*/
void glorbit(double t, double* x, Vector3d& acc)
{
	double k1[6], k2[6], k3[6], k4[6], w[6];
	int i;

	deq(x, k1, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k1[i] * t / 2;
	deq(w, k2, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k2[i] * t / 2;
	deq(w, k3, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k3[i] * t;
	deq(w, k4, acc);

	for (i = 0; i < 6; i++)
		x[i] += (k1[i] + 2 * k2[i] + 2 * k3[i] + k4[i]) * t / 6;
}

int BS_geph2pos( GTime	time, Geph* geph, Vector3d& rs, double*	dts)
{
	double t = time - geph->toe;
	*dts	=  geph->taun
			+  geph->gamn * t;

	double x[6] = {0};
	for (int i = 0; i < 3; i++)
	{
		x[i  ] = geph->pos[i];
		x[i+3] = geph->vel[i];
	}

	for (double tt = t < 0 ? -GLOSTEP : GLOSTEP; fabs(t) > 1E-9; t -= tt)
	{
		if (fabs(t) < GLOSTEP) tt=t;
		glorbit(tt, x, geph->acc);
	}

	for (int i = 0; i < 3; i++) rs(i) = x[i];

	return 0;
}

int BS_eph2pos( GTime	time, Eph* eph, Vector3d& rs, double*	dts) {

	if (eph->A <= 0) return -1;

	double tk = time - eph->toe;
	int prn = eph->Sat.prn;
	int sys = eph->Sat.sys;

	double mu;
	double omge;
	switch (sys)
	{
		case E_Sys::GAL: mu = MU_GAL; omge = OMGE_GAL; break;
		case E_Sys::BDS: mu = MU_CMP; omge = OMGE_CMP; break;
		default:     	 mu = MU_GPS; omge = OMGE;     break;
	}

	double M = eph->M0 + (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) * tk;

	double E	= M;
	double Ek	= 0;
	int n;
	for (n = 0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++)
	{
		Ek = E;
		E -= (E - eph->e * sin(E) - M) / (1 - eph->e * cos(E));
	}

	if (n >= MAX_ITER_KEPLER)
	{
		fprintf(stderr,"WARNING for %s ephemeris: iteration divergent", eph->Sat.id().c_str());
		return -1;
	}

	double sinE  = sin(E);
	double cosE  = cos(E);
	double u 	 = atan2(sqrt(1 - eph->e * eph->e) * sinE, cosE - eph->e) + eph->omg;
	double r 	 = eph->A * (1 - eph->e * cosE);
	double i	 = eph->i0 + eph->idot * tk;
	double sin2u = sin(2 * u);
	double cos2u = cos(2 * u);

	u 			+= eph->cus * sin2u + eph->cuc * cos2u;
	r 			+= eph->crs * sin2u + eph->crc * cos2u;
	i 			+= eph->cis * sin2u + eph->cic * cos2u;

	double x	 = r * cos(u);
	double y	 = r * sin(u);
	double cosi  = cos(i);

	/* beidou geo satellite */
	if (sys == +E_Sys::BDS && prn <= 5)
	{
		double O	= eph->OMG0
					+ eph->OMGd * tk
					- omge * eph->toes;
		double sinO = sin(O);
		double cosO = cos(O);
		double xg = x * cosO - y * cosi * sinO;
		double yg = x * sinO + y * cosi * cosO;
		double zg = y * sin(i);
		double sino = sin(omge * tk);
		double coso = cos(omge * tk);
		rs(0) = +xg * coso + yg * sino * COS_5 + zg * sino * SIN_5;
		rs(1) = -xg * sino + yg * coso * COS_5 + zg * coso * SIN_5;
		rs(2) = -yg * SIN_5 + zg * COS_5;
	}
	else
	{
		double O	= eph->OMG0
					+ (eph->OMGd - omge) * tk
					- omge * eph->toes;
		double sinO = sin(O);
		double cosO = cos(O);
		rs(0) = x * cosO - y * cosi * sinO;
		rs(1) = x * sinO + y * cosi * cosO;
		rs(2) = y * sin(i);
	}

	tk = time - eph->toc;
	*dts	= eph->f0
			+ eph->f1 * tk
			+ eph->f2 * tk * tk;
	return 0;
}

void print_SP3header(FILE* fpout)
{
	/* line one */
	fprintf(fpout,"#dP%4.0f %2.0f %2.0f %2.0f %2.0f %11.8f ", BS_Start_Epoc[0], BS_Start_Epoc[1], BS_Start_Epoc[2], BS_Start_Epoc[3], BS_Start_Epoc[4], BS_Start_Epoc[5]);
	fprintf(fpout,"%7d ORBIT WGS84 BCT   GA", BS_Epoch_Num);

	/* Line two */
	double mjdate = 7.0*BS_Start_Week + BS_Start_TOW/86400.0 + 44244.0;
	fprintf(fpout,"\n## %4d %15.8f %5.0f %15.13f", BS_Start_Week, BS_Start_TOW, mjdate, mjdate-floor(mjdate));

	/* satellite lines */
	int NumSatLeft = 85, satind=0, satinline=0;
	if(BS_Satel_List.size()>85) NumSatLeft = BS_Satel_List.size();
	auto it = BS_Satel_List.begin();
	while(satind < NumSatLeft || satinline > 0)
	{
		if(satind == 0) fprintf(fpout, "\n+  %3d   ", (int)BS_Satel_List.size());
		else if(satinline == 0) fprintf(fpout, "\n+        ");

		if(it==BS_Satel_List.end()) fprintf(fpout, "  0");
		else
		{
			fprintf(fpout,"%s",it->first.id().c_str());
			it++;
		}

		satind++;
		if(satinline<16) satinline++;
		else satinline = 0;
	}

	/* accuracy lines */
	satind = 0;
	satinline = 0;
	while(satind<NumSatLeft)
	{
		if(satinline == 0) fprintf(fpout, "\n++       ");

		fprintf(fpout, "  0");

		satind++;
		if(satinline<16) satinline++;
		else satinline = 0;
	}

	/* char variable lines */
	if(GNSSinp.size() > 1) fprintf(fpout, "\n%%c M ");
	else fprintf(fpout, "\n%%c %s ", GNSSinp.c_str());
	fprintf(fpout, " cc GPS ccc cccc cccc cccc cccc ccccc ccccc ccccc ccccc");
	fprintf(fpout, "\n%%c cc cc GPS ccc cccc cccc cccc cccc ccccc ccccc ccccc ccccc");

	/* float variable lines */
	fprintf(fpout, "\n%%f  1.2500000  1.025000000  0.00000000000  0.000000000000000");
	fprintf(fpout, "\n%%f  0.0000000  0.000000000  0.00000000000  0.000000000000000");

	/* float variable lines */
	fprintf(fpout, "\n%%i    0    0    0    0      0      0      0      0         0");
	fprintf(fpout, "\n%%i    0    0    0    0      0      0      0      0         0");

	fprintf(fpout, "\n/* Created using Ginan at: %s. ", timeget().to_string(0).c_str());
	fprintf(fpout, "\n/* WARNING: For Geoscience Australia's internal use only");
}

void print_SP3epoch(FILE* fpout, GTime tsync, map<SatSys, vector<double>>& temp_eph)
{
	double ep[6];
	time2epoch(tsync, ep);
	fprintf(fpout,"\n*  %4.0f %2.0f %2.0f %2.0f %2.0f %11.8f ", ep[0], ep[1], ep[2], ep[3], ep[4], ep[5]);

	for(auto& [sat, neph] : BS_Satel_List){
		if(temp_eph.find(sat) == temp_eph.end())
			fprintf(fpout, "\nP%s%14.6f%14.6f%14.6f 999999.999999                    ", sat.id().c_str(), 0.0, 0.0, 0.0);
		else
		{
			auto vect = temp_eph[sat];
			fprintf(fpout, "\nP%s%14.6f%14.6f%14.6f%14.6f                    ", sat.id().c_str(), vect[0]/1000, vect[1]/1000, vect[2]/1000, vect[3]*1e6);
		}
	}
}

int main(int argc, char **argv)
{
	boost::log::core::get()->set_filter (boost::log::trivial::severity >= boost::log::trivial::info);
	string atxfile = "igs14.atx";
	vector<string> navfiles;
	int GALSelection = 7;
	/* Argument parsing */
	string inpfile;
    string outfile = "BS_orbits.sp3";
    bool fstreq = false;
    BS_Max_Dtime[E_Sys::GPS] = 86400.0;
    BS_Max_Dtime[E_Sys::GLO] = 86400.0;
    BS_Max_Dtime[E_Sys::GAL] = 86400.0;
    BS_Max_Dtime[E_Sys::QZS] = 86400.0;
    BS_Max_Dtime[E_Sys::BDS] = 86400.0;
    int ephopt = 0;
    for (int i = 1; i < argc; i++)
    {
        if      (!strcmp(argv[i], "-inp") && i+1 < argc)
        {
        	inpfile.assign(argv[++i]);
        	navfiles.push_back(inpfile);
        }
        else if (!strcmp(argv[i], "-out") && i+1<argc) outfile.assign(argv[++i]);
        else if (!strcmp(argv[i], "-ant") && i+1<argc) atxfile.assign(argv[++i]);
        else if (!strcmp(argv[i], "-gns") && i+1<argc) GNSSinp.assign(argv[++i]);
        else if (!strcmp(argv[i], "-int") && i+1<argc) BS_Epoch_Inter = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-Gdt") && i+1<argc) BS_Max_Dtime[E_Sys::GPS] = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-Rdt") && i+1<argc) BS_Max_Dtime[E_Sys::GLO] = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-Edt") && i+1<argc) BS_Max_Dtime[E_Sys::GAL] = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-Jdt") && i+1<argc) BS_Max_Dtime[E_Sys::QZS] = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-Cdt") && i+1<argc) BS_Max_Dtime[E_Sys::BDS] = atof(argv[++i]);
    	else if (!strcmp(argv[i], "-clst"))			   ephopt = 1;
    	else if (!strcmp(argv[i], "-inav"))            GALSelection = 5;
        else if (!strcmp(argv[i], "-fnav"))  		   GALSelection = 2;
        else if (!strcmp(argv[i], "-frst"))			   fstreq=true;
        else if (!strcmp(argv[i], "-help"))
        {
        	helpstring();
        	exit(0);
        }
    }
	if(BS_Epoch_Inter <= 0.0) BS_Epoch_Inter = 900.0;

	bool pass = readantexf(atxfile, nav);
	if(!pass)
	{
		fprintf(stderr, "ERROR: invalid ANTEX file %s\n", atxfile.c_str());
		exit(0);
	}
	std::cout << "ATX file " << atxfile << std::endl;

	map<E_Sys,bool> GNSS_on;
    if (GNSSinp.find("A") != std::string::npos) GNSSinp = "GRECJ"; //All constellations
    if (GNSSinp.find("G") != std::string::npos) GNSS_on[E_Sys::GPS] = true; //GPS
    if (GNSSinp.find("R") != std::string::npos) GNSS_on[E_Sys::GLO] = true; //GLONASS
    if (GNSSinp.find("E") != std::string::npos) GNSS_on[E_Sys::GAL] = true; //Galileo
    if (GNSSinp.find("C") != std::string::npos) GNSS_on[E_Sys::BDS] = true; //Beidou
    if (GNSSinp.find("J") != std::string::npos) GNSS_on[E_Sys::QZS] = true; //QZSS
    if (GNSS_on.empty())
	{
        fprintf(stderr, "No GNSS selected\n");
        exit(0);
    }
	std::cout << "Processed GNSS " << GNSSinp << std::endl;

	char			type = 'N';
	ObsList 		obsList;
	RinexStation	sta;
	double			version = 0;
	E_Sys			sys;
	int 			tsys;
	map<E_Sys, vector<CodeType>>	sysCodeTypes;
	int 			nfil = 0;

	for (auto& file : navfiles)
	{
		std::ifstream	inputStream;
		inputStream.open(file, std::ifstream::in);
		int info = readrnx(inputStream, type , obsList, nav, &sta, version, sys, tsys, sysCodeTypes);
		if (info == false)
		{
			std::cout << "ERROR: invalid BRDC file " << file.c_str() << std::endl;
		}
		else{
			std::cout << "Processed file " << file << " " << type << " " << version << " " << std::endl;
			nfil++;
			info = readrnx(inputStream, type , obsList, nav, &sta, version, sys, tsys, sysCodeTypes);
		}
	}

	if(nfil == 0)
	{
		std::cout << "No valid Nav. files" << std::endl;
		exit(0);
	}

	FILE* sp3fp=fopen(outfile.c_str(),"wt");
	if(!sp3fp)
	{
		std::cout << "Cannot open output file: " << outfile << std::endl;
		exit(0);
	}
	std::cout << "Output file: " << outfile << std::endl;

	GTime tstart = GTime::noTime();
	GTime tfinsh = GTime::noTime();

	for(auto& [satint,ephlist] : nav.ephMap)
	{
		SatSys sat;
		sat.fromHash(satint);
		if(!GNSS_on[sat.sys]) continue;
		int neph = 0;
		for(auto it = nav.ephMap[satint].begin(); it != nav.ephMap[satint].end();)
		{
			auto [dummy, eph] = *it;
			double dtoc = eph.toe - eph.toc;
			double dttr = eph.toe - eph.ttr;
			bool alert = false;

			if(dtoc !=0.0) alert = true;
			if(fabs(dttr)>10000.0) alert = true;

			if(sys == +E_Sys::GAL && !(eph.code & GALSelection)) alert=true;		/* INAVs messages, change from 5 to 2 for FNAVs (we dont want to mix the two) */

			if(alert)
			{
				it=nav.ephMap[satint].erase(it);
			}
			else
			{
				//fprintf(stdout, "%s, %s, %d, %.1f, %s\n", sat.id().c_str(), eph.toe.to_string(0).c_str(), eph.iode, dtoc, eph.ttr.to_string(0).c_str());
				neph++;
				if (tstart == GTime::noTime() || tstart > eph.toe)		tstart = eph.toe;
				if (tfinsh == GTime::noTime() || tfinsh < eph.toe)		tfinsh = eph.toe;
				it++;
			}
		}
		if(neph>0) BS_Satel_List[sat]=neph;
	}

	if(GNSS_on[E_Sys::GLO]) for(auto& [satint,gephlist] : nav.gephMap)
	{
		SatSys sat;
		sat.fromHash(satint);
		int neph = 0;
		for(auto it = nav.gephMap[satint].begin(); it != nav.gephMap[satint].end();){
			auto& [dummy, geph] = *it;
			double dtoc = geph.toe - geph.tof;
			bool alert = false;

			//if(dtoc !=0.0) alert=true;
			//if(fabs(dttr)>10000.0) alert=true;

			if(alert){
				it = nav.gephMap[satint].erase(it);
				continue;
			}
			//fprintf(stdout, "%s %s %d, %.1f, %d\n", sat.id().c_str(), geph.toe.to_string(0).c_str(), geph.iode, dtoc, geph.svh);

			neph++;
			if (tstart == GTime::noTime() || tstart > geph.toe ) 	tstart = geph.toe;
			if (tfinsh == GTime::noTime() || tfinsh < geph.toe ) 	tfinsh = geph.toe;
			it++;
		}

		if (neph > 0)
			BS_Satel_List[sat] = neph;
	}

	double StarTow	= time2gpst(tstart, &BS_Start_Week);
	//BS_Start_TOW	= BS_Epoch_Inter*floor(StarTow/BS_Epoch_Inter);
	BS_Start_TOW	= 3600.0 * floor(StarTow / 3600.0);
	GTime tsync 	= gpst2time(BS_Start_Week, BS_Start_TOW);
	BS_Epoch_Num	= (int)(floor((tfinsh - tsync)/BS_Epoch_Inter) + 1);
	time2epoch(tsync, BS_Start_Epoc);

	fprintf(stdout, "\nTstart= %s, Tini= %s, Nepoc=%d\n", tstart.to_string(0).c_str(), tsync.to_string(0).c_str(), BS_Epoch_Num);

	if(fstreq)for(auto it = BS_Satel_List.begin(); it != BS_Satel_List.end();)
	{
		SatSys sat = it->first;
		bool nofst = false;
		Vector3d rs(0, 0, 0);
		Vector3d dant(0, 0, 0);
		double dts;
		if(sat.sys == +E_Sys::GLO)
		{
			Geph* gephp=BS_selgeph( tsync, sat, -1, nav);
			if(!gephp) nofst = true;
			else if(BS_geph2pos( tsync, gephp, rs, &dts) < 0) nofst = true;
			else if(rs.norm() < RE_WGS84) nofst = true;
			else if(BS_satantoff( tsync, sat, rs, dant, gephp->frq ) < 0) nofst = true;
			else nofst = false;
		}
		else
		{
			Eph* ephp=BS_seleph( tsync, sat, -1, nav);
			if(!ephp) nofst = true;
			else if(BS_eph2pos( tsync, ephp, rs, &dts) < 0) nofst = true;
			else if(rs.norm() < RE_WGS84) nofst = true;
			else if(BS_satantoff( tsync, sat, rs, dant, 0 ) < 0) nofst = true;
			else nofst = false;
		}

		if(nofst){
			it = BS_Satel_List.erase(it);
		}
		else{
			++it;
		}
	}

	print_SP3header(sp3fp);

	for(int epc=0; epc<BS_Epoch_Num; epc++){
		fprintf(stdout, "\n%s", tsync.to_string(0).c_str());
		map<SatSys,vector<double>> temp_eph;
		for(auto& [sat,neph] : BS_Satel_List){
			Vector3d rs(0, 0, 0);
			Vector3d dant(0, 0, 0);
			double dts;

			if(sat.sys == +E_Sys::GLO){
				Geph* gephp=BS_selgeph( tsync, sat, -1, nav, ephopt);
				if(!gephp) continue;
				if(BS_geph2pos( tsync, gephp, rs, &dts) < 0) continue;
				if(rs.norm() < RE_WGS84) continue;
				if(BS_satantoff( tsync, sat, rs, dant, gephp->frq ) < 0) continue;
				fprintf(stdout, " %s ", sat.id().c_str());
			}
			else{
				Eph* ephp=BS_seleph( tsync, sat, -1, nav, ephopt);
				if(!ephp) continue;
				if(BS_eph2pos( tsync, ephp, rs, &dts) < 0) continue;
				if(rs.norm() < RE_WGS84) continue;
				if(BS_satantoff( tsync, sat, rs, dant, 0 ) < 0) continue;
				fprintf(stdout, " %s ", sat.id().c_str());
			}

			vector<double> sateph;
			sateph.push_back(rs(0) - dant(0));
			sateph.push_back(rs(1) - dant(1));
			sateph.push_back(rs(2) - dant(2));
			sateph.push_back(dts);

			//fprintf(stdout, "\n%s %11.6f %11.6f %11.6f", sat.id().c_str(), dant(0),dant(1),dant(2));

			temp_eph[sat] = sateph;
		}

		if (temp_eph.size() > 0)
			print_SP3epoch(sp3fp, tsync, temp_eph);

		tsync = tsync + BS_Epoch_Inter;
	}

	fprintf(sp3fp, "\nEOF");

	return(EXIT_SUCCESS);
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE arithmetic backend

#include <iostream>
#include <boost/test/included/unit_test.hpp>

#include "../src/arithmetic_backend.hpp"
#include "../src/arithmetic_backend_factory.hpp"

using namespace std;
using namespace homomorphine;

//
// Tests for SEAL backend
//

#ifdef __HAS_SEAL__

// Test SEAL backend
BOOST_AUTO_TEST_CASE( simple_seal_bfv_backend_test )
{
  BOOST_TEST_MESSAGE( "Testing a generic homomorphic encryption backend (SEAL implementation with BFV)..." );

  long result;
  stringstream public_key;
  stringstream secret_key;
  ArithmeticBackend* backend = ArithmeticBackendFactory::create("seal");

  backend->setAlgorithm("BFV");
  backend->init();

  // generate uuencoded keys
  backend->generateKeys();
  backend->writePublicKeyToStream(public_key);
  backend->writeSecretKeyToStream(secret_key);

  // encrypt using a new object
  ArithmeticBackend* backend_operations = ArithmeticBackendFactory::create("seal");

  backend_operations->setAlgorithm("BFV");
  backend_operations->init();

  backend_operations->readPublicKeyFromStream(public_key);
  backend_operations->encrypt(10000);

  // do some basic numeric operations
  backend_operations->add(20);
  backend_operations->multiply(50);
  backend_operations->negate();
  backend_operations->add(50);
  backend_operations->multiply(11);

  // check the results
  backend_operations->readSecretKeyFromStream(secret_key);
  result = backend_operations->decrypt();

  BOOST_TEST ( result == -5510450 );

  // cleanup
  delete(backend);
  delete(backend_operations);
}

BOOST_AUTO_TEST_CASE( simple_seal_ckks_backend_test )
{
  BOOST_TEST_MESSAGE( "Testing a generic homomorphic encryption backend (SEAL implementation with CKKS)..." );

  long result;
  stringstream public_key;
  stringstream secret_key;
  ArithmeticBackend* backend = ArithmeticBackendFactory::create("seal");

  backend->setAlgorithm("CKKS");
  backend->init();

  // generate uuencoded keys
  backend->generateKeys();
  backend->writePublicKeyToStream(public_key);
  backend->writeSecretKeyToStream(secret_key);

  // encrypt using a new object
  ArithmeticBackend* backend_operations = ArithmeticBackendFactory::create("seal");

  backend_operations->setAlgorithm("CKKS");
  backend_operations->init();

  backend_operations->readPublicKeyFromStream(public_key);
  backend_operations->encrypt(10);

  // do some basic numeric operations
  backend_operations->add(20);
  backend_operations->negate();
  backend_operations->add(50);

  // check the results
  backend_operations->readSecretKeyFromStream(secret_key);
  result = backend_operations->decrypt();

  BOOST_TEST ( result == 20 );

  // cleanup
  delete(backend);
  delete(backend_operations);
}
#endif

//
// Tests for HELib backend
//

#ifdef __HAS_HELIB__
// Test HELib backend
BOOST_AUTO_TEST_CASE( simple_helib_backend_test )
{
  BOOST_TEST_MESSAGE( "Testing a generic homomorphic encryption backend (HELib implementation)..." );

  int result;
  stringstream cipher;
  stringstream public_key;
  stringstream secret_key;

  // initialize HELib backend
  ArithmeticBackend* backend = ArithmeticBackendFactory::create("helib");
  backend->init();

  // generate keys
  backend->generateKeys();
  backend->writePublicKeyToStream(public_key);
  backend->writeSecretKeyToStream(secret_key);
  backend->encrypt(10);

  // encrypt using a new object
  ArithmeticBackend* backend_operations = ArithmeticBackendFactory::create("helib");
  backend_operations->init();

  backend_operations->readPublicKeyFromStream(public_key);
  backend_operations->setCipher(backend->getCipher());

  // do some basic numeric operations
  backend_operations->add(10);
  backend_operations->multiply(5);

  backend_operations->writeCipherToStream(cipher);

  // check the results
  backend->readCipherFromStream(cipher);
  result = backend->decrypt();

  // TODO - still not working properly

  // cleanup
  delete(backend);
  delete(backend_operations);
}
#endif


#define BOOST_TEST_MODULE test_adult_transformed_dataset
#include <boost/test/included/unit_test.hpp>

#define TIMERS
//#define TRACE_EVALUATOR

#include "../lib/evaluator.h"
#include "../lib/border_system_creator.h"
#include "../lib/layer_partitioning_creator.h"
#include "../lib/classifier_tmc.h"

BOOST_AUTO_TEST_CASE( adult_transformed_dataset_2_5k ) {
    const std::string names_file("data/adult_transformed/adult_transformed.names");
    const std::string data_file("data/adult_transformed/adult_transformed_2.5k.data");
    const std::string eval_file("data/adult_transformed/adult_transformed.test");
    BOOST_TEST_MESSAGE("Creating train sample from file: " << data_file);
    BOOST_TEST_MESSAGE("Creating evaluation sample from file: " << eval_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);
    std::shared_ptr<Sample> pSampleEval = sample_creator.from_file(eval_file);

    BOOST_TEST_MESSAGE("Resulting sample size: " << pSample->size());
    BOOST_CHECK_EQUAL(pSample->dim(), 13);
    BOOST_CHECK_EQUAL(pSample->size(), 2465);
    BOOST_CHECK_EQUAL(pSampleEval->size(), 13906);
    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample);
    auto pEvaluator = std::make_shared<Evaluator>();
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train, confusion_matrix_eval;
    double roc_err_train, roc_err_eval, err_rate_train, err_rate_eval;
    size_t counter = 0;
    do {
        BOOST_TEST_MESSAGE("############      Iteration " << counter++ << "       ############");
    }
    while (pLayerPartitioningCreator->do_one_step());
        const auto pLP = pLayerPartitioningCreator->get_layer_partitioning();
        const auto pBSC = std::make_shared<BorderSystemCreator>();
        const auto pBS = pBSC->from_layer_partitioning(pLP);
        (*pEvaluator).set_classifier(std::make_shared<ClassifierTmc>(pBS));

        (*pEvaluator).set_sample(pSample);
        confusion_matrix_train = pEvaluator->get_confusion_matrix();
        roc_err_train = pEvaluator->get_roc_error();
        err_rate_train = pEvaluator->get_error_rate();

        (*pEvaluator).set_sample(pSampleEval);
        confusion_matrix_eval = pEvaluator->get_confusion_matrix();
        roc_err_eval = pEvaluator->get_roc_error();
        err_rate_eval = pEvaluator->get_error_rate();

        BOOST_TEST_MESSAGE("");
        BOOST_TEST_MESSAGE("############      Iteration " << counter++ << "       ############");
        BOOST_TEST_MESSAGE("Train conf matr ### Eval conf matr");
        BOOST_TEST_MESSAGE((long int)confusion_matrix_train.first.first << "  " << (long int)confusion_matrix_train.first.second << "    ###    "
                                                                        << (long int)confusion_matrix_eval.first.first << "   " << (long int)confusion_matrix_eval.first.second);
        BOOST_TEST_MESSAGE((long int)confusion_matrix_train.second.first << "  " << (long int)confusion_matrix_train.second.second << "    ###    "
                                                                         << (long int)confusion_matrix_eval.second.first << "   " << (long int)confusion_matrix_eval.second.second);
        BOOST_TEST_MESSAGE("------------------------------------------------");
        BOOST_TEST_MESSAGE("Train rank err ### Eval rank err");
        BOOST_TEST_MESSAGE( roc_err_train << "    ###    " << roc_err_eval);
        BOOST_TEST_MESSAGE("------------------------------------------------");
        BOOST_TEST_MESSAGE("Train err rate ### Eval err rate");
        BOOST_TEST_MESSAGE( err_rate_train << "    ###    " << err_rate_eval);
        BOOST_TEST_MESSAGE("################################################");
//}
//while (pLayerPartitioningCreator->do_one_step());
    BOOST_CHECK_EQUAL(roc_err_train, 0.023456166910557547);
    BOOST_CHECK_EQUAL(roc_err_eval, 0.18955266653760158);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train_({{98637823,15610269},{22187662,338645086}});
    BOOST_CHECK(confusion_matrix_train == confusion_matrix_train_);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_eval_({{421210579.5,274297694.5},{277723922.5,1882390339.5}});
    BOOST_CHECK(confusion_matrix_eval == confusion_matrix_eval_);
}

BOOST_AUTO_TEST_CASE( adult_transformed_dataset ) {
    const std::string names_file("data/adult_transformed/adult_transformed.names");
    const std::string data_file("data/adult_transformed/adult_transformed.data");
    const std::string eval_file("data/adult_transformed/adult_transformed.test");
    BOOST_TEST_MESSAGE("Creating train sample from file: " << data_file);
    BOOST_TEST_MESSAGE("Creating evaluation sample from file: " << eval_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);
    std::shared_ptr<Sample> pSampleEval = sample_creator.from_file(eval_file);

    BOOST_TEST_MESSAGE("Resulting sample size: " << pSample->size());
    BOOST_CHECK_EQUAL(pSample->dim(), 13);
    BOOST_CHECK_EQUAL(pSample->size(), 26314);
    BOOST_CHECK_EQUAL(pSampleEval->size(), 13906);
    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample);
    auto pEvaluator = std::make_shared<Evaluator>();
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train, confusion_matrix_eval;
    double roc_err_train, roc_err_eval, err_rate_train, err_rate_eval;
    size_t counter = 0;
    do {
        BOOST_TEST_MESSAGE("############      Iteration " << counter++ << "       ############");
    }
    while (pLayerPartitioningCreator->do_one_step());
    const auto pLP = pLayerPartitioningCreator->get_layer_partitioning();
    const auto pBSC = std::make_shared<BorderSystemCreator>();
    const auto pBS = pBSC->from_layer_partitioning(pLP);
    (*pEvaluator).set_classifier(std::make_shared<ClassifierTmc>(pBS));

    (*pEvaluator).set_sample(pSample);
    confusion_matrix_train = pEvaluator->get_confusion_matrix();
    roc_err_train = pEvaluator->get_roc_error();
    err_rate_train = pEvaluator->get_error_rate();
    /*
    std::ofstream fs;
    fs.open(data_file+".eval");
    if (!fs.is_open())
        throw std::runtime_error("Cannot open file: " + data_file + ".eval");
    pEvaluator->evaluate_data_file(fs, data_file, pFN);
    fs.close();
     */

    (*pEvaluator).set_sample(pSampleEval);
    confusion_matrix_eval = pEvaluator->get_confusion_matrix();
    roc_err_eval = pEvaluator->get_roc_error();
    err_rate_eval = pEvaluator->get_error_rate();
    /*
    fs.open(eval_file+".eval");
    if (!fs.is_open())
        throw std::runtime_error("Cannot open file: " + eval_file + ".eval");
    pEvaluator->evaluate_data_file(fs, eval_file, pFN);
    fs.close();
     */

    BOOST_TEST_MESSAGE("");
    BOOST_TEST_MESSAGE("############      Iteration " << counter++ << "       ############");
    BOOST_TEST_MESSAGE("Train conf matr ### Eval conf matr");
    BOOST_TEST_MESSAGE((long int)confusion_matrix_train.first.first << "  " << (long int)confusion_matrix_train.first.second << "    ###    "
                                                                    << (long int)confusion_matrix_eval.first.first << "   " << (long int)confusion_matrix_eval.first.second);
    BOOST_TEST_MESSAGE((long int)confusion_matrix_train.second.first << "  " << (long int)confusion_matrix_train.second.second << "    ###    "
                                                                     << (long int)confusion_matrix_eval.second.first << "   " << (long int)confusion_matrix_eval.second.second);
    BOOST_TEST_MESSAGE("------------------------------------------------");
    BOOST_TEST_MESSAGE("Train rank err ### Eval rank err");
    BOOST_TEST_MESSAGE( roc_err_train << "    ###    " << roc_err_eval);
    BOOST_TEST_MESSAGE("------------------------------------------------");
    BOOST_TEST_MESSAGE("Train err rate ### Eval err rate");
    BOOST_TEST_MESSAGE( err_rate_train << "    ###    " << err_rate_eval);
    BOOST_TEST_MESSAGE("################################################");
//}
//while (pLayerPartitioningCreator->do_one_step());
    BOOST_CHECK_EQUAL(roc_err_train, 0.04449670928309904);
    BOOST_CHECK_EQUAL(roc_err_eval, 0.12003629225644707);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train_({{1067540668.5,274333677.5},{345089247.5,4037598025.5}});
    BOOST_CHECK(confusion_matrix_train == confusion_matrix_train_);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_eval_({{464481958.5,232380654.5},{234452543.5,1924307379.5}});
    BOOST_CHECK(confusion_matrix_eval == confusion_matrix_eval_);

    Timers::server().report();
}


#include <array>
#include <vector>
#include <functional>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <iomanip>

#include <exception>
#include <mutex>
#include <thread>
#include <cmath>
#include <csignal>
#include <cerrno>
#include <sys/stat.h>

#include <Eigen/Dense>

#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/rate_limiting.h>
#include <franka/model.h>
#include <franka/robot.h>
#include <franka/gripper.h>

#include <trajectory_msgs/JointTrajectory.h>
#include <ros/ros.h>
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/String.h"
#include "sensor_msgs/JointState.h"

#include "/home/franka/Projects/libfranka/examples/examples_common.h"
using namespace std;

/*GLOBAL VARIABLE*/
const double object_grasp_width = 0.02;
const std::string ROBOT_IP("172.16.0.2");

const std::array<double, 7> KP = {{60.0, 60.0, 60.0, 60.0, 40.0, 20.0, 10.0}};
const std::array<double, 7> KD = {{10.0, 10.0, 10.0, 8.0, 5.0, 2.0, 2.0}};

int main(int argc, char** argv) {
    // ros initialisation
    ros::init(argc, argv, "lowlevel_node");
    ros::NodeHandle nh;
    ros::Rate rate(20);

    ros::Publisher jointsPub = nh.advertise<sensor_msgs::JointState>("/robot1/joint_states", 2);
    ros::Publisher fkPub     = nh.advertise<std_msgs::Float64MultiArray>("/robot1/fk", 2);
    //ros msg initialisation
    sensor_msgs::JointState msg;
    msg.name.resize(9);
    msg.position.resize(9);
    msg.velocity.resize(9);
    for(size_t i=0; i<7; i++){
        msg.name[i]     = "panda_joint" + to_string(i+1);
    }
    for(size_t i=0; i<2; i++){
        msg.name[i+7]     = "panda_finger_joint" + to_string(i+1);
    }
    std_msgs::Float64MultiArray cur_trans;
    cur_trans.data.resize(16);
    //
    std::thread recev_cmd;
    bool start_running = false;
try{
    //franka initialisation
    franka::Robot robot(ROBOT_IP);
    franka::Gripper gripper(ROBOT_IP);

    robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                                {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                                {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                                {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});

    /* set stiffness frame (object relative to EE)
     Set to EE instead of the object so that the end effector force can be directly read*/
    robot.setK({{1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}});

    /*initial pose*/
#if 1
    ROS_INFO("go back to home position");
    std::array<double,7> q_init;
    //q_init = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
    //q_init = {{ 0, -0.785, 0.0, -2.356, 0.0, 1.57, 0.785 }};
    q_init = {{ 0, -0.58, 0, -1.76, -0.23, 2.73, 0.79 }};
    MotionGenerator motion_generator(0.2, q_init);
    gripper.stop(); // to make sure homing executes successfully
    ROS_INFO("go back to home position finished");
#endif
    double width_gripper_open = gripper.readOnce().max_width;
    if (width_gripper_open < 0.079){
        ROS_ERROR("Failed initialising the gripper! Make sure nothing is between the fingers when homing.");
    }
    else
       robot.control(motion_generator);

     /*user confirmation*/
    std::cout << "WARNING: Collision thresholds are set to high values. "
              << "Make sure you have the user stop at hand!" << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();

    /***   main func    ***/
    /*load the kinematics and dynamics model*/
    franka::Model model = robot.loadModel();
    franka::RobotState initial_state = robot.readOnce();

    /*q_cmd used in control callback*/
    std::array<double, 7> q_cmd_now = initial_state.q;

    /*define callback for the torque control loop*/
    std::function<franka::Torques(const franka::RobotState&, franka::Duration)> joint_PD_control_callback = [&](const franka::RobotState& robot_state, franka::Duration dt)->franka::Torques{
            std::array<double, 7> coriolis = model.coriolis(robot_state);

            std::array<double, 7> tau_jpd;
            for (int i = 0; i < 7; i++){
                if(!start_running){
                    tau_jpd[i] = KP[i]*(robot_state.q[i] - robot_state.q[i]) - KD[i]*robot_state.dq[i] + coriolis[i];
                    ROS_INFO("stop mode");
                }
                else
                    tau_jpd[i] = 0.3*KP[i]*(q_cmd_now[i] - robot_state.q[i]) - KD[i]*robot_state.dq[i] + coriolis[i];
            }
         //   cout << "i am running fine" << endl;

        	for(size_t i=0; i<7; i++){
                msg.position[i] = robot_state.q[i];
                msg.velocity[i] = robot_state.dq[i];
            }

            for(size_t i=0; i<2; i++){
                msg.position[i+7] = width_gripper_open / 2.0;
                msg.velocity[i+7] = 0;
         	    //auto gripper_state = gripper.readOnce(); not work
            }

          	msg.header.stamp = ros::Time::now();
            jointsPub.publish(msg);

            cur_trans.data.insert(cur_trans.data.begin(), robot_state.O_T_EE.begin(), robot_state.O_T_EE.end());
            fkPub.publish(cur_trans);
            cur_trans.data.clear();

            // send torque command
            //cout << q_cmd_now[0] << "torque" << endl;
            return tau_jpd;
    };

    boost::function<void(const trajectory_msgs::JointTrajectoryConstPtr&)> callback = [&](const trajectory_msgs::JointTrajectoryConstPtr& msg){
        //cout<<"hello i am in callback"<<endl;
        auto points = msg->points;
        for(size_t i=0; i<7; i++){
            q_cmd_now[i] = points[0].positions[i];
        }
    };
    boost::function<void(const std_msgs::String::ConstPtr&)> startcallback = [&](const std_msgs::String::ConstPtr& msg){
        //cout<<"hello i am in callback"<<endl;
        if(msg->data == "start")
            start_running = true;
        else
            start_running = false;
    };
    /*run it*/
    ros::Subscriber sub = nh.subscribe("robot1/jointTrajectory", 4, callback);
    ros::Subscriber start_sub = nh.subscribe("robot1/start", 4, startcallback);
    recev_cmd = std::thread([&](){
        while(ros::ok()){
            rate.sleep();
            ros::spinOnce();
        }
    });
    //recev_cmd.join();
    robot.control(joint_PD_control_callback);

}
catch(const franka::Exception& ex){
	cout << ex.what() << endl;
    cerr << "error" << endl;
}
if(recev_cmd.joinable())
    recev_cmd.join();
std::cout << "Quitted robot control loop." << std::endl;
std::cout << "Program ended." << std::endl;
return 0;
}

// define thread for receive q_d, qdot_d
//std::thread receive_thread([&Q_DSIR, &nh]() {
//});


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

#include <boost/simd/toolbox/arithmetic/functions/hypot.hpp>
#include <boost/simd/include/functions/scalar/bitwise_cast.hpp>
#include <boost/simd/include/functions/scalar/sqrt.hpp>
#include <boost/simd/include/functions/scalar/sqr.hpp>
#include <boost/simd/include/functions/scalar/max.hpp>
#include <boost/simd/include/functions/scalar/min.hpp>
#include <boost/simd/include/functions/scalar/logical_or.hpp>
#include <boost/simd/include/functions/scalar/logical_and.hpp>
#include <boost/simd/include/functions/scalar/exponent.hpp>
#include <boost/simd/include/functions/scalar/is_nan.hpp>
#include <boost/simd/include/functions/scalar/is_inf.hpp>
#include <boost/simd/include/functions/scalar/ldexp.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/sdk/meta/make_dependent.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  template < class T, class I = typename dispatch::meta::as_integer<T, signed>::type>
  struct hypot_constants;
  //TODO make proper constants

  template <class I> struct hypot_constants<float, I>
  {
    typedef I  int_type;
    static inline int_type C0() { return (30);};
    static inline int_type C1() { return (50);};
    static inline int_type C2() { return (60);};
    static inline int_type MC1(){ return (-50);};
    static inline int_type MC2(){ return (-60);};
    static inline int_type C3() { return (0x00800000);};
    static inline int_type M1() { return (0xfffff000);};
  };

  template <class I> struct hypot_constants<double, I>
  {
    typedef I  int_type;
    static inline int_type C0() { return (60);};
    static inline int_type C1() { return (500);};
    static inline int_type C2() { return (600);};
    static inline int_type MC1(){ return (-500);};
    static inline int_type MC2(){ return (-600);};
    static inline int_type C3() { return (0x0010000000000000ll);}
    static inline int_type M1() { return (0xffffffff00000000ll);};
  };

 BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::hypot_, tag::cpu_,
                       (A0),
                       (scalar_<arithmetic_<A0> >)(scalar_<arithmetic_<A0> >)
                      )
 {
   typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return internal(result_type(a0), result_type(a1));
    }
  private:

    static inline float internal(const float& a0, const  float& a1)
      {
      // flibc does that in ::hypotf(a0, a1) in asm with no more speed!
      // proper internal is 30% slower
        return static_cast<float>(::sqrt(boost::simd::sqr(static_cast<typename meta::make_dependent<double, A0>::type>(a0))+
                                         boost::simd::sqr(static_cast<typename meta::make_dependent<double, A0>::type>(a1))));
      }

    template < class AA0>
    static inline AA0  internal(const AA0& a0, const  AA0& a1)
    {
      // in double ::hypot is very slow and is 4 times slower than internal
      // this routine in float (with float constants) is 30% slower than
      // the straightforward preceding overload for floats
      // The float constants are provided in order to modify
      // the algorithm if a architecture gived different speed results
      if ( logical_or(logical_and(is_nan(a0), is_inf(a1)),
                      logical_and(is_nan(a1), is_inf(a0)))) return Inf<result_type>();
      typedef typename dispatch::meta::as_integer<AA0, signed>::type  int_type;
      AA0 x =  boost::simd::abs(a0);
      AA0 y =  boost::simd::abs(a1);
      if (boost::simd::is_inf(x+y)) return Inf<AA0>();
      if (boost::simd::is_nan(x+y)) return Nan<AA0>();
      AA0 a =  boost::simd::max(x, y);
      AA0 b =  boost::simd::min(x, y);
      int_type ea =   boost::simd::exponent(a);
      int_type eb  =  boost::simd::exponent(b);
      if (ea-eb > hypot_constants<AA0>::C0()) return a+b;
      int_type e = Zero<int_type>();
      if (ea > hypot_constants<AA0>::C1())
      {
        e = hypot_constants<AA0>::MC2();
      }
      if (eb < hypot_constants<AA0>::MC1())
      {
        e = hypot_constants<AA0>::C1();
      }
      if (e)
      {
        a =  boost::simd::ldexp(a, e);
        b =  boost::simd::ldexp(b, e);
      }
      AA0 w = a-b;
      if (w > b)
      {
        AA0 t1 = b_and(a, hypot_constants<AA0>::M1());
        AA0 t2 = a-t1;
        w  = (t1*t1-(b*(-b)-t2*(a+t1)));
      }
      else
      {
        AA0 y1 = b_and(b, hypot_constants<AA0>::M1());
        AA0 y2 = b - y1;
        typedef typename dispatch::meta::as_integer<AA0, unsigned>::type type;
        AA0 t1 =  bitwise_cast<AA0>(bitwise_cast<type>(a)+hypot_constants<AA0>::C3());
        AA0 t2 = (a+a) - t1;
        w  = (t1*y1-(w*(-w)-(t1*y2+t2*b)));
      }
      w = boost::simd::sqrt(w);
      if (e) w = boost::simd::ldexp(w, -e);
      return w;
    }
  };
} } }

#endif


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2016 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef ITERATE_GRID_HH
#define ITERATE_GRID_HH

#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/sequence.hpp>

#include "dune/grid/common/grid.hh"
#include "dune/geometry/quadraturerules.hh"

#include "fem/functionspace.hh"
#include "fem/quadrature.hh"
#include "fem/variables.hh"

#include "utilities/threading.hh"

namespace Kaskade
{
  namespace GridIterateDetail
  {
    template <class VariableDescriptions, class Functor, class Functions, class Spaces, class CellRange>
    void gridIterateRange(VariableDescriptions const& varDesc, Functions const& functions, Spaces const& spaces, Functor& f, CellRange cells)
    {
      using namespace boost::fusion;
      using namespace Dune;

      typedef typename SpaceType<Spaces,0>::type::Grid Grid;
      typedef typename SpaceType<Spaces,0>::type::GridView GridView;
      typedef typename SpaceType<Spaces,0>::type::Scalar Scalar;
      typedef typename Grid::ctype CoordType;
      int const dim = Grid::dimension;

      GridView const& gridView = at_c<0>(spaces)->gridView();

      // Shape function cache. Remember that every thread has to use its own cache!
      typedef ShapeFunctionCache<Grid,Scalar> SfCache;
      SfCache sfCache;

      // Quadrature rule cache
      typedef Dune::QuadratureRule<CoordType,dim> QuadRule;
      QuadratureTraits<QuadRule> quadratureCache;

      // Evaluators for shape functions of all FE spaces. Remember that
      // every thread has to use its own collection of evaluators!
      auto evaluators = getEvaluators(spaces,&sfCache);

      // Iterate over all cells.
      for (auto ci=cells.begin(); ci!=cells.end(); ++ci) {

        // for all spaces involved, compute the shape functions and
        // their global indices, which are needed for evaluating the
        // functional's derivative.
        auto idx = gridView.indexSet().index(*ci);
        moveEvaluatorsToCell(evaluators,*ci,idx);

        // loop over all quadrature points and evaluate variables
        int const p = f.integrationOrder(*ci,maxOrder(evaluators));

        QuadRule const& qr = quadratureCache.rule(ci->type(),p);
        useQuadratureRuleInEvaluators(evaluators,qr,0);

        size_t nQuadPos = qr.size();
        for (size_t g=0; g<nQuadPos; ++g) {
          // pos of integration point
          auto const& quadPos = qr[g].position();

          // for all spaces involved, update the evaluators associated
          // to this quadrature point
          moveEvaluatorsToIntegrationPoint(evaluators,quadPos,qr,g,0);

          // evaluate functions and call functor
          f(ci,idx,quadPos,evaluateFunctions<VariableDescriptions>(functions,evaluators,valueMethod),
            ci->geometry().integrationElement(quadPos)*qr[g].weight());
        }
      }
    };

  } // end of namespace GridIterateDetail

  /**
   * \brief A function that supports general iterations over a spatial domain and
   * supports efficient evaluation of FE functions at the iterated
   * quadrature points.
   *
   * \param varDesc a heterogeneous container of VariableDescription structures
   *
   * \param functions a heterogeneous container of FE functions or function
   * views that can be evaluated given an Evaluator of the associated space.
   *
   * \param spaces a heterogeneous container of pointers to FE function spaces.
   * VariableDescription classes that gives the mapping of functions in
   * data to their associated spaces.
   *
   * \param f the functor that does the work. On each quadrature point,
   * is operator() is called with a heterogeneous container of the
   * values of the functions in data.
   */
  template <class VariableDescriptions, class Functor, class Functions, class Spaces>
  void gridIterate(VariableDescriptions const& varDesc, Functions const& functions, Spaces const& spaces, Functor& f)
  {
    using namespace boost::fusion;
    auto const& cellRanges = deref(begin(functions)).space().gridManager().leafCellRanges(); // WARNING: what if the grid view is not leaf?

    int n = std::min(NumaThreadPool::instance().cpus(),cellRanges.maxRanges());
    std::vector<Functor> fs(n,f);

    parallelFor([&varDesc,&functions,&spaces,&cellRanges,&fs](int i, int n)
    {
      GridIterateDetail::gridIterateRange(varDesc,functions,spaces,fs[i],cellRanges.range(n,i));
    },n);

    for (auto& g: fs)
      f.join(g);
  }

  //---------------------------------------------------------------------

  /**
   * \brief A trivial Scaling.
   *
   * The identity scaling which leaves its operand simply as it is. This
   * is a convenience class to evaluate plain, unscaled norms.
   */
  struct IdentityScaling
  {
    template <class Cell, class F>
    void scale(Cell const&,
               Dune::FieldVector<typename Cell::Geometry::ctype,Cell::dimension> const&,
               F&) const {}
  };

  /**
   * \brief A \ref Collector that sums up the weighted contributions.
   *
   * This realizes a plain integration.
   */
  struct SummationCollector
  {
    template <class Cell>
    int integrationOrder(Cell const& /* ci */, int shapeFunctionOrder) const
    {
      return shapeFunctionOrder;
    }

    template <class CellIterator, class Index, class Sequence>
    void operator()(CellIterator const&, Index, double weight, Sequence const& x)
    {
      if (sum.empty())
        sum.resize(boost::fusion::size(x),0);
      auto i = sum.begin();                           // provide a reference to the iterator
      boost::fusion::for_each(x,Add(weight,i));       // because that is incremented on each call of Add
    }                                                 // (hack: for_each requires an immutable functor)

    void join(SummationCollector const& c)
    {
      if (sum.empty())
        sum = c.sum;
      else
      {
        assert(sum.size()==c.sum.size());
        for (int i=0; i<sum.size(); ++i)
          sum[i] += c.sum[i];
      }
    }

    std::vector<double> sum;

  private:
    struct Add
    {
      Add(double w_, std::vector<double>::iterator& i_): w(w_), i(i_) {}
      template <class T>
      void operator()(T const& t) const { *i += w*t; ++i; }

    private:
      double w;
      std::vector<double>::iterator& i;
    };
  };

  /**
   * \brief A functor for computing scaled \f$L^2\f$-norms of a set of (possibly vector-valued) functions.
   *
   * This is intended to be used with gridIterate. The plain \f$L^2\f$-norm is computed if the
   * IdentityScaling is provided.
   */
  template <class Functions, class Scaling, class Collector>
  class ScaledTwoNorm2Collector
  {
  public:

    /**
     * \brief Constructor.
     *
     * \param scaling
     * \param collector
     */
    ScaledTwoNorm2Collector(Scaling const& scaling_, Collector& collector_): scaling(scaling_), collector(collector_) {    }

    template <class Cell>
    int integrationOrder(Cell const& cell, int shapeFunctionOrder) const
    {
      return 2*collector.integrationOrder(cell,shapeFunctionOrder);
    }

    template <class CellIterator, class Index, class Sequence>
    void operator()(CellIterator const& ci, Index idx,
                    Dune::FieldVector<typename CellIterator::Entity::Geometry::ctype,CellIterator::Entity::dimension> const& pos,
                    Sequence const& seq, double weight)
    {
      using namespace boost::fusion;
      typename result_of::as_vector<Sequence>::type values(seq);
      scaling.scale(*ci,pos,values);
      collector(ci,idx,weight,transform(values,TwoNorm2()));
    }

    void join(ScaledTwoNorm2Collector const& c)
    {
      collector.join(c.collector);
    }

    Collector const& data() const { return collector; }

  private:
    // a boost::fusion functor computing the squared euclidean norm of given vectors
    struct TwoNorm2
    {
      template <class T> struct result {};

      template <class Vec>
      struct result<TwoNorm2(Vec)> { typedef  Dune::FieldVector<double,1> type; };

      template <class Vec>
      typename result<TwoNorm2(Vec)>::type operator()(Vec const& v) const { return v.two_norm2(); }
    };

    Scaling const& scaling;
    Collector collector; // keep collector by value as we write concurrently in different objects.
  };

  /**
   * \brief Evaluates the square of the scaled \f$L^2\f$-norms of a set of functions.
   * \tparam Variables a boost::fusion sequence of VariableDescription types
   * \tparam Functions a boost::fusion sequence of FE function types (or types providing the required interface subset)
   * \tparam Spaces
   * \tparam Scaling
   * \tparam Collector
   *
   * \param varDesc a boost::fusion sequence of VariableDescription entries, e.g. obtained from VariableSetDescription::Variables()
   *
   * The required interface subset of FE functions includes the value() and the space() methods.
   */
  template <class Variables, class Functions, class Spaces, class Scaling, class Collector>
  void scaledTwoNormSquared(Variables const& varDesc, Functions const& f, Spaces const& spaces,
                            Scaling const& scaling, Collector& sum)
  {
    ScaledTwoNorm2Collector<Functions,Scaling,Collector> collector(scaling,sum);
    gridIterate(varDesc,f,spaces,collector);
    sum = collector.data();
  }

  //---------------------------------------------------------------------

  /**
   * \cond internals
   */
  namespace relativeErrorDetail {

    template <class F>
    struct Difference
    {
      typedef typename F::Scalar RT;
      typedef RT Scalar;

      static int const Components = F::components;

      typedef typename F::Space     Space;
      typedef typename F::ValueType ValueType;

      Difference(F const& f1_, F const& f2_): f1(f1_), f2(f2_) { }

      ValueType value(typename Space::Evaluator const& evaluator) const
      {
        return f1.value(evaluator)-f2.value(evaluator);
      }

      Space const& space() const { return f1.space(); }

    private:
      F const& f1;
      F const& f2;
    };

    struct MakeDifference
    {
      template <class T> struct result {};

      template <class Pair>
      struct result<MakeDifference(Pair)>
      {
        typedef typename boost::fusion::result_of::value_at_c<Pair,0>::type T;

        typedef Difference<typename boost::remove_const<typename boost::remove_reference<T>::type>::type> type;
      };

      template <class Pair>
      typename result<MakeDifference(Pair)>::type operator()(Pair const& pair) const
      {
        return typename result<MakeDifference(Pair)>::type(boost::fusion::at_c<0>(pair),boost::fusion::at_c<1>(pair));
      }
    };

  } // End of namespace relativeErrorDetail
  /**
   * \endcond
   */

  /**
   * For each variable, this function computes the following pair of values:
   * \f[ (\|f_1-f_2\|,\|f_3\|) \f]
   * \tparam Variables a boost::fusion sequence of variable descriptions
   * \tparam OutIter an output iterator with value type std::pair(double,double)
   * \tparam Functions a boost::fusion sequence of finite element functions, referenced by the variable descriptions
   * \tparam Spaces a boost::fusion sequence of pointers to spaces, referenced by the variable descriptions
   * \tparam Scaling
   */
  template <class Variables, class OutIter, class Functions, class Spaces, class Scaling>
  void relativeError(Variables const& varDesc, Functions const& f1, Functions const& f2, Functions const& f3,
                     Spaces const& spaces, Scaling const& scaling, OutIter out)
  {
    using namespace boost::fusion;

    int const s = size(f1);
    SummationCollector sum;
    //   scaledTwoNormSquared(join(varDesc,varDesc),
    //                        join( f3, as_vector(transform(zip(f1,f2),relativeErrorDetail::MakeDifference())) ),
    //                        spaces,scaling,sum);
    scaledTwoNormSquared(join(varDesc,varDesc),
                         join(as_vector(transform(zip(f1,f2),relativeErrorDetail::MakeDifference())),f3),
                         spaces,scaling,sum);
    // To my understanding, the as_vector call in the statement above
    // should not be necessary. However, it prevents a (rare but
    // reproducible) segfault with gcc 4.3.4 -O2. Funny thing is, some
    // output statements to cerr have the same effect. The cause may be
    // either a wild pointer bug in the code exposed by -O2 or a code
    // generation bug in gcc.  Sigh. At least, since Difference objects
    // are lightweight, the explicit vector construction should not be a
    // performance penalty. ws 2009-12-03

    for (int i=0; i<s; ++i) {
      *out = std::make_pair(std::sqrt(sum.sum[i]),std::sqrt(sum.sum[i+s]));
      ++out;
    }
  }
} // end of namespace Kaskade

#endif


// Modified work copyright (c) 2019 Gregory Kramida

//libraries
#include <boost/python.hpp>
#include <Eigen/Eigen>
#include <Python.h>

#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION

#include <numpy/arrayobject.h>

//local

#include "numpy_conversions_shared.hpp"
#include "../math/typedefs.hpp"

// These macros were renamed in NumPy 1.7.1.
#if !defined(NPY_ARRAY_C_CONTIGUOUS) && defined(NPY_C_CONTIGUOUS)
#define NPY_ARRAY_C_CONTIGUOUS NPY_C_CONTIGUOUS
#endif

#if !defined(NPY_ARRAY_ALIGNED) && defined(NPY_ALIGNED)
#define NPY_ARRAY_ALIGNED NPY_ALIGNED
#endif

namespace bp = boost::python;

using namespace Eigen;

template<class MatType> // MatrixXf or MatrixXd
struct EigenMatrixToPython {
	static PyObject* convert(const MatType& mat) {
		npy_intp shape[2] = { mat.rows(), mat.cols() };
		PyArrayObject* python_array = (PyArrayObject*) PyArray_SimpleNew(
				2, shape, NumpyEquivalentType<typename MatType::Scalar>::type_code);

		copy_array(mat.data(),
				(typename MatType::Scalar*) PyArray_DATA(python_array),
				mat.rows(),
				mat.cols(),
				false,
				true,
				MatType::Flags & Eigen::RowMajorBit);
		return (PyObject*) python_array;
	}
};

template<>
struct EigenMatrixToPython<math::MatrixXv2f>{
	static PyObject* convert(const math::MatrixXv2f& mat) {
		npy_intp shape[3] = { mat.rows(), mat.cols(), 2 };
		PyArrayObject* python_array = (PyArrayObject*) PyArray_SimpleNew(
				3, shape, NumpyEquivalentType<math::MatrixXv2f::Scalar>::type_code);
		copy_array(mat.data(),
				(math::MatrixXv2f::Scalar*) PyArray_DATA(python_array),
				mat.rows(),
				mat.cols(),
				false,
				true,
				math::MatrixXv2f::Flags & Eigen::RowMajorBit);
		return (PyObject*) python_array;
	}
};

template<typename MatType>
struct EigenMatrixFromPython {
	typedef typename MatType::Scalar T;

	EigenMatrixFromPython() {
		bp::converter::registry::push_back(&convertible,
				&construct,
				bp::type_id<MatType>());
	}

	static void* convertible(PyObject* obj_ptr) {
		PyArrayObject* array = reinterpret_cast<PyArrayObject*>(obj_ptr);
		if (!PyArray_Check(array)) {
			//LOG(ERROR) << "PyArray_Check failed";
			return 0;
		}
		if (PyArray_NDIM(array) > 2) {
			//LOG(ERROR) << "PyArray_Check failed";
			return 0;
		}
		if (PyArray_ObjectType(obj_ptr, 0) != NumpyEquivalentType<T>::type_code) {
			//LOG(ERROR) << "types not compatible";
			return 0;
		}
		int flags = PyArray_FLAGS(array);
		if (!(flags & NPY_ARRAY_C_CONTIGUOUS)) {
			//LOG(ERROR) << "Contiguous C array required";
			return 0;
		}
		if (!(flags & NPY_ARRAY_ALIGNED)) {
			//LOG(ERROR) << "Aligned array required";
			return 0;
		}
		return obj_ptr;
	}

	static void construct(PyObject* obj_ptr,
			bp::converter::rvalue_from_python_stage1_data* data) {
		const int R = MatType::RowsAtCompileTime;
		const int C = MatType::ColsAtCompileTime;

		using bp::extract;

		PyArrayObject* array = reinterpret_cast<PyArrayObject*>(obj_ptr);
		int ndims = PyArray_NDIM(array);
		npy_intp* dimensions = PyArray_DIMS(array);

		int dtype_size = (PyArray_DESCR(array))->elsize;
		int s1 = PyArray_STRIDE(array, 0);
		//CHECK_EQ(0, s1 % dtype_size);
		int s2 = 0;
		if (ndims > 1) {
			s2 = PyArray_STRIDE(array, 1);
			//CHECK_EQ(0, s2 % dtype_size);
		}

		int nrows = R;
		int ncols = C;
		if (ndims == 2) {
			if (R != Eigen::Dynamic) {
				//CHECK_EQ(R, array->dimensions[0]);
			} else {
				nrows = dimensions[0];
			}

			if (C != Eigen::Dynamic) {
				//CHECK_EQ(C, array->dimensions[1]);
			} else {
				ncols = dimensions[1];
			}
		} else {
			//CHECK_EQ(1, ndims);
			// Vector are a somehow special case because for Eigen, everything is
			// a 2D array with a dimension set to 1, but to numpy, vectors are 1D
			// arrays
			// So we could get a 1x4 array for a Vector4

			// For a vector, at least one of R, C must be 1
			//CHECK(R == 1 || C == 1);

			if (R == 1) {
				if (C != Eigen::Dynamic) {
					//CHECK_EQ(C, array->dimensions[0]);
				} else {
					ncols = dimensions[0];
				}
				// We have received a 1xC array and want to transform to VectorCd,
				// so we need to transpose
				// TODO: An alternative is to add wrappers for RowVector, but maybe
				// implicit transposition is more natural
				std::swap(s1, s2);
			} else {
				if (R != Eigen::Dynamic) {
					//CHECK_EQ(R, array->dimensions[0]);
				} else {
					nrows = dimensions[0];
				}
			}
		}

		T* raw_data = reinterpret_cast<T*>(PyArray_DATA(array));

		typedef Map<Matrix<T, Dynamic, Dynamic, RowMajor>, Aligned, Stride<Dynamic, Dynamic> > MapType;

		void* storage = ((bp::converter::rvalue_from_python_storage<MatType>*)
				(data))->storage.bytes;

		new (storage) MatType;
		MatType* emat = (MatType*) storage;
		// TODO: This is a (potentially) expensive copy operation. There should
		// be a better way
		*emat = MapType(raw_data, nrows, ncols,
				Stride<Dynamic, Dynamic>(s1 / dtype_size, s2 / dtype_size));
		data->convertible = storage;
	}
};

//TODO: make a more universal mechanism for nested matrix types
template<>
struct EigenMatrixFromPython<math::MatrixXv2f> {
	typedef math::MatrixXv2f::Scalar T;

	EigenMatrixFromPython() {
		bp::converter::registry::push_back(&convertible, // @suppress("Invalid arguments")
				&construct,
				bp::type_id<math::MatrixXv2f>());
	}

	static void* convertible(PyObject* obj_ptr) {
		PyArrayObject* array = reinterpret_cast<PyArrayObject*>(obj_ptr);
		if (PyArray_NDIM(array) == 3) {
			npy_intp* dimensions = PyArray_DIMS(array);
			if (dimensions[2] != 2) {
				//LOG(ERROR) << "PyArray_Check failed";
				return 0;
			}
		} else {
			//LOG(ERROR) << "PyArray_Check failed";
			return 0;
		}
		if (PyArray_ObjectType(obj_ptr, 0) != NumpyEquivalentType<T>::type_code) { // @suppress("Symbol is not resolved")
			//LOG(ERROR) << "types not compatible";
			return 0;
		}
		int flags = PyArray_FLAGS(array);
		if (!(flags & NPY_ARRAY_C_CONTIGUOUS)) {
			//LOG(ERROR) << "Contiguous C array required";
			return 0;
		}
		if (!(flags & NPY_ARRAY_ALIGNED)) {
			//LOG(ERROR) << "Aligned array required";
			return 0;
		}
		return obj_ptr;
	}

	static void construct(PyObject* obj_ptr,
			bp::converter::rvalue_from_python_stage1_data* data) {
		const int R = math::MatrixXv2f::RowsAtCompileTime;
		const int C = math::MatrixXv2f::ColsAtCompileTime;

		using bp::extract;

		PyArrayObject* array = reinterpret_cast<PyArrayObject*>(obj_ptr);
		int ndims = PyArray_NDIM(array);
		npy_intp* dimensions = PyArray_DIMS(array);

		int dtype_size = (PyArray_DESCR(array))->elsize * 2;
		int s1 = PyArray_STRIDE(array, 0);
		//CHECK_EQ(0, s1 % dtype_size);
		int s2 = 0;
		if (ndims > 1) {
			s2 = PyArray_STRIDE(array, 1);
			//CHECK_EQ(0, s2 % dtype_size);
		}

		int nrows = R;
		int ncols = C;

		if (R != Eigen::Dynamic) {
			//CHECK_EQ(R, array->dimensions[0]);
		} else {
			nrows = dimensions[0];
		}

		if (C != Eigen::Dynamic) {
			//CHECK_EQ(C, array->dimensions[1]);
		} else {
			ncols = dimensions[1];
		}

		T* raw_data = reinterpret_cast<T*>(PyArray_DATA(array));

		typedef Map<Matrix<T, Dynamic, Dynamic, RowMajor>, Aligned, Stride<Dynamic, Dynamic> > MapType;

		void* storage = ((bp::converter::rvalue_from_python_storage<math::MatrixXv2f>*)
				(data))->storage.bytes;

		new (storage) math::MatrixXv2f;
		math::MatrixXv2f* emat = (math::MatrixXv2f*) storage;
		// TODO: This is a (potentially) expensive copy operation. There should
		// be a better way
		*emat = MapType(raw_data, nrows, ncols,
				Stride<Dynamic, Dynamic>(s1 / dtype_size, s2 / dtype_size));
		data->convertible = storage;
	}
};

template<class TransformType> // MatrixXf or MatrixXd
struct EigenTransformToPython {
	static PyObject* convert(const TransformType& transform) {
		return EigenMatrixToPython<typename TransformType::MatrixType>::convert(transform.matrix());
	}
};

template<typename TransformType>
struct EigenTransformFromPython {
	EigenTransformFromPython() {
		bp::converter::registry::push_back(&convertible,
				&construct,
				bp::type_id<TransformType>());
	}

	static void* convertible(PyObject* obj_ptr) {
		return EigenMatrixFromPython<typename TransformType::MatrixType>::convertible(obj_ptr);
	}

	static void construct(PyObject* obj_ptr,
			bp::converter::rvalue_from_python_stage1_data* data) {
		EigenMatrixFromPython<typename TransformType::MatrixType>::construct(obj_ptr, data);
	}
};

// === Lower-level macros ===
#define EIGEN_MATRIX_CONVERTER(Type) \
  EigenMatrixFromPython<Type>();  \
  bp::to_python_converter<Type, EigenMatrixToPython<Type> >();

#define EIGEN_TRANSFORM_CONVERTER(Type) \
  EigenTransformFromPython<Type>();  \
  bp::to_python_converter<Type, EigenTransformToPython<Type> >();

// === Higher-level macros ===

#define MAT_CONV(R, C, T) \
  typedef Matrix<T, R, C> Matrix ## R ## C ## T; \
  EIGEN_MATRIX_CONVERTER(Matrix ## R ## C ## T);

// This requires a MAT_CONV for that Matrix type to be registered first
#define MAP_CONV(R, C, T) \
  typedef Map<Matrix ## R ## C ## T> Map ## R ## C ## T; \
  EIGEN_MATRIX_CONVERTER(Map ## R ## C ## T);

#define T_CONV(R, C, T) \
  typedef Transpose<Matrix ## R ## C ## T> Transpose ## R ## C ## T; \
  EIGEN_MATRIX_CONVERTER(Transpose ## R ## C ## T);

#define BLOCK_CONV(R, C, BR, BC, T) \
  typedef Block<Matrix ## R ## C ## T, BR, BC> Block ## R ## C ## BR ## BC ## T; \
  EIGEN_MATRIX_CONVERTER(Block ## R ## C ## BR ## BC ## T);

static const int X = Eigen::Dynamic;

#if PY_VERSION_HEX >= 0x03000000
void*
#else
void
#endif
setup_Eigen_matrix_converters() {
	static bool is_setup = false;
	if (is_setup)
		return NUMPY_IMPORT_ARRAY_RETVAL; // @suppress("No return value")
	is_setup = true;

	import_array(); // @suppress("No return value")

	EIGEN_MATRIX_CONVERTER(Matrix2f);
	EIGEN_MATRIX_CONVERTER(Matrix2d);
	EIGEN_MATRIX_CONVERTER(Matrix3f);
	EIGEN_MATRIX_CONVERTER(Matrix3d);
	EIGEN_MATRIX_CONVERTER(Matrix4f);
	EIGEN_MATRIX_CONVERTER(Matrix4d);

	EIGEN_MATRIX_CONVERTER(Vector2f);
	EIGEN_MATRIX_CONVERTER(Vector3f);
	EIGEN_MATRIX_CONVERTER(Vector4f);
	EIGEN_MATRIX_CONVERTER(Vector2d);
	EIGEN_MATRIX_CONVERTER(Vector3d);
	EIGEN_MATRIX_CONVERTER(Vector4d);

	EIGEN_MATRIX_CONVERTER(Vector2i);
	EIGEN_MATRIX_CONVERTER(Vector3i);
	EIGEN_MATRIX_CONVERTER(Vector4i);

	EIGEN_MATRIX_CONVERTER(RowVector2f);
	EIGEN_MATRIX_CONVERTER(RowVector3f);
	EIGEN_MATRIX_CONVERTER(RowVector4f);
	EIGEN_MATRIX_CONVERTER(RowVector2d);
	EIGEN_MATRIX_CONVERTER(RowVector3d);
	EIGEN_MATRIX_CONVERTER(RowVector4d);

	EIGEN_TRANSFORM_CONVERTER(Affine2f);
	EIGEN_TRANSFORM_CONVERTER(Affine3f);
	EIGEN_TRANSFORM_CONVERTER(Affine2d);
	EIGEN_TRANSFORM_CONVERTER(Affine3d);

	EIGEN_TRANSFORM_CONVERTER(Isometry2f);
	EIGEN_TRANSFORM_CONVERTER(Isometry3f);
	EIGEN_TRANSFORM_CONVERTER(Isometry2d);
	EIGEN_TRANSFORM_CONVERTER(Isometry3d);

	EIGEN_TRANSFORM_CONVERTER(Projective2f);
	EIGEN_TRANSFORM_CONVERTER(Projective3f);
	EIGEN_TRANSFORM_CONVERTER(Projective2d);
	EIGEN_TRANSFORM_CONVERTER(Projective3d);

	MAT_CONV(2, 3, double);
	MAT_CONV(X, 3, double);
	MAT_CONV(X, X, double);
	MAT_CONV(X, 1, double);
	MAT_CONV(1, X, double);
	MAT_CONV(3, 4, double);
	MAT_CONV(2, X, double);

	MAT_CONV(2, 3, float);
	MAT_CONV(X, 3, float);
	MAT_CONV(X, X, float);
	MAT_CONV(X, 1, float);
	MAT_CONV(1, X, float);
	MAT_CONV(3, 4, float);
	MAT_CONV(2, X, float);
	MAT_CONV(6, 1, float);

	EIGEN_MATRIX_CONVERTER(math::MatrixXus);
	EIGEN_MATRIX_CONVERTER(math::MatrixX1us);
	EIGEN_MATRIX_CONVERTER(math::Matrix1Xus);

	EIGEN_MATRIX_CONVERTER(math::MatrixXuc);
	EIGEN_MATRIX_CONVERTER(math::MatrixX1uc);
	EIGEN_MATRIX_CONVERTER(math::Matrix1Xuc);

	//TODO: generalize these to math::Matrix<subtype> in a separate converter
	EigenMatrixFromPython<math::MatrixXv2f>();
	bp::to_python_converter<math::MatrixXv2f, EigenMatrixToPython<math::MatrixXv2f> >();

#if PY_VERSION_HEX >= 0x03000000
	return 0;
#endif
}


#include <SDL.h>

#include <fanFilm.h>
#include <fanVector.h>
#include <fanPixel.h>
#include <fanCamera.h>
#include <fanTriangleMesh.h>
#include <fanBufferObject.h>
#include <texture/film/SDLFilm.h>
#include <fanLensMovement.h>
#include <lens/OrthogonalLens.h>
#include <lens/PerspectiveLens.h>
#include <camera/PointScannerCamera.h>
#include <camera/WireframeCamera.h>
#include <camera/DepthCamera.h>
#include <objects/TriangleMeshObject.h>

#include <boost/shared_ptr.hpp>

#include <cstdlib>

#include <iostream>

#include "QuadTree.h"

using namespace fan;
using namespace std;

int main() {
    if ( SDL_Init( SDL_INIT_VIDEO ) != 0 ) {
        return 1;
    }

    fanScene scene;

    QuadTree tree( fanVector3<float>( 0, 0, 0 ),
                   fanVector3<float>( 1024, 0, 512 ),
                   fanVector3<float>( 0, 1024, 512 ),
                   fanVector3<float>( 1024, 1024, 1024 ) );

    fanVector<int, 2> size;
    size[0] = 800; size[1] = 600;
    SDLFilm sdl( size );

    fanVector3<float> pos(0, 0, 1000);
    fanVector3<float> lookAt(500, 500, 0);
    fanVector3<float> lookAxis = lookAt - pos;

    OrthogonalLens OrthoLens( pos,
                         lookAt,
                         fanVector3<float>(1, 1, 1),
                         fanVector3<float>(1600, 1200, 5000) );
    PerspectiveLens PerspLens( fanVector3<float>(0, 0, 1000),
                         fanVector3<float>(500, 500, 0),
                         fanVector3<float>(0, 0, 1),
                         fanVector3<float>(1600, 1200, 5000),
                         3000 );

    fanCamera* currentCamera = NULL;
    fanLens* currentLens = NULL;
    PointScannerCamera pointCamera;
    WireframeCamera wireframeCamera;
    DepthCamera depthCamera;

    bool done = false;

    boost::shared_ptr<TriangleMeshObject> terrain(
            new TriangleMeshObject( fanMatrix<float, 4, 4>{1, 0, 0, 0,
                                                           0, 1, 0, 0,
                                                           0, 0, 1, 0,
                                                           0, 0, 0, 1, } ) );

    currentCamera = &depthCamera;
    currentLens = &PerspLens;
    Uint32 time;
    scene.mTriangleMeshObjects.push_back( terrain );
    size_t level = 0;
    while ( !done ) {
        time = SDL_GetTicks();
        terrain->mMeshes.clear();
        tree.generateTerrian( *terrain, level );
        currentCamera->takePicture( scene, sdl, *currentLens );
        std::cout << SDL_GetTicks() - time << " ms in takePicture" << std::endl;
        sdl.develope();

        SDL_Event event;
        if (SDL_WaitEvent(&event)) {
            if ( event.type == SDL_QUIT ) {
                done = true;
            }
            switch( event.type ) {
                case SDL_QUIT:
                    done = true;
                break;
                case SDL_KEYDOWN:
                {
                    fanVector<float, 3> move;
                    switch( event.key.keysym.sym ) {
                        case SDLK_w: Pitch(M_PI/360.0f, OrthoLens); Pitch(M_PI/360.0f, PerspLens); break;
                        case SDLK_s: Pitch(-M_PI/360.0f, OrthoLens); Pitch(-M_PI/360.0f, PerspLens); break;
                        case SDLK_a: Yaw(M_PI/360.0f, OrthoLens); Yaw(M_PI/360.0f, PerspLens); break;
                        case SDLK_d: Yaw(-M_PI/360.0f, OrthoLens); Yaw(-M_PI/360.0f, PerspLens); break;
                        case SDLK_q: Roll(M_PI/360.0f, OrthoLens); Roll(M_PI/360.0f, PerspLens); break;
                        case SDLK_e: Roll(-M_PI/360.0f, OrthoLens); Roll(-M_PI/360.0f, PerspLens); break;
                        case SDLK_u: PitchAroundFocus(M_PI/360.0f, OrthoLens); PitchAroundFocus(M_PI/360.0f, PerspLens); break;
                        case SDLK_j: PitchAroundFocus(-M_PI/360.0f, OrthoLens); PitchAroundFocus(-M_PI/360.0f, PerspLens); break;
                        case SDLK_h: YawAroundFocus(M_PI/360.0f, OrthoLens); YawAroundFocus(M_PI/360.0f, PerspLens); break;
                        case SDLK_k: YawAroundFocus(-M_PI/360.0f, OrthoLens); YawAroundFocus(-M_PI/360.0f, PerspLens); break;
                        case SDLK_y: RollAroundFocus(M_PI/360.0f, OrthoLens); RollAroundFocus(M_PI/360.0f, PerspLens); break;
                        case SDLK_i: RollAroundFocus(-M_PI/360.0f, OrthoLens); RollAroundFocus(-M_PI/360.0f, PerspLens); break;
                        case SDLK_ESCAPE: done = true; break;
                        case SDLK_1: currentCamera = &pointCamera; break;
                        case SDLK_2: currentCamera = &wireframeCamera; break;
                        case SDLK_3: currentCamera = &depthCamera; break;
                        case SDLK_o: currentLens = &OrthoLens; break;
                        // case SDLK_p: currentLens = &PerspLens; break;
                        case SDLK_n: ++level; break;
                        case SDLK_b: if ( level != 0 ) --level; break;
                    }
                }
                break;
            }
        }
    }

    atexit(SDL_Quit);
}


/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

/**
 * \file
 *
 * Implementation of geometry.h
 *
 * \author Bhaskara Marthi
 */

#include <occupancy_grid_utils/geometry.h>
#include <queue>
#include <set>
#include <boost/foreach.hpp>

namespace occupancy_grid_utils
{

namespace gm=geometry_msgs;
namespace nm=nav_msgs;
using std::vector;
typedef std::set<Cell> Cells;

struct Line
{
  Line (const gm::Point32& p1, const gm::Point32& p2)
  {
    const float dx = p2.x-p1.x;
    const float dy = p2.y-p1.y;
    if (fabs(dx) < 1e-3 && fabs(dy) < 1e-3)
    {
      boost::format e("Points (%.2f, %.2f) and (%.2f, %.2f) are too close");
      throw GridUtilsException(e % p1.x % p1.y % p2.x % p2.y);
    }
    a = dy;
    b = -dx;
    c = p1.y*dx - p1.x*dy;
  }

  // Line intersects a convex polygon if two of the vertices have opposite signs
  bool intersects (const gm::Polygon& poly) const
  {
    bool seen_nonnegative = false;
    bool seen_nonpositive = false;
    BOOST_FOREACH (const gm::Point32& p, poly.points)
    {
      const float d = a*p.x+b*p.y+c;
      if (d>=0)
        seen_nonnegative = true;
      if (d<=0)
        seen_nonpositive = true;
      if (seen_nonnegative && seen_nonpositive)
        return true;
    }
    return false;
  }

  // Coefficients of line equation ax+by+c=0
  float a;
  float b;
  float c;
};

// Visitor for the flood fill
struct CellsInPolygon
{
  CellsInPolygon (const nm::MapMetaData& info, const gm::Polygon& poly) :
    info(info)
  {
    const size_t n = poly.points.size();
    for (size_t i=0; i<n; i++)
    {
      const size_t j = i>0 ? i-1 : n-1;
      sides.push_back(Line(poly.points[i], poly.points[j]));
    }
  }

  // Add cell to the set of visited cells.  If cell doesn't intersect the
  // boundary of the polygon, return true (continue propagating), else false.
  bool operator() (const Cell& c)
  {
    cells.insert(c);
    const gm::Polygon cell_poly = cellPolygon(info, c);
    BOOST_FOREACH (const Line& s, sides)
    {
      if (s.intersects(cell_poly))
        return false;
    }
    return true;
  }

  Cells cells;
  const nm::MapMetaData& info;
  std::vector<Line> sides;
};

// Generic flood fill
template <class Visitor>
void flood_fill (const nm::MapMetaData& info, const std::set<Cell>& start,
                 Visitor& vis)
{
  Cells seen;
  std::queue<Cell> q;
  BOOST_FOREACH (const Cell& c, start)
    q.push(c);
  while (!q.empty())
  {
    const Cell c = q.front();
    q.pop();
    ROS_DEBUG_STREAM_NAMED ("flood_fill", "Cell " << c);
    if (seen.find(c)==seen.end())
    {
      seen.insert(c);
      ROS_DEBUG_NAMED ("flood_fill", "  Visiting");
      if (vis(c))
      {
        ROS_DEBUG_NAMED ("flood_fill", "  Adding neighbors");
        for (int dy=-1; dy<=1; dy++)
        {
          for (int dx=-1; dx<=1; dx++)
          {
            if (dx!=0 || dy!=0)
            {
              const Cell c2(c.x+dx, c.y+dy);
              if (withinBounds(info, c2))
              {
                q.push(c2);
                ROS_DEBUG_STREAM_NAMED ("flood_fill", "    Added " << c2);
              }
            }
          }
        }
      }
    }
  }
}

// Generic flood fill
template <class Visitor>
void flood_fill (const nm::MapMetaData& info, const Cell& start,
                 Visitor& vis)
{
  std::set<Cell> s;
  s.insert(start);
  flood_fill(info, s, vis);
}

Cell center(const nm::MapMetaData& info,
            const gm::Polygon& poly)
{
  float sx=0;
  float sy=0;
  BOOST_FOREACH (const gm::Point32& p, poly.points)
  {
    sx += p.x;
    sy += p.y;
  }
  gm::Point p;
  p.x = sx/poly.points.size();
  p.y = sy/poly.points.size();
  return pointCell(info, p);
}

// We do this by starting at the center and flood-filling outwards till we
// reach the boundary
Cells cellsInConvexPolygon (const nm::MapMetaData& info,
                            const gm::Polygon& poly)
{
  CellsInPolygon visitor(info, poly);
  flood_fill(info, center(info, poly), visitor);
  return visitor.cells;
}

struct DistanceQueueItem
{
  DistanceQueueItem (const Cell& c, const float distance) :
    c(c), distance(distance)
  {}

  inline
  bool operator< (const DistanceQueueItem& other) const
  {
    // Higher priority means lower distance
    return (distance > other.distance);
  }

  Cell c;
  float distance;
};

DistanceField distanceField (const nav_msgs::OccupancyGrid& m,
                             const float max_dist)
{
  // Initialize
  vector<unsigned> dims;
  dims.push_back(m.info.width);
  dims.push_back(m.info.height);
  DistanceField::ArrayPtr distances(new DistanceField::Array(dims));
  for (unsigned x=0; x<m.info.width; x++)
    for (unsigned y=0; y<m.info.height; y++)
      (*distances)[x][y] = max_dist;
  std::set<Cell> seen; // Set of cells that have already been added to distance field
  std::priority_queue<DistanceQueueItem> q;

  // Sweep over the grid and add all obstacles to the priority queue
  for (index_t i=0; i<m.info.width*m.info.height; i++)
  {
    if (m.data[i]!=UNOCCUPIED && m.data[i]!=UNKNOWN)
    {
      DistanceQueueItem item(indexCell(m.info, i), 0);
      q.push(item);
    }
  }

  // Iteration guaranteed to be in nondecreasing order of distance
  while (!q.empty())
  {
    DistanceQueueItem item = q.top();
    ROS_DEBUG_THROTTLE_NAMED (1.0, "distance_field",
                              "Distance %.4f, %zu seen",
                              item.distance, seen.size());
    q.pop();

    // If not already seen, add this cell to distance field, and
    // add its neighbors to the queue
    if (seen.find(item.c)==seen.end())
    {
      seen.insert(item.c);
      const Cell& c = item.c;
      (*distances)[c.x][c.y] = item.distance;
      if (max_dist>0 && item.distance>max_dist)
        continue;
      for (char vertical=0; vertical<2; vertical++)
      {
        for (char d=-1; d<=1; d+=2)
        {
          const char dx = vertical ? 0 : d;
          const char dy = vertical ? d : 0;
          const Cell neighbor(c.x+dx, c.y+dy);
          if (withinBounds(m.info, neighbor))
          {
            const float dist = item.distance+m.info.resolution;
            q.push(DistanceQueueItem(neighbor, dist));
          }
        }
      }
    }
  }

  return DistanceField(distances);
}

} // namespace


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_MAXNUMMAG_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_MAXNUMMAG_HPP_INCLUDED

#include <boost/simd/ieee/functions/maxnummag.hpp>
#include <boost/simd/include/functions/simd/maxmag.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( maxnummag_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<integer_<A0>,X>))
                                      ((simd_<integer_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return maxmag(a0, a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( maxnummag_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<unspecified_<A0>,X>))
                                      ((simd_<unspecified_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::as_logical<A0>::type lA0;
      const lA0 cond = is_nan(a0);
      const A0 a = if_else(cond,a0,a1);
      const A0 b = if_else(cond,a1,a0);
      return maxmag(a, b);
    }
  };

} } }

#endif


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

#include <boost/simd/swar/functions/enumerate.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/sdk/meta/as_arithmetic.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_, tag::cpu_
                                    , (A0)(X)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      ((target_< simd_< arithmetic_<T>,X> >))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, T const& ) const
    {
      typedef typename meta::scalar_of<result_type>::type s_t;
      result_type that;
      for(std::size_t i=0;i<result_type::static_size;++i)
        that[i] = a0 + i;
      return that;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_, tag::cpu_
                                    , (A0)(A1)(X)(Y)
                                    , ((simd_< arithmetic_<A0>, X >))
                                      ((target_< simd_< arithmetic_<A1>, Y> >))
                                    )
  {
    typedef typename A1::type result_type;
    result_type operator()(A0 const& a0, A1 const& ) const
    {
      return splat<result_type>(a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_, tag::cpu_
                                    , (A0)(X)(T)
                                    , ((generic_< arithmetic_<A0> >))
                                      ((target_< simd_< logical_<T>,X> >))
                                    )
  {
    typedef typename meta::as_arithmetic<typename T::type>::type result_type;

    result_type operator()(A0 const& a0, T const& ) const
    {
      return enumerate<result_type>(a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_, tag::cpu_
                                      , (A0)(A1)(X)(T)
                                    , (scalar_< arithmetic_<A0> >)
                                      (scalar_< arithmetic_<A1> >)
                                      ((target_< simd_< arithmetic_<T>,X> >))
                                    )
  {
    typedef typename T::type result_type;
      typedef typename meta::scalar_of<result_type>::type s_t;
    result_type operator()(A0 const& a0, A1 const& a1, T const& ) const
    {
      result_type that;
      for(std::size_t i=0;i<result_type::static_size;++i)
        that[i] = a0 + a1*i;
      return that;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_, tag::cpu_
                                    , (A0)(A1)(X)(T)
                                    , ((simd_< arithmetic_<A0>, X >))
                                      (generic_< arithmetic_<A1> >)
                                      ((target_< simd_< arithmetic_<T>,X> >))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(A0 const& a0, A1 const& a1, T const& ) const
    {
      return plus(a0, a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::enumerate_, tag::cpu_
                                    , (A0)(A1)(X)(T)
                                    , ((simd_< arithmetic_<A0>, X >))
                                      (generic_< arithmetic_<A1> >)
                                      ((target_< simd_< logical_<T>,X> >))
                                    )
  {
    typedef typename meta::as_arithmetic<typename T::type>::type result_type;

    result_type operator()(A0 const& a0, A1 const& a1, T const& ) const
    {
      return enumerate<result_type>(a0, a1);
    }
  };
} } }

#endif