YASMIN/nav/NavController.h

#ifndef NAVCONTROLLER_H
#define NAVCONTROLLER_H

#include "../sensors/AccelerometerSensor.h"
#include "../sensors/GyroscopeSensor.h"
#include "../sensors/BarometerSensor.h"
#include "../sensors/WiFiSensor.h"
#include "../sensors/SensorFactory.h"
#include "../sensors/StepSensor.h"
#include "../sensors/TurnSensor.h"

#include "../ui/debug/SensorDataWidget.h"
#include "../ui/map/MapView.h"

#include <Indoor/Assertions.h>
#include <thread>

#include "State.h"
#include "Filter.h"
#include "Controller.h"

#include <KLib/misc/gnuplot/Gnuplot.h>
#include <KLib/misc/gnuplot/GnuplotSplot.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementPoints.h>
#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>

#include "Settings.h"
#include "RegionalResampling.h"

Q_DECLARE_METATYPE(const void*)








class NavController :
		public SensorListener<AccelerometerData>,
		public SensorListener<GyroscopeData>,
		public SensorListener<BarometerData>,
		public SensorListener<WiFiMeasurements>,
		public SensorListener<GPSData>,
		public SensorListener<StepData>,
		public SensorListener<TurnData> {

private:

	Controller* mainController;
	Grid<MyGridNode>* grid;
	WiFiModelLogDistCeiling wifiModel;
	Floorplan::IndoorMap* im;

	MyObservation curObs;
	MyControl curCtrl;

	bool running = false;
	std::thread tUpdate;
	std::thread tDisplay;

	std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>> pf;

public:

	virtual ~NavController() {
		if (running) {stop();}
	}

	NavController(Controller* mainController, Grid<MyGridNode>* grid, Floorplan::IndoorMap* im) : mainController(mainController), grid(grid), wifiModel(im), im(im) {

		wifiModel.loadAPs(im, Settings::wifiTXP, Settings::wifiEXP, Settings::wifiWAF);

		SensorFactory::get().getAccelerometer().addListener(this);
		SensorFactory::get().getGyroscope().addListener(this);
		SensorFactory::get().getBarometer().addListener(this);
		SensorFactory::get().getWiFi().addListener(this);
		SensorFactory::get().getSteps().addListener(this);
		SensorFactory::get().getTurns().addListener(this);

		std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));

		//std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationWeightedAverage<MyState>());
		std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>> estimation(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.1));

		//std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
		//std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.10));
		std::unique_ptr<RegionalResampling> resample(new RegionalResampling());


		std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(wifiModel));
		std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid, &curCtrl));

		pf = std::unique_ptr<K::ParticleFilter<MyState, MyControl, MyObservation>>(new K::ParticleFilter<MyState, MyControl, MyObservation>(Settings::numParticles, std::move(init)));
		pf->setTransition(std::move(transition));
		pf->setEvaluation(std::move(eval));
		pf->setEstimation(std::move(estimation));
		pf->setResampling(std::move(resample));

		pf->setNEffThreshold(1.0);

	}

	void start() {
		Assert::isFalse(running, "already started!");
		running = true;
		tUpdate = std::thread(&NavController::update, this);
		tDisplay = std::thread(&NavController::display, this);
	}

	void stop() {
		Assert::isTrue(running, "not started!");
		running = false;
		tUpdate.join();
		tDisplay.join();
	}

	void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override {
		(void) sensor;
		curObs.currentTime = ts;
	}

	void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override {
		(void) sensor;
		curObs.currentTime = ts;
	}

	void onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) override {
		(void) sensor;
		curObs.currentTime = ts;
	}

	void onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) override {
		(void) sensor;
		(void) ts;
		curObs.currentTime = ts;
		curObs.wifi = data;
	}

	void onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) override {
		(void) sensor;
		(void) ts;
		curObs.currentTime = ts;
		curObs.gps = data;
	}

	void onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) override {
		(void) sensor;
		(void) ts;
		curObs.currentTime = ts;
		curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent;		// set to zero after each transition
	}

	void onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) override {
		(void) sensor;
		(void) ts;
		curObs.currentTime = ts;
		curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent;			// set to zero after each transition
	}

	int cameraMode = 0;
	void toggleCamera() {
		cameraMode = (cameraMode + 1) % 3;
	}

