OTHER2017/pf/EvalWalk.h

#ifndef EVALWALK_H
#define EVALWALK_H

#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterEvaluation.h>
#include <KLib/math/filter/particles/ParticleFilterInitializer.h>
#include <KLib/math/filter/particles/ParticleFilterTransition.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingNEff.h>

#include "../plots/PlotErrFunc.h"
#include "../WalkResult.h"

#include <thread>

#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
#include "Indoor/sensors/radio/setup/WiFiFingerprint.h"
#include "Indoor/sensors/radio/setup/WiFiFingerprints.h"

#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
#include "Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h"

#include "Indoor/sensors/radio/VAPGrouper.h"
#include "Indoor/sensors/imu/StepDetection.h"
#include "Indoor/sensors/imu/TurnDetection.h"
#include "Indoor/sensors/activity/ActivityDetector.h"

#include "Indoor/floorplan/v2/Floorplan.h"
#include "Indoor/floorplan/v2/FloorplanReader.h"
#include "Indoor/floorplan/v2/FloorplanHelper.h"
#include "Indoor/floorplan/v2/FloorplanCeilings.h"

#include "Indoor/sensors/radio/model/WiFiModels.h"

#include "Indoor/sensors/offline/FileReader.h"
#include "../Helper.h"
#include "PF.h"
#include "../plots/PlotErrTime.h"

#include <Indoor/debug/PlotWifiMeasurements.h>

#include <Indoor/sensors/offline/FilePlayer.h>
#include <Indoor/sensors/offline/FileReader.h>
#include <Indoor/sensors/offline/Listener.h>


#define PLOT_LIVE
//#define PLOT_WIFI
//#define PLOT_ERROR_TIME
//#define PLOT_ERROR_FUNC

#define PLOT_UPDATE_STEP	20


class EvalWalk : public Offline::Listener {

	Grid<MyGridNode>* grid;
	K::ParticleFilter<MyState, MyControl, MyObservation>* pf;

	std::string runName;

#ifdef PLOT_LIVE
	Plotty plotty;
#endif

#ifdef PLOT_WIFI
	PlotWifiMeasurements plotWifi;
#endif

#ifdef PLOT_ERROR_TIME
	PlotErrTime pet;
#endif

#ifdef PLOT_ERROR_FUNC
	PlotErrFunc pef;
#endif

	Offline::FileReader reader;
	Offline::FilePlayer player;
	Offline::FileReader::GroundTruth groundTruthLive;

	Timestamp lastTransition;

	MyObservation curObs;
	MyControl curCtrl;
	MyState curEst;

	StepDetection stepDetect;
	TurnDetection turnDetect;
	ActivityDetector actDetect;

	std::vector<Point3> groundTruth;

	Floorplan::IndoorMap* map;

	EarthMapping em;

	std::string walkName;

	float absHead = 0;

public:

	WalkResult res;

	~EvalWalk() {
		delete grid;
		delete pf;
	}

	EvalWalk(Floorplan::IndoorMap* map) :

	#ifdef PLOT_LIVE
		plotty(map),
	#endif

		map(map), em(map)

	#ifdef PLOT_ERROR_TIME
		,pet("\\small{time (sec)}", "\\small{error (m)}", "\\small{APs visible}")
	#endif
	#ifdef PLOT_ERROR_FUNC
		,pef("\\small{error (m)}", "\\small{updates (\\%)}")
	#endif
	{

		const std::string saveFile = Settings::pathData + "/grid.dat";
		grid = new Grid<MyGridNode>(Settings::Grid::gridSize_cm);

		#ifdef PLOT_LIVE
			// once
			plotty.buildFloorplan();
		#endif

		// deserialize grid
		std::ifstream inp(saveFile, std::ofstream::binary);
		if (inp) {
			grid->read(inp);
			inp.close();
		} else {
			// build the grid
			GridFactory<MyGridNode> gf(*grid);
			gf.build(map);
			Importance::addImportance(*grid);
			std::ofstream out(saveFile, std::ofstream::binary);
			grid->write(out);
			out.close();
		}

		pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)) );


		// transition
		pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );

		// resampling step?
		pf->setNEffThreshold(0.02);
		pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );

		//pf->setNEffThreshold(0.75);
		//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.10)) );

		//pf->setNEffThreshold(0.75);
		//pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.05)) );

