#ifndef SMOOTHINGEVAL1_H
#define SMOOTHINGEVAL1_H

#include "SmoothingEvalBase.h"
#include "FixedLagEvalBase.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/ParticleFilterEstimationWeightedAverageWithAngle.h>

class SmoothingEval1 : public FixedLagEvalBase {

public:

    SmoothingEval1() {

        int initX = 1120;
        int initY = 150;
        int initZ = 3*350;
        int initHeading = 90;

        // create the particle filterp.state.x_cm = 1120;
        pf = new K::ParticleFilterHistory<MyState, MyControl, MyObservation>( MiscSettings::numParticles, std::unique_ptr<MyInitializer>(new MyInitializer(grid, initX, initY, initZ, initHeading)));

		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::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<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)));


        //create the backward smoothing filter
        bf = new K::BackwardSimulation<MyState>(500);
        bf->setSampler( std::unique_ptr<K::CumulativeSampler<MyState>>(new K::CumulativeSampler<MyState>()));


	}


    void fixedIntervallSimpleTransPath1() {

        runName = "fixedIntervallSimpleTransPath1";
        bool smoothing_resample = false;
        smoothing_time_delay = 1;

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

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

        //Smoothing using Simple Trans
        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
        if(smoothing_resample)
            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
        bf->setTransition(std::unique_ptr<MySmoothingTransitionSimple>( new MySmoothingTransitionSimple()) );

	}

    void fixedIntervallSimpleTransPath4(){

        runName = "fixedIntervallSimpleTransPath4";

        BarometerEvaluation::barometerSigma = 0.10;
        sr = new SensorReader("./measurements/bergwerk/path4/nexus/vor/1454776525797.csv"); // forward
        srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/vor/Turns.txt");
        srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/vor/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)) );


        //Smoothing Variables
        smoothing_walk_mu = 0.7;
        smoothing_walk_sigma = 0.5;
        smoothing_heading_sigma = 5.0;
        smoothing_baro_sigma = 0.05;

        bool smoothing_resample = false;
        smoothing_time_delay = 1;

        //Smoothing using Simple Trans
        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>>(new K::ParticleFilterEstimationWeightedAverageWithAngle<MyState>()));
        if(smoothing_resample)
            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
    }

    // ============================================================ Dijkstra ============================================== //

    // Hier dauert sehr laaaaaaaannnnnnnggge @Frank
    void fixedIntervallDijkstraTransPath4(){

        runName = "fixedIntervallSimpleTrans";
        bool smoothing_resample = false;
        smoothing_time_delay = 1;

        BarometerEvaluation::barometerSigma = 0.10;
        sr = new SensorReader("./measurements/bergwerk/path4/nexus/1454776525797.csv"); // forward
        srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/Turns.txt");
        srs = new SensorReaderStep("./measurements/bergwerk/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)) );

        //Smoothing using Simple Trans
        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
        if(smoothing_resample)
            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
        bf->setTransition(std::unique_ptr<MySmoothingTransition>( new MySmoothingTransition(&grid)) );
    }



    /* ------------------------------------------------------------------------------------------------------------------------------- */
    /* ----------------------------------------------- Bergwerk ------ Fusion 2016 --------------------------------------------------- */
    /* ------------------------------------------------------------------------------------------------------------------------------- */

    void bergwerk_path1_nexus() {
        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);

        //Smoothing Variables
        smoothing_walk_mu = 0.7;
        smoothing_walk_sigma = 0.5;
        smoothing_heading_sigma = 15.0;
        smoothing_baro_sigma = 0.05;

        bool smoothing_resample = false;
        smoothing_time_delay = 1;

        //Smoothing using Simple Trans
        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
        if(smoothing_resample)
            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
    }

    void bergwerk_path1_nexus_simple() {
        runName = "bergwerk_path1_nexus_simple_fixedlag";
        bergwerk_path1_nexus();

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

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

    }


    void bergwerk_path1_nexus_imp() {

        runName = "bergwerk_path1_nexus_importance_fixedlag";
        bergwerk_path1_nexus();

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

    }

    void bergwerk_path1_nexus_multi() {

        runName = "bergwerk_path1_nexus_multi_fixedlag";
        bergwerk_path1_nexus();

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

    }

    void bergwerk_path1_nexus_shortest() {

        runName = "bergwerk_path1_nexus_shortest_fixedlag";
        bergwerk_path1_nexus();

        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.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_path2_nexus() {
        BarometerEvaluation::barometerSigma = 0.10;
        sr = new SensorReader("./measurements/bergwerk/path2/nexus/vor/1454779863041.csv"); // forward
        srt = new SensorReaderTurn("./measurements/bergwerk/path2/nexus/vor/Turns.txt");
        srs = new SensorReaderStep("./measurements/bergwerk/path2/nexus/vor/Steps2.txt");
        gtw = getGroundTruthWay(*sr, floors.gtwp, path2dbl);

        smoothing_walk_mu = 0.7;
        smoothing_walk_sigma = 0.5;
        smoothing_heading_sigma = 5.0;
        smoothing_baro_sigma = 0.05;

        bool smoothing_resample = false;
        smoothing_time_delay = 1;

        //Smoothing using Simple Trans
        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
        if(smoothing_resample)
            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
    }

    void bergwerk_path2_nexus_simple() {

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

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

    }


    void bergwerk_path2_nexus_imp() {

        runName = "bergwerk_path2_nexus_importance_fixedlag";
        bergwerk_path2_nexus();

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

    }

    void bergwerk_path2_nexus_multi() {

        runName = "bergwerk_path2_nexus_multi_fixedlag";
        bergwerk_path2_nexus();

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

    }

    void bergwerk_path2_nexus_shortest() {

        runName = "bergwerk_path2_nexus_shortest_fixedlag";
        bergwerk_path2_nexus();

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

    }

    /* ------------------------------------------------------------------------------------------------------------------------------- */

    void bergwerk_path3_nexus() {
        BarometerEvaluation::barometerSigma = 0.10;
        sr = new SensorReader("./measurements/bergwerk/path3/nexus/vor/1454782562231.csv"); // forward
        srt = new SensorReaderTurn("./measurements/bergwerk/path3/nexus/vor/Turns.txt");
        srs = new SensorReaderStep("./measurements/bergwerk/path3/nexus/vor/Steps2.txt");
        gtw = getGroundTruthWay(*sr, floors.gtwp, path3dbl);

        smoothing_walk_mu = 0.7;
        smoothing_walk_sigma = 0.5;
        smoothing_heading_sigma = 5.0;
        smoothing_baro_sigma = 0.05;

        bool smoothing_resample = false;
        smoothing_time_delay = 1;

        //Smoothing using Simple Trans
        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
        if(smoothing_resample)
            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
    }

    void bergwerk_path3_nexus_simple() {

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

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

    }


    void bergwerk_path3_nexus_imp() {

        runName = "bergwerk_path3_nexus_importance_fixedlag";
        bergwerk_path3_nexus();

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

    }

    void bergwerk_path3_nexus_multi() {

        runName = "bergwerk_path3_nexus_multi_fixedlag";
        bergwerk_path3_nexus();

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

    }

    void bergwerk_path3_nexus_shortest() {

        runName = "bergwerk_path3_nexus_shortest_fixedlag";
        bergwerk_path3_nexus();

        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.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_path4_nexus() {
        BarometerEvaluation::barometerSigma = 0.10;
        sr = new SensorReader("./measurements/bergwerk/path4/nexus/vor/1454776525797.csv"); // forward
        srt = new SensorReaderTurn("./measurements/bergwerk/path4/nexus/vor/Turns.txt");
        srs = new SensorReaderStep("./measurements/bergwerk/path4/nexus/vor/Steps2.txt");
        gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);

        smoothing_walk_mu = 0.7;
        smoothing_walk_sigma = 0.5;
        smoothing_heading_sigma = 5.0;
        smoothing_baro_sigma = 0.05;

        bool smoothing_resample = false;
        smoothing_time_delay = 1;

        //Smoothing using Simple Trans
        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
        if(smoothing_resample)
            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
    }

    void bergwerk_path4_nexus_simple() {

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

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

    }


    void bergwerk_path4_nexus_imp() {

        runName = "bergwerk_path4_nexus_importance_fixedlag";
        bergwerk_path4_nexus();

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

    }

    void bergwerk_path4_nexus_multi() {

        runName = "bergwerk_path4_nexus_multi_fixedlag";
        bergwerk_path4_nexus();

        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 bergwerk_path4_nexus_shortest() {

        runName = "bergwerk_path4_nexus_shortest_fixedlag";
        bergwerk_path4_nexus();

        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_galaxy() {
        BarometerEvaluation::barometerSigma = 0.20;
        stepSize = 0.9;
        sr = new SensorReader("./measurements/bergwerk/path1/galaxy/vor/1454776168794.csv"); // forward
        srt = new SensorReaderTurn("./measurements/bergwerk/path1/galaxy/vor/Turns.txt");
        srs = new SensorReaderStep("./measurements/bergwerk/path1/galaxy/vor/Steps2.txt");
        gtw = getGroundTruthWay(*sr, floors.gtwp, path1dbl);

        smoothing_walk_mu = 0.7;
        smoothing_walk_sigma = 0.5;
        smoothing_heading_sigma = 5.0;
        smoothing_baro_sigma = 0.15;

        bool smoothing_resample = false;
        smoothing_time_delay = 1;

        //Smoothing using Simple Trans
        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
        if(smoothing_resample)
            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
    }

    void bergwerk_path1_galaxy_simple() {
        runName = "bergwerk_path1_galaxy_simple_fixedlag";
        bergwerk_path1_galaxy();
        for (auto& n : grid) {n.imp = 1;}	// remove importance
        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
    }

    void bergwerk_path1_galaxy_multi() {
        runName = "bergwerk_path1_galaxy_multi_fixedlag";
        bergwerk_path1_galaxy();
        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
        walk->pOther = 0.15;
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
    }

    void bergwerk_path1_galaxy_shortest() {
        runName = "bergwerk_path1_galaxy_shortest_fixedlag";
        bergwerk_path1_galaxy();
        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path1dbl.back()]) );
        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
        walk->pOther = 0.15;
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
    }

    /* ------------------------------------------------------------------------------------------------------------------------------- */

    void bergwerk_path2_galaxy() {
        BarometerEvaluation::barometerSigma = 0.20;
        stepSize = 0.9;
        sr = new SensorReader("./measurements/bergwerk/path2/galaxy/vor/1454780113404.csv"); // forward
        srt = new SensorReaderTurn("./measurements/bergwerk/path2/galaxy/vor/Turns.txt");
        srs = new SensorReaderStep("./measurements/bergwerk/path2/galaxy/vor/Steps2.txt");
        gtw = getGroundTruthWay(*sr, floors.gtwp, path2dbl);

        smoothing_walk_mu = 0.7;
        smoothing_walk_sigma = 0.5;
        smoothing_heading_sigma = 5.0;
        smoothing_baro_sigma = 0.15;

        bool smoothing_resample = false;
        smoothing_time_delay = 1;

        //Smoothing using Simple Trans
        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
        if(smoothing_resample)
            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
    }

    void bergwerk_path2_galaxy_simple() {
        runName = "bergwerk_path2_galaxy_simple_fixedlag";
        bergwerk_path2_galaxy();
        for (auto& n : grid) {n.imp = 1;}	// remove importance
        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
    }

    void bergwerk_path2_galaxy_multi() {
        runName = "bergwerk_path2_galaxy_multi_fixedlag";
        bergwerk_path2_galaxy();
        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
        walk->pOther = 0.15;
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
    }

    void bergwerk_path2_galaxy_shortest() {
        runName = "bergwerk_path2_galaxy_shortest_fixedlag";
        bergwerk_path2_galaxy();
        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path2dbl.back()]) );
        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
        walk->pOther = 0.15;
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
    }

    /* ------------------------------------------------------------------------------------------------------------------------------- */

    void bergwerk_path3_galaxy() {
        BarometerEvaluation::barometerSigma = 0.20;
        stepSize = 0.9;
        sr = new SensorReader("./measurements/bergwerk/path3/galaxy/vor/1454782896548.csv"); // forward
        srt = new SensorReaderTurn("./measurements/bergwerk/path3/galaxy/vor/Turns.txt");
        srs = new SensorReaderStep("./measurements/bergwerk/path3/galaxy/vor/Steps2.txt");
        gtw = getGroundTruthWay(*sr, floors.gtwp, path3dbl);

        smoothing_walk_mu = 0.7;
        smoothing_walk_sigma = 0.5;
        smoothing_heading_sigma = 5.0;
        smoothing_baro_sigma = 0.15;

        bool smoothing_resample = false;
        smoothing_time_delay = 1;

        //Smoothing using Simple Trans
        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
        if(smoothing_resample)
            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
    }

    void bergwerk_path3_galaxy_simple() {
        runName = "bergwerk_path3_galaxy_simple_fixedlag";
        bergwerk_path3_galaxy();
        for (auto& n : grid) {n.imp = 1;}	// remove importance
        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
    }

    void bergwerk_path3_galaxy_multi() {
        runName = "bergwerk_path3_galaxy_multi_fixedlag";
        bergwerk_path3_galaxy();
        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
        walk->pOther = 0.15;
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
    }

    void bergwerk_path3_galaxy_shortest() {
        runName = "bergwerk_path3_galaxy_shortest_fixedlag";
        bergwerk_path3_galaxy();
        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path3dbl.back()]) );
        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
        walk->pOther = 0.15;
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
    }


    /* ------------------------------------------------------------------------------------------------------------------------------- */


    void bergwerk_path4_galaxy() {
        BarometerEvaluation::barometerSigma = 0.20;
        stepSize = 0.9;
        sr = new SensorReader("./measurements/bergwerk/path4/galaxy/vor/1454779020844.csv"); // forward
        srt = new SensorReaderTurn("./measurements/bergwerk/path4/galaxy/vor/Turns.txt");
        srs = new SensorReaderStep("./measurements/bergwerk/path4/galaxy/vor/Steps2.txt");
        gtw = getGroundTruthWay(*sr, floors.gtwp, path4dbl);

        smoothing_walk_mu = 0.7;
        smoothing_walk_sigma = 0.5;
        smoothing_heading_sigma = 5.0;
        smoothing_baro_sigma = 0.15;

        bool smoothing_resample = false;
        smoothing_time_delay = 1;

        //Smoothing using Simple Trans
        bf->setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
        if(smoothing_resample)
            bf->setResampling( std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()) );
        bf->setTransition(std::unique_ptr<MySmoothingTransitionExperimental>( new MySmoothingTransitionExperimental()) );
    }

    void bergwerk_path4_galaxy_simple() {
        runName = "bergwerk_path4_galaxy_simple_fixedlag";
        bergwerk_path4_galaxy();
        for (auto& n : grid) {n.imp = 1;}	// remove importance
        GridWalkSimpleControl<MyGridNode>* walk = new GridWalkSimpleControl<MyGridNode>();
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
    }

    void bergwerk_path4_galaxy_multi() {
        runName = "bergwerk_path4_galaxy_multi_fixedlag";
        bergwerk_path4_galaxy();
        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
        GridWalkPathControl<MyGridNode>* walk = new GridWalkPathControl<MyGridNode>(grid, DijkstraMapper(grid), end);
        walk->pOther = 0.15;
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );
    }

    void bergwerk_path4_galaxy_shortest() {

        runName = "bergwerk_path4_galaxy_shortest_fixedlag";
        bergwerk_path4_galaxy();
        MyGridNode& end = (MyGridNode&)grid.getNodeFor( conv(floors.gtwp[path4dbl.back()]) );
        DebugShortestPath<MyGridNode>* walk = new DebugShortestPath<MyGridNode>(grid, DijkstraMapper(grid), end, this->floors);
        //walk->times = 2;
        walk->pOther = 0.15;
        pf->setTransition( std::unique_ptr<MyTransition>( new MyTransition(grid, *walk)) );

    }


};

#endif // EVAL1_H