#include "NavControllerGrid.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"
#include "NodeResampling.h"

#include "../ui/debug/SensorDataWidget.h"
#include "../ui/map/3D/MapView3D.h"
#include "../ui/map/2D/MapView2D.h"
#include "../ui/debug/InfoWidget.h"

#include "../Controller.h"

Q_DECLARE_METATYPE(const void*)

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

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

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

	// resampling
	std::unique_ptr<NodeResampling<MyState, MyGridNode>> resample(new NodeResampling<MyState, MyGridNode>(*grid));
    //std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
    //std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::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<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new PFEval(grid, wifiModel));
    std::unique_ptr<SMC::ParticleFilterTransition<MyState, MyControl>> transition(new PFTrans(grid));

	// setup the filter
    pf = std::unique_ptr<SMC::ParticleFilter<MyState, MyControl, MyObservation>>(new SMC::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.85);         //before 0.75, edit by toni
	//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);

}

void GridBased::NavControllerGrid::start() {

	Assert::isFalse(running, "already started!");
	running = true;
	curCtrl.resetAfterTransition();	// ensure we start empty ;)
	tFilter = std::thread(&NavControllerGrid::filterUpdateLoop, this);
	tDisplay = std::thread(&NavControllerGrid::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 GridBased::NavControllerGrid::stop() {
	Assert::isTrue(running, "not started!");
	running = false;
	tFilter.join();
	tDisplay.join();
}

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

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

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

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

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

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

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

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

/** called when any sensor has received new data */
void GridBased::NavControllerGrid::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 GridBased::NavControllerGrid::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)); }

		}
	}



	/** check whether its time for a filter update, and if so, execute the update and return true */
	bool GridBased::NavControllerGrid::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));
			QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
			return true;

		} else {

			return false;

		}

	}



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

		//lastEst = curEst;
		MyState sCurEst = pf->update(&curCtrl, curObs);
		curEst.pos_m = sCurEst.position.inMeter();
		curEst.head = sCurEst.heading.direction;
		//Log::add("Nav", "cur est: " + curEst.position.asString());

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

		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.pos_m);
		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(sCurEst.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();

	}



	/** UI update loop */
	void GridBased::NavControllerGrid::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));
		}
	}