//		K::ParticleFilterResamplingNEff<MyState>* res = new K::ParticleFilterResamplingNEff<MyState>(0.75, 0.05);
//		pf->setNEffThreshold(1.0);
//		pf->setResampling( std::unique_ptr<K::ParticleFilterResamplingNEff<MyState>>(res) );

		// move during resampling. NOT ALLOWED!
//		res->setDrawCallback([&] (K::Particle<MyState>& p) {
//			static std::minstd_rand gen;
//			static int cnt = 0; ++cnt;
//			bool init = cnt < pf->getParticles().size() * 50;

//			const MyGridNode* n = grid->getNodePtrFor(p.state.position);
//			std::normal_distribution<float> distTurn(0, (init) ? (0.5) : (0.05));
//			for (int j = 0; j < 2; ++j) {
//				std::uniform_int_distribution<int> distIdx(0, n->getNumNeighbors()-1);
//				const int idx = distIdx(gen);
//				n = &(grid->getNeighbor(*n, idx));
//			}
//			p.state.position = *n;
//			p.state.heading.direction += distTurn(gen);
//		});

		// state estimation step
		//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
		//pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
		pf->setEstimation( std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.25f)));

	}

	struct LeWalk {
		std::string name;
		float absHead;
		std::string walkFile;
		std::vector<int> gtIndices;
		LeWalk(const std::string& name, float absHead, std::string walkFile, std::vector<int> gtIndices) :
			name(name), absHead(absHead), walkFile(walkFile), gtIndices(gtIndices) {
			;
		}
	};

	struct LeModel {
		std::string name;
		std::string plotKey;
		WiFiModel* model;
		LeModel(const std::string& name, const std::string& plotKey, WiFiModel* model) : name(name), plotKey(plotKey), model(model) {;}
	};

	static void serialize(const K::Statistics<float>& stats, const LeWalk& walk, const LeModel& model, const int idx) {
		const std::string file = Settings::fPathGFX + "/walks/data/" + model.name + "/" + walk.name + "_" + model.name + "_" + std::to_string(idx) + ".txt";
		std::ofstream out(file);
		for (const float f : stats.getAll()) {
			out << f << "\n";
		}
		out.close();
	}

	static K::Statistics<float>* deserialize(const LeWalk& walk, const LeModel& model, const int idx) {
		const std::string file = Settings::fPathGFX + "/walks/data/" + model.name + "/" + walk.name + "_" + model.name + "_" + std::to_string(idx) + ".txt";
		K::Statistics<float>* res = new K::Statistics<float>();
		std::ifstream inp(file);
		while(inp) {
			float f;
			inp >> f;
			res->add(f);
		}
		return res;
	}

	static std::vector<LeWalk> allWalks() {
		return {
			LeWalk("path1a", M_PI/2, Settings::path1a, Settings::GroundTruth::path1),
			LeWalk("path1b", M_PI/2, Settings::path1b, Settings::GroundTruth::path1),
			LeWalk("toni-all-1a", M_PI/2, Settings::path_toni_all_1a, Settings::GroundTruth::path1),
			LeWalk("toni-all-1b", M_PI/2, Settings::path_toni_all_1b, Settings::GroundTruth::path1),
			LeWalk("path2a", 0, Settings::path2a, Settings::GroundTruth::path2),
			LeWalk("path2b", 0, Settings::path2b, Settings::GroundTruth::path2),
			LeWalk("toni-all-2a", M_PI/2, Settings::path_toni_all_2a, Settings::GroundTruth::path2),
			LeWalk("toni-all-2b", M_PI/2, Settings::path_toni_all_2b, Settings::GroundTruth::path2),
			/////////LeWalk("toni-inst-1a", M_PI/2, Settings::path_toni_inst_1a, Settings::GroundTruth::path_toni_inst_1),
			LeWalk("toni-inst-1b", M_PI/2, Settings::path_toni_inst_1b, Settings::GroundTruth::path_toni_inst_1),
			LeWalk("toni-inst-2a", M_PI/2, Settings::path_toni_inst_2a, Settings::GroundTruth::path_toni_inst_2),
			LeWalk("toni-inst-2b", M_PI/2, Settings::path_toni_inst_2b, Settings::GroundTruth::path_toni_inst_2),
			LeWalk("toni-inst-3a", M_PI/2, Settings::path_toni_inst_3a, Settings::GroundTruth::path_toni_inst_3),
			LeWalk("toni-inst-3b", M_PI/2, Settings::path_toni_inst_3b, Settings::GroundTruth::path_toni_inst_3),

		};
	}

	static std::vector<LeModel> allModels(Floorplan::IndoorMap* map) {
		WiFiModelFactory fac(map);
		return {
			LeModel("empiric", "\\noOptEmpiric{}", fac.loadXML(Settings::wifiAllFixed)),
			LeModel("opt 1", "\\optParamsAllAP{}", fac.loadXML(Settings::wifiAllOptPar)),
			LeModel("opt 2", "\\optParamsEachAP{}", fac.loadXML(Settings::wifiEachOptPar)),
			LeModel("opt 3", "\\optParamsPosEachAP{}", fac.loadXML(Settings::wifiEachOptParPos)),
			LeModel("per floor", "\\optPerFloor{}", fac.loadXML(Settings::wifiEachOptParPos_multimodel)),
			LeModel("per bbox", "\\optPerRegion{}", fac.loadXML(Settings::wifiEachOptParPos_perBBox)),
		};
	}

	static constexpr int numRepeats = 25;

	/** create GFX for all previously walked parts */
	static void walkEverythingBuildStats(Floorplan::IndoorMap* map) {

		const std::vector<LeWalk> walks = allWalks();
		const std::vector<LeModel> models = allModels(map);

		PlotErrFunc* pefAll = new PlotErrFunc();
		pefAll->setYRange(0, 90, 5);
		pefAll->getPlot().getAxisY().setRange(0, 90);
		pefAll->getPlot().getAxisY().setTicsStep({0, 25, 50, 75, 90});
		pefAll->getPlot().getAxisX().setRange(0, 15);

		for (const LeModel& mdl : models) {

			K::Statistics<float>* modelStatsAvg = new K::Statistics<float>();
			K::Statistics<float>* modelStatsMed = new K::Statistics<float>();

			K::Statistics<float>* modelStatsSingle = new K::Statistics<float>();
			pefAll->add(mdl.plotKey, modelStatsSingle);


			int numStuck = 0;
			int numTotal = 0;

			std::cout << "model " << mdl.name << std::endl;


			// ... to perform every walk...
			for (const LeWalk& walk : walks) {

				int numStuckWalk = 0;
				int numTotalWalk = 0;

				// stats for all runs of one walk
				K::Statistics<float> walkStatsAvg;

				for (int i = 0; i < 75; ++i) {

					++numTotal;
					++numTotalWalk;

					K::Statistics<float>* walkStats = deserialize(walk, mdl, i);

					//if (walkStats->getMedian() < 15) {
					if (walkStats->getQuantile(0.75) < 20) {
						//modelStatsSingle->add(*walkStats);
					} else {
						++numStuck;
						++numStuckWalk;
					}

					modelStatsSingle->add(*walkStats);
					walkStatsAvg.add(*walkStats);

				}

				if (modelStatsSingle->getCount() > 0) {
					pefAll->plot();
				}

				std::cout << "|" << walk.name << "(" << walkStatsAvg.getMedian() << "@" << (numStuckWalk*100/numTotalWalk) << "%)\t";

			}

			std::cout << " | OVERALL:" << "(" << modelStatsSingle->getMedian() << "@" << (numStuck*100/numTotal) << "%)" << std::endl;
			//std::cout << " | OVERALL:" << modelStatsSingle->getQuantile(0.25) << " " << modelStatsSingle->getMedian() << " " << modelStatsSingle->getQuantile(0.75) << " " << modelStatsSingle->getAvg() << std::endl;
			sleep(1);

		}

		pefAll->getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 3.7));
		pefAll->getGP().setOutput(Settings::fPathGFX + "/overall-system-error.tex");
		pefAll->writePlotToFile(Settings::fPathGFX + "/overall-system-error.gp");
		pefAll->getPlot().getMargin().set(4, 0.2, 0.1, 1.8);
		pefAll->getPlot().setStringMod(new K::GnuplotStringModLaTeX());

		pefAll->getPlot().getKey().setVisible(true);
		pefAll->getPlot().getKey().setPosition(K::GnuplotKey::Hor::RIGHT, K::GnuplotKey::Ver::BOTTOM);
		pefAll->getPlot().getKey().setWidthIncrement(7);
		pefAll->getPlot().getAxisX().setTicsLabelFormat("%h m");
		pefAll->getPlot().getAxisX().setRange(0, 22);
		pefAll->getPlot().getAxisX().setLabel("");
		pefAll->getPlot().getAxisY().setLabel("filter updates (%)");
		pefAll->getPlot().getAxisY().setLabelOffset(3.0, 0);
		pefAll->plot();

		sleep(100);


	}

	/** perform all walks and write them to file */
	static void walkEverything(Floorplan::IndoorMap* map) {

		const std::vector<LeWalk> walks = allWalks();
		const std::vector<LeModel> models = allModels(map);

		PlotErrFunc* pefAll = new PlotErrFunc();

		// use every model ...
		for (const LeModel& mdl : models) {

			K::Statistics<float>* modelStatsSingle = new K::Statistics<float>();
			pefAll->add(mdl.name, modelStatsSingle);

			// ... to perform every walk...
			for (const LeWalk& walk : walks) {

				//K::Statistics<float> curWalkRepeatStatsAvg;
				//K::Statistics<float> curWalkRepeatStatsMed;

				// ... several times
				//#pragma omp parallel for num_threads(2)
				for (int i = numRepeats*2; i < numRepeats*3; ++i) {

					EvalWalk ew(map);
					ew.walkName = walk.name + " - " + mdl.name + " - " + std::to_string(i);

					// get ground-truth
					ew.absHead = walk.absHead;
					ew.groundTruth = FloorplanHelper::getGroundTruth(map, walk.gtIndices);

					// eval
					std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(ew.grid, *mdl.model, ew.em) );
					ew.pf->setEvaluation( std::move(eval) );

					// data-file
					ew.reader.open(walk.walkFile);
					ew.groundTruthLive = ew.reader.getGroundTruth(map, walk.gtIndices);
					ew.player.setReader(&ew.reader);
					ew.player.setListener(&ew);
					ew.player.start();

					// wait for completion
					ew.player.join();

					// write plots
					ew.writeToFile();

					// add every single error for each timestamp to the overall model error
					modelStatsSingle->reset();
					modelStatsSingle->add(ew.statsErr);

					// export this walks's statistics
					serialize(ew.statsErr, walk, mdl, i);

					pefAll->plot();

				}

				// all repeats done
				const std::string walkname = walk.name + " - " + mdl.name;
				std::cout << "walk result for " << walkname << std::endl;
				//std::cout << "\t" << curWalkRepeatStatsAvg.asString() << std::endl;
				//std::cout << "\t" << curWalkRepeatStatsMed.asString() << std::endl;
				std::cout << std::endl;
				//sleep(1);

			}

			// all walks for one model done
			std::cout << "MODEL RESULTS for " << mdl.name  << std::endl;
			//std::cout << "\t" << modelStatsAvg->asString() << std::endl;
			//std::cout << "\t" << modelStatsMed->asString() << std::endl;
			std::cout << std::endl;
			std::cout << std::endl;
			std::cout << std::endl;

			sleep(2);

		}


	}

	void walk1() {

		// path1
//		absHead = M_PI/2;
//		const std::string path = Settings::path1b;
//		const std::vector<int> pathPoints = Settings::GroundTruth::path1;

		// path2
//		absHead = 0;
//		const std::string path = Settings::path2b;
//		const std::vector<int> pathPoints = Settings::GroundTruth::path2;

		// path_toni_inst_2
		absHead = M_PI;
		const std::string path = Settings::path_toni_inst_2b;
		const std::vector<int> pathPoints = Settings::GroundTruth::path_toni_inst_2;

		runName = "";

		// get ground-truth
		groundTruth = FloorplanHelper::getGroundTruth(map, pathPoints);

		// wifi model
		//WiFiModelLogDistCeiling wifiModel(map);
		//wifiModel.loadXML(Settings::wifiAllFixed);
		//wifiModel.loadXML(Settings::wifiEachOptParPos);

		//WiFiModelPerFloor wifiModel(map);
		//wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel);

		WiFiModelPerBBox wifiModel(map);
		wifiModel.loadXML(Settings::wifiEachOptParPos_perBBox);

		// eval
		std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel, em) );
		pf->setEvaluation( std::move(eval) );

		// data-file
		reader.open(path);
		groundTruthLive = reader.getGroundTruth(map, pathPoints);
		player.setReader(&reader);
		player.setListener(this);
		player.start();

		// wait for completion
		player.join();

	}

	void walk(const std::string& path, const std::vector<int> pathPoints, const std::string& model) {

		absHead = 0;
		runName = "";

		// get ground-truth
		groundTruth = FloorplanHelper::getGroundTruth(map, pathPoints);

		// wifi model
		//WiFiModelLogDistCeiling wifiModel(map);
		//wifiModel.loadXML(Settings::wifiAllFixed);
		//wifiModel.loadXML(Settings::wifiEachOptParPos);

		//WiFiModelPerFloor wifiModel(map);
		//wifiModel.loadXML(Settings::wifiEachOptParPos_multimodel);

		//WiFiModelPerBBox wifiModel(map);
		//wifiModel.loadXML(model);
		WiFiModelFactory fac(map);
		WiFiModel* wifiModel = fac.loadXML(model);

		// eval
		std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, *wifiModel, em) );
		pf->setEvaluation( std::move(eval) );

		// data-file
		reader.open(path);
		groundTruthLive = reader.getGroundTruth(map, pathPoints);
		player.setReader(&reader);
		player.setListener(this);
		player.start();

		// wait for completion
		player.join();

	}



	virtual void onGyroscope(const Timestamp ts, const GyroscopeData data) override {

		const float delta_rad = turnDetect.addGyroscope(ts, data);
		curCtrl.turnSinceLastTransition_rad += delta_rad;

	}

	virtual void onAccelerometer(const Timestamp ts, const AccelerometerData data) override {
		turnDetect.addAccelerometer(ts, data);
		const bool step = stepDetect.add(ts, data);
		if (step) {
			++curCtrl.numStepsSinceLastTransition;
		}
		gotSensorData(ts);
		actDetect.add(ts, data);
	}

	virtual void onGravity(const Timestamp ts, const GravityData data) override {
		;
	}

	virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) override {
		curObs.wifi = data;
		//curObs.wifi = WiFiMeasurements::mix(curObs.wifi, data);
#ifdef PLOT_WIFI
		plotWifi.add(Settings::WiFiModel::vg_eval.group(data));
		plotWifi.plot();
#endif
	}

	virtual void onBarometer(const Timestamp ts, const BarometerData data) override {
		actDetect.add(ts, data);
		curCtrl.activityNew = actDetect.get();
		curObs.activityNew = actDetect.get();
	}

	virtual void onGPS(const Timestamp ts, const GPSData data)  override {
		curObs.gps = data;
	}

	virtual void onCompass(const Timestamp ts, const CompassData data) override {
		const float newAzimuth =- data.azimuth + M_PI/2;		// oriented towards north for our map
		const float newAzimuth_safe = Angle::makeSafe_2PI(newAzimuth);
		const float diff = Angle::getSignedDiffRAD_2PI(curCtrl.compassAzimuth_rad, newAzimuth_safe);
		curCtrl.compassAzimuth_rad += diff * 0.01;
		curCtrl.compassAzimuth_rad = Angle::makeSafe_2PI(curCtrl.compassAzimuth_rad);
		curObs.compassAzimuth_rad = curCtrl.compassAzimuth_rad;
		//curCtrl.compassAzimuth_rad = curCtrl.compassAzimuth_rad * 0.99 + newAzimuth * 0.01;
	}


