#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 "../PlotErrFunc.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/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/WiFiModelLogDistCeiling.h"

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

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

class EvalWalk : public Offline::Listener {

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

	std::string runName;
	WiFiModelLogDistCeiling wifiModel;

	Plotty plotty;

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

	Timestamp lastTransition;

	MyObservation curObs;
	MyControl curCtrl;
	MyState curEst;

	StepDetection stepDetect;
	TurnDetection turnDetect;

	std::vector<Point3> groundTruth;

	Floorplan::IndoorMap* map;

public:

	EvalWalk(Floorplan::IndoorMap* map) : wifiModel(map), plotty(map), map(map) {

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

		plotty.buildFloorplan();

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

		// TODO: flexible model
		wifiModel.loadAPs(map, Settings::WiFiModel::TXP, Settings::WiFiModel::EXP, Settings::WiFiModel::WAF, false);
		std::unique_ptr<PFEval> eval = std::unique_ptr<PFEval>( new PFEval(grid, wifiModel) );
		pf->setEvaluation( std::move(eval) );

		wifiModel.saveXML("/tmp/test.xml");
		wifiModel.loadXML("/tmp/test.xml");

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

		// 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.50f)));




	}

	void walk1() {

		runName = "path2_forward_simple";
		std::string path = Settings::path1a;
		groundTruth = FloorplanHelper::getGroundTruth(map, Settings::GroundTruth::path1);

		//GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
		pf->setTransition( std::unique_ptr<PFTrans>( new PFTrans(grid)) );

		reader.open(path);
		groundTruthLive = reader.getGroundTruth(map, Settings::GroundTruth::path1);
		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);
	}

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

	virtual void onWiFi(const Timestamp ts, const WiFiMeasurements data) override {
		std::cout << "WIFI" << std::endl;
		curObs.wifi = data;
	}

	virtual void onBarometer(const Timestamp ts, const BarometerData data) override {
		;
	}

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


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

		static PlotErrFunc pef("\\small{error (m)}", "\\small{updates (\\%)}");

		std::cout << "update" << std::endl;

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

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

		const Point3 curGT = groundTruthLive.get(lastTransition);

		plotty.setCurEst(curEst.position.inMeter());
		plotty.setGroundTruth(curGT);

		// error between ground-truth and estimation
		const float estRealErr = curEst.position.inMeter().getDistance(curGT);
		statsErr.add(estRealErr);
		pef.clear();
		pef.add("", statsErr);
		pef.plot();

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

		const K::GnuplotPoint3 p3(curEst.position.x_cm, curEst.position.y_cm, curEst.position.z_cm);
		plotty.pathEst.add(p3/100);

		plotty.particles.clear();
		for (const auto p : pf->getParticles()) {
			const K::GnuplotPoint3 p3(p.state.position.x_cm, p.state.position.y_cm, p.state.position.z_cm);
			plotty.particles.add(p3/100);
		}

		// GT
		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);
		plotty.setTitle(title);

		{
			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";
		}


		// plot
		plotty.plot();

		std::this_thread::sleep_for(std::chrono::milliseconds(30));

		curCtrl.resetAfterTransition();

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

	}

};

#endif // EVALWALK_H