#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/3D/MapView3D.h"
#include "../ui/debug/InfoWidget.h"

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

#include "State.h"
#include "Filter.h"
#include "Controller.h"
#include "NavControllerListener.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>

#ifndef ANDROID
#include <valgrind/callgrind.h>
#endif

#include "Settings.h"
#include "RegionalResampling.h"
#include "NodeResampling.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>,
		public SensorListener<ActivityData> {


private:

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

	MyObservation curObs;
	MyControl curCtrl;

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

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

	/** the estimated path */
	std::vector<Point3> estPath;

	/** all listeners */
	std::vector<NavControllerListener*> listeners;

public:

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

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

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

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

		// resampling
		std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
		//std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>> resample(new K::ParticleFilterResamplingSimple<MyState>());
		//std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>> resample(new K::ParticleFilterResamplingPercent<MyState>(0.05));
		//std::unique_ptr<RegionalResampling> resample(new RegionalResampling());

		// eval and transition
		wifiModel.loadAPs(im, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF);
		std::unique_ptr<K::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
		std::unique_ptr<K::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));

		// setup the filter
		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(0.75);
		//pf->setNEffThreshold(0.65);		// still too low?
		//pf->setNEffThreshold(0.25);		// too low

		// attach as listener to all sensors
		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);
		SensorFactory::get().getActivity().addListener(this);

		// hacky.. but we need to call this one from the main thread!
		//mainController->getMapView()->showParticles(pf->getParticles());
		qRegisterMetaType<const void*>();

	}

	/** attach a new event listener */
	void addListener(NavControllerListener* l) {
		listeners.push_back(l);
	}

	void start() {

		Assert::isFalse(running, "already started!");
		running = true;
		curCtrl.resetAfterTransition();	// ensure we start empty ;)
		tFilter = std::thread(&NavController::filterUpdateLoop, this);
		tDisplay = std::thread(&NavController::updateMapViewLoop, this);

		// start all sensors
		SensorFactory::get().getAccelerometer().start();
		SensorFactory::get().getGyroscope().start();
		SensorFactory::get().getBarometer().start();
		SensorFactory::get().getWiFi().start();

#ifndef ANDROID
		// #include <valgrind/callgrind.h>
		// run with
		// valgrind --tool=callgrind --quiet  --instr-atstart=no  ./yasmin
		// show with
		// kcachegrind callgrind.out.xxxx
		CALLGRIND_START_INSTRUMENTATION;
#endif

	}

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


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

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

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

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

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

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

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

	void onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) override {
		(void) sensor;
		(void) ts;
		curCtrl.activity = data.curActivity;
		curObs.activity = data.curActivity;
		debugActivity(data.curActivity);
		gotSensorData(ts);
	}

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

private:

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


	void debugActivity(const ActivityData& activity)  {
		QString act;
		switch(activity.curActivity) {
			case ActivityButterPressure::Activity::STAY:	act = "STAY"; break;
			case ActivityButterPressure::Activity::DOWN:	act = "DOWN"; break;
			case ActivityButterPressure::Activity::UP:		act = "UP"; break;
			default:										act = "???"; break;
		}
		Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
	}

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


		while(running && !Settings::Filter::useMainThread) {

//			// 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()));

			const bool wasUpdated = filterUpdateIfNeeded();
			if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); }

		}
	}

	Timestamp lastTransition;

	/** 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; std::cout << "ts:" << curObs.currentTime << " avg:" << (avgSum/avgCount) << std::endl;
			QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
			return true;

		} else {

			return false;

		}

	}


	MyState curEst;
	DijkstraPath<MyGridNode> pathToDest;

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

		//lastEst = curEst;
		curEst = pf->update(&curCtrl, curObs);
		Log::add("Nav", "cur est: " + curEst.position.asString());

		// inform listeners about the new estimation
		for (NavControllerListener* l : listeners) {l->onNewEstimation(curEst.position.inMeter());}

		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");
		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "showParticles", Qt::QueuedConnection, Q_ARG(const void*, &pf->getParticles())), "call failed");

		// update estimated path
		estPath.push_back(curEst.position.inMeter());
		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView3D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
		Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");

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

	}


	const int display_ms = Settings::MapView::msPerFrame.ms();

	/** UI update loop */
	void updateMapViewLoop() {

		while(running) {
			const Timestamp ts1 = Timestamp::fromUnixTime();
			updateMapView();
			const Timestamp ts2 = Timestamp::fromUnixTime();
			const Timestamp tsDiff = ts2-ts1;
			const QString mapViewTime = QString::number(tsDiff.ms());
			//QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
			std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
		}
	}

	Point3 curPosFast;
	Point3 curPosSlow;


	/** update the map-view (called from within a background-loop) */
	void updateMapView() {

		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();

		// how to update the camera
		if (cameraMode == 0) {
			mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m), dir);
		} else if (cameraMode == 1) {
			mainController->getMapView3D()->setLookAt(curPosFast + Point3(0,0,myHeight_m) - dir2*4, dir2);
		} else if (cameraMode == 2) {
			const Point3 spectator = curPosFast + Point3(0,0,25) - dir*15;
			const Point3 spectatorDir = (curPosFast - spectator).normalized();
			mainController->getMapView3D()->setLookEye(spectator);
			mainController->getMapView3D()->setLookDir(spectatorDir);
		}

		mainController->getMapView3D()->setClipAbove(curEst.position.inMeter().z + 2);
		mainController->getMapView3D()->setCurrentEstimation(curEst.position.inMeter(), dir);
		mainController->getMapView2D()->setCurrentEstimation(curEst.position.inMeter(), dir);

	}

};

#endif // NAVCONTROLLER_H