private:

	/** called when any sensor has received new data */
	void gotSensorData(const Timestamp ts) {
		curObs.currentTime = ts;
		filterUpdateIfNeeded();
	}

	/** check whether its time for a filter update, and if so, execute the update and return true */
	bool filterUpdateIfNeeded() {

		static float avgSum = 0;
		static int avgCount = 0;

		// fixed update rate based on incoming sensor data
		// allows working with live data and faster for offline data
		const Timestamp diff = curObs.currentTime - lastTransition;
		if (diff >= Settings::Filter::updateEvery) {

			// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
			const Timestamp err = diff - Settings::Filter::updateEvery;
			lastTransition = curObs.currentTime - err;

			const Timestamp ts1 = Timestamp::fromUnixTime();
			filterUpdate();
			const Timestamp ts2 = Timestamp::fromUnixTime();
			const Timestamp tsDiff = ts2-ts1;

			//const QString filterTime = QString::number(tsDiff.ms());
			//avgSum += tsDiff.ms(); ++avgCount;
			//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
			return true;

		} else {

			return false;

		}

	}



	K::Statistics<float> statsErr;
	int updateCount = 0;

	int getNumFHWSAPs(const WiFiMeasurements& mes) {
		std::unordered_set<std::string> set;
		for (const WiFiMeasurement& m : mes.entries) {
			std::string mac = m.getAP().getMAC().asString();
			mac.back() = '0';
			set.insert(mac);
		}
		return set.size();
	}


	void writeToFile() {

		std::string path = Settings::fPathGFX;
		std::string base = path + "/walks/" + "walk-" + walkName;

#ifdef PLOT_LIVE
		plotty.gp << "unset arrow 1\n";
		plotty.gp << "unset arrow 2\n";
		plotty.gp << "unset colorbox\n";
		plotty.gp << "unset border\n";
		plotty.gp << "set view equal xy\n";
		plotty.splot.getView().setCamera(74,30);
		plotty.splot.getView().setScaleAll(3.5);
		plotty.splot.getAxisX().setTicsVisible(false);
		plotty.splot.getAxisY().setTicsVisible(false);
		plotty.splot.getAxisZ().setTicsVisible(false);

		plotty.particles.clear();
		plotty.gp.setTerminal("epslatex", K::GnuplotSize(8.6, 4.5));
		plotty.gp.setOutput(base + "-map.tex");
		plotty.writeCodeTo(base + "-map.gp");
		plotty.plot();
#endif

#ifdef PLOT_ERROR_FUNC
		pef.getPlot().getMargin().set(4, 0.2, 0.1, 2.0);
		pef.getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 3.0));
		pef.getGP().setOutput(base + "-error-cum.tex");
		pef.writePlotToFile(base + "-error-cum.gp");
		pef.plot();
