#ifndef EVAL1_H
#define EVAL1_H

#include "EvalBase.h"
#include "../DijkstraMapper.h"
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdate.h>
#include <Indoor/grid/walk/GridWalkRandomHeadingUpdateAdv.h>
#include <Indoor/grid/walk/GridWalkPushForward.h>
#include <Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h>

#include <Indoor/grid/walk/GridWalkSimpleControl.h>
#include <Indoor/grid/walk/GridWalkPathControl.h>
#include <Indoor/grid/walk/GridWalkShortestPathControl.h>
#include "DebugShortestPath.h"

#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingSimple.h>
#include <KLib/math/filter/particles/resampling/ParticleFilterResamplingPercent.h>

#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationRegionalWeightedAverage.h>
#include <KLib/math/filter/particles/estimation/ParticleFilterEstimationOrderedWeightedAverage.h>

class Eval1 : public EvalBase {

public:

	Eval1() {


		pf = new K::ParticleFilter<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, 1120, 150, 3*350, 90)) );



		std::unique_ptr<MyEvaluation> eval = std::unique_ptr<MyEvaluation>( new MyEvaluation() );
		eval.get()->setUsage(true, true, true, true, true);
		pf->setEvaluation( std::move(eval) );

		// resampling step?
		pf->setNEffThreshold(1.0);
		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)));


		//		std::vector<int> wp = path2;// std::reverse(wp.begin(), wp.end());

//		MyGridNode& start = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.front()]) );
//		MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[wp.back()]) );

//		//GridWalkRandomHeadingUpdate<MyGridNode>* walk = new GridWalkRandomHeadingUpdate<MyGridNode>();
//		//GridWalkRandomHeadingUpdateAdv<MyGridNode>* walk = new GridWalkRandomHeadingUpdateAdv<MyGridNode>();
//		//GridWalkPushForward<MyGridNode>* walk = new GridWalkPushForward<MyGridNode>();
//		//GridWalkLightAtTheEndOfTheTunnel<MyGridNode>* walk = new GridWalkLightAtTheEndOfTheTunnel<MyGridNode>(grid, DijkstraMapper(grid), end);
//		//GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
//		GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);

//		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );

//		// path1
////		sr = new SensorReader("./measurements/path1/1/1454345312844.csv"); // forward
////		srt = new SensorReaderTurn("./measurements/path1/1/Turns.txt");
////		srs = new SensorReaderStep("./measurements/path1/1/Steps2.txt");
////		sr = new SensorReader("./measurements/path1/2/1454345421125.csv"); // backward
////		srt = new SensorReaderTurn("./measurements/path1/2/Turns.txt");
////		srs = new SensorReaderStep("./measurements/path1/2/Steps2.txt");

//		// path2
//		sr = new SensorReader("./measurements/path2/1/1454345775306.csv"); // forward
//		srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
//		srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
//		//sr = new SensorReader("./measurements/path2/2/1454346071347.csv"); // backward
//		//srt = new SensorReaderTurn("./measurements/path2/2/Turns.txt");
//		//srs = new SensorReaderStep("./measurements/path2/2/Steps2.txt");

//		// path3
////		sr = new SensorReader("./measurements/path3/1/1454345546308.csv"); // forward
////		srt = new SensorReaderTurn("./measurements/path3/1/Turns.txt");
////		srs = new SensorReaderStep("./measurements/path3/1/Steps2.txt");
////		sr = new SensorReader("./measurements/path3/2/1454345622819.csv"); // backward
////		srt = new SensorReaderTurn("./measurements/path3/2/Turns.txt");
////		srs = new SensorReaderStep("./measurements/path3/2/Steps2.txt");

//		// path4
////		sr = new SensorReader("./measurements/path4/1454595382218.csv"); // forward
////		srt = new SensorReaderTurn("./measurements/path4/Turns.txt");
////		srs = new SensorReaderStep("./measurements/path4/Steps2.txt");