private:

	/** particle-filter update loop */
	void update() {

		Timestamp lastTransition;

		while(running) {

//			// fixed update rate based on the systems time -> LIVE! even for offline data
//			const Timestamp ts1 = Timestamp::fromUnixTime();
//			doUpdate();
//			const Timestamp ts2 = Timestamp::fromUnixTime();
//			const Timestamp needed = ts2-ts1;
//			const Timestamp sleep = Timestamp::fromMS(500) - needed;
//			std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms()));

			// 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 > Timestamp::fromMS(500)) {
				doUpdate();
				lastTransition = curObs.currentTime;
			} else {
				std::this_thread::sleep_for(std::chrono::milliseconds(10));
			}

		}
	}


	MyState curEst;
	//MyState lastEst;

	void doUpdate() {

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

		// hacky.. but we need to call this one from the main thread!
		//mainController->getMapView()->showParticles(pf->getParticles());
		qRegisterMetaType<const void*>();
		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");


		PFTrans* trans = (PFTrans*)pf->getTransition();
		const MyGridNode* node = grid->getNodePtrFor(curEst.position);
		if (node) {
			const DijkstraPath<MyGridNode> path = trans->modDestination.getShortestPath(*node);
		//	mainController->getMapView()->showGridImportance();
			Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView(), "setPath", Qt::QueuedConnection, Q_ARG(const void*, &path)), "call failed");
		}

		/*
		static K::Gnuplot gp;
		K::GnuplotSplot plot;
		K::GnuplotSplotElementLines lines; plot.add(&lines);
		K::GnuplotSplotElementPoints points; plot.add(&points);
		K::GnuplotSplotElementPoints best; plot.add(&best);			best.setPointSize(2); best.setColorHex("#0000ff");

		for (const K::Particle<MyState>& p : pf->getParticles()) {
			const Point3 pos = p.state.position.inMeter();
			points.add(K::GnuplotPoint3(pos.x, pos.y, pos.z));
		}

		for (const Floorplan::Floor* f : im->floors) {
			for (const Floorplan::FloorOutlinePolygon* polygon : f->outline) {
				for (int i = 0; i < polygon->poly.points.size(); ++i) {
					const Point2 p1 = polygon->poly.points[i];
					const Point2 p2 = polygon->poly.points[(i+1)%polygon->poly.points.size()];
					K::GnuplotPoint3 gp1(p1.x, p1.y, f->atHeight);
					K::GnuplotPoint3 gp2(p2.x, p2.y, f->atHeight);
					lines.addSegment(gp1, gp2);
				}
			}
		}

		K::GnuplotPoint3 gpBest(curEst.position.x_cm/100.0f, curEst.position.y_cm/100.0f, curEst.position.z_cm/100.0f);
		best.add(gpBest);

		gp.draw(plot);
		gp.flush();
		*/


	}

	const int display_ms = 50;

	/** UI update loop */
	void display() {
		while(running) {
			doDisplay();
			std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
		}
	}

	Point3 curPosFast;
	Point3 curPosSlow;



	void doDisplay() {

		const float kappa1 = display_ms / 1000.0f;
		const float kappa2 = kappa1 * 0.7;

		const float myHeight_m = 1.80;

		curPosFast = curPosFast * (1-kappa1) + curEst.position.inMeter() * (kappa1);
		curPosSlow = curPosSlow * (1-kappa2) + curEst.position.inMeter() * (kappa2);

		const Point3 dir = (curPosFast - curPosSlow).normalized();
		const Point3 dir2 = Point3(dir.x, dir.y, -0.2).normalized();

		if (cameraMode == 0) {
			mainController->getMapView()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir);
		} else if (cameraMode == 1) {
			mainController->getMapView()->setLookAt(curPosFast + Point3(0,0,myHeight_m) - dir2*4, dir2);
		} else if (cameraMode == 2) {
			const Point3 spectator = curPosFast + Point3(0,0,20) - dir*15;
			const Point3 spectatorDir = (curPosFast - spectator).normalized();
			mainController->getMapView()->setLookEye(spectator);
			mainController->getMapView()->setLookDir(spectatorDir);
		}

		mainController->getMapView()->setCurrentEstimation(curPosFast, dir);


	}

};

#endif // NAVCONTROLLER_H