#endif

#ifdef PLOT_ERROR_TIME
		pet.getPlot().getMargin().set(4, 0.2, 0.1, 2.0);
		pet.getGP().setTerminal("epslatex", K::GnuplotSize(8.6, 3.0));
		pet.getGP().setOutput(base + "-error-time.tex");
		pet.writePlotToFile(base + "-error-time.gp");
		pet.plot();
#endif

	}

	/** perform a filter-update (called from a background-loop) */
	void filterUpdate() {

		++updateCount;

		MyControl ctrlCopy = curCtrl;
		//static float absHead = relHeadingOffset;
		absHead += ctrlCopy.turnSinceLastTransition_rad;

		//lastEst = curEst;
		curEst = pf->update(&curCtrl, curObs);

		const Point3 curGT = groundTruthLive.get(lastTransition);

		// error between ground-truth and estimation
		const float estRealErr = curEst.position.inMeter().getDistance(curGT);

		WalkResult::Entry e;
		e.ts = lastTransition;
		e.estimation = curEst.position.inMeter();
		e.groundTruth = curGT;
		e.err = estRealErr;
		this->res.entries.push_back(e);

		// start the error-over-time plot after some filter updates
		if (updateCount > 12) {


			statsErr.add(estRealErr);

#ifdef PLOT_ERROR_FUNC
			pef.showMarkers(true, true);
			pef.clear();
			pef.add("", &statsErr);
#endif

#ifdef PLOT_ERROR_TIME
			// timed error
			pet.addErr(lastTransition, estRealErr);
			pet.addB(lastTransition, getNumFHWSAPs(curObs.wifi));
#endif

#ifdef PLOT_LIVE
			// update estimated path
			const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm);
			plotty.pathEst.add(p3/100);
#endif

		}



		// plot
		static int cnt = 0;
		if (++cnt % PLOT_UPDATE_STEP == 0) {

			std::cout << statsErr.asString() << std::endl;

#ifdef PLOT_LIVE
			// estimation and ground-truth
			plotty.setCurEst(curEst.position.inMeter());
			plotty.setGroundTruth(curGT);

			// show particles
			float maxWeight = 0;
			float minWeight = 99;
			plotty.particles.clear();
			for (int i = 0; i < pf->getParticles().size(); i += 10) {
				const auto p = pf->getParticles()[i];
				const K::GnuplotPoint3 p3(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
				plotty.particles.add(p3/100, p.weight);
				if (p.weight > maxWeight) {maxWeight = p.weight;}
				if (p.weight < minWeight) {minWeight = p.weight;}
			}
			plotty.gp << "set cbrange [" << minWeight << ":" << maxWeight << "] \n";

			// show ground-truth
			plotty.pathReal.clear();
			for (const Point3 pt : groundTruth) {
				plotty.pathReal.add(K::GnuplotPoint3(pt.x, pt.y, pt.z));
			}

			std::string title =
					" time " + std::to_string(curObs.currentTime.sec()) +
					" steps: " + std::to_string(ctrlCopy.numStepsSinceLastTransition) +
					" turn: " + std::to_string(ctrlCopy.turnSinceLastTransition_rad) +
					" APs: " + std::to_string(curObs.wifi.entries.size()) +
					" Act: " + std::to_string((int)curCtrl.activityNew) +
					" " + walkName;
			plotty.setTitle(title);

			// relative heading and compass
			{
				Point2 cen(0.1, 0.9);
				Point2 dir(std::cos(absHead), std::sin(absHead));
				Point2 arr = cen + dir * 0.1;
				plotty.gp << "set arrow 1 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n";
				dir = Point2(std::cos(ctrlCopy.compassAzimuth_rad), std::sin(ctrlCopy.compassAzimuth_rad));
				arr = cen + dir * 0.05;
				plotty.gp << "set arrow 2 from screen " << cen.x << "," << cen.y << " to screen " << arr.x << "," << arr.y << "\n";
			}

#endif

#ifdef PLOT_LIVE
			plotty.plot();	
#endif

#ifdef PLOT_ERROR_TIME
			pet.plot();
#endif

#ifdef PLOT_ERROR_FUNC
			pef.plot();
#endif

		}

		//std::this_thread::sleep_for(std::chrono::milliseconds(5));

		curCtrl.resetAfterTransition();

//		const MyGridNode* node = grid->getNodePtrFor(curEst.position);
//		if (node) {
//			try {
//				pathToDest = trans->modDestination.getShortestPath(*node);
//			} catch (...) {;}
//		}
		//	mainController->getMapView()->showGridImportance();

	}

};

#endif // EVALWALK_H