//		gtw = getGroundTruthWay(*sr, floors.gtwp, wp);

	}

	void setEvalFails() {

	}

	/**
	 * starting with bad barometer readings. takes some time to move upwards
	 * will be fixed by using the path
	 */
	void path2_forward_simple() {

		// forward
		runName = "path2_forward_simple";

		BarometerEvaluation::barometerSigma = 0.16;
		sr = new SensorReader("./measurements/path2/1/1454345775306.csv");
		srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
		srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
		gtw = getGroundTruthWay(*sr, floors.gtwp, path2);

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

	}

	void path2_forward_path() {

		// forward
		runName = "path2_forward_path";

		BarometerEvaluation::barometerSigma = 0.16;
		sr = new SensorReader("./measurements/path2/1/1454345775306.csv");
		srt = new SensorReaderTurn("./measurements/path2/1/Turns.txt");
		srs = new SensorReaderStep("./measurements/path2/1/Steps2.txt");
		gtw = getGroundTruthWay(*sr, floors.gtwp, path2);

		MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2.back()]) );
		GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );

	}



//	TODO: plot grid-nodes for stairs for the paper (also look at z-transitions. some have NO x/y change even though they should have!)


	void path3_forward_simple() {

		// forward
		runName = "path3_forward_simple";

		BarometerEvaluation::barometerSigma = 0.16;
		sr = new SensorReader("./measurements/path3/1/1454345546308.csv"); // forward
		srt = new SensorReaderTurn("./measurements/path3/1/Turns.txt");
		srs = new SensorReaderStep("./measurements/path3/1/Steps2.txt");
		gtw = getGroundTruthWay(*sr, floors.gtwp, path3);

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

	}

	void path3_forward_path() {

//		looks like the left stairs here are NOT working as expected?!
//		plot them
//		look at the probability-draw, maybe there is an issue there

		// forward
		runName = "path3_forward_path";

		BarometerEvaluation::barometerSigma = 0.16;
		sr = new SensorReader("./measurements/path3/1/1454345546308.csv"); // forward
		srt = new SensorReaderTurn("./measurements/path3/1/Turns.txt");
		srs = new SensorReaderStep("./measurements/path3/1/Steps2.txt");
		gtw = getGroundTruthWay(*sr, floors.gtwp, path3);

		MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3.back()]) );
		GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );

	}

	void path4_nexus_simple() {

		runName = "path4_nexus_simple";

		BarometerEvaluation::barometerSigma = 0.16;
		sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
		srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
		srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
		gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);

		// remove importance
		for (auto& n : grid) {n.imp = 1;}

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

	}

	void path4_nexus_imp() {

		runName = "path4_nexus_importance";

		BarometerEvaluation::barometerSigma = 0.05;
		sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
		srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
		srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
		gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);

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

	}

	void path4_nexus_path() {

		runName = "path4_nexus_path";

		BarometerEvaluation::barometerSigma = 0.05;
		sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
		srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
		srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
		gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);

		MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
		GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );

	}

	void path4_nexus_path_b() {

		runName = "path4_nexus_path";

		BarometerEvaluation::barometerSigma = 0.05;
		sr = new SensorReader("./measurements/path4/nexus/1454695040555.csv"); // forward
		srt = new SensorReaderTurn("./measurements/path4/nexus/Turns.txt");
		srs = new SensorReaderStep("./measurements/path4/nexus/Steps2.txt");
		gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);

		MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
		DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
		pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );

	}

	void bergwerk_path1_nexus_simple() {

		runName = "bergwerk_path1_nexus_simple";

		BarometerEvaluation::barometerSigma = 0.10;
		sr = new SensorReader("./measurements/bergwerk/path1/nexus/vor/1454775984079.csv"); // forward
		srt = new SensorReaderTurn("./measurements/bergwerk/path1/nexus/vor/Turns.txt");
		srs = new SensorReaderStep("./measurements/bergwerk/path1/nexus/vor/Steps2.txt");
		gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl);

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

	}


};

#endif // EVAL1_H