#ifndef SMOOTHINGEVAL1_H
#define SMOOTHINGEVAL1_H

#include "SmoothingEvalBase.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 SmoothingEval1 : public SmoothingEvalBase {

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


        //create the backward smoothing filter
        bf = new K::BackwardSimulation<MyState>(500);


	}


    void fixedIntervallSimpleTrans() {

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

        BarometerEvaluation::barometerSigma = 0.105;
        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)) );

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

	}


};

#endif // EVAL1_H