#ifndef FIXEDLAGEVALBASE_H
#define FIXEDLAGEVALBASE_H

#include "../Settings.h"
#include "../Helper.h"
#include "../Vis.h"

#include <KLib/math/filter/particles/ParticleFilter.h>
#include <KLib/math/filter/particles/ParticleFilterHistory.h>
#include <KLib/math/filter/smoothing/BackwardSimulation.h>
#include <KLib/math/filter/smoothing/CondensationBackwardFilter.h>
#include <KLib/math/filter/smoothing/sampling/ParticleTrajectorieSampler.h>
#include <KLib/math/filter/smoothing/sampling/CumulativeSampler.h>
#include <KLib/math/statistics/Statistics.h>

#include "GroundTruthWay.h"

#include "../particles/MyState.h"
#include "../particles/MyObservation.h"
#include "../particles/MyEvaluation.h"
#include "../particles/MyTransition.h"
#include "../particles/MyInitializer.h"
#include "../particles/smoothing/MySmoothingTransition.h"
#include "../particles/smoothing/MySmoothingTransitionSimple.h"
#include "../particles/smoothing/MySmoothingTransitionExperimental.h"

#include "../reader/SensorReader.h"
#include "../reader/SensorReaderStep.h"
#include "../reader/SensorReaderTurn.h"
#include "../reader/SensorReaderAccel.h"

#include "../lukas/TurnObservation.h"
#include "../lukas/StepObservation.h"
#include "../lukas/ActivityDetection.h"

#include "../toni/BarometerSensorReader.h"

#include "../frank/WiFiSensorReader.h"
#include "../frank/BeaconSensorReader.h"
#include "../frank/OrientationSensorReader.h"

class FixedLagEvalBase {

protected:

	Grid<MyGridNode> grid;
	Helper::FHWSFloors floors;
	Vis vis;

    K::ParticleFilterHistory<MyState, MyControl, MyObservation>* pf;
    K::BackwardFilter<MyState>* bf;

	SensorReader* sr;
	SensorReaderTurn* srt;
    SensorReaderStep* srs;


	std::string runName;

	GroundTruthWay gtw;

	// OLD
	//std::vector<int> way0 = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
	//std::vector<int> way1 = {29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 13, 14, 15, 16, 17, 18, 19, 2, 1, 0};
	//std::vector<int> way2 = {29, 28, 27, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 1, 2, 19, 18, 17, 16, 15, 14, 13, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29};

	// NEW
	std::vector<int> path1 = {29, 28,27,26,255,25,24,23,22,21,20};
	std::vector<int> path1dbl = {29, 29, 28,27,26,255,25,24,23,22,21,20};
	std::vector<int> path2 = {19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
	std::vector<int> path2dbl = {19, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 23, 7, 6};
	std::vector<int> path3 = {5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
	std::vector<int> path3dbl = {5, 5, 27, 26, 255, 25, 4, 3, 2, 215, 1, 0, 30, 31};
	std::vector<int> path4 = {29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};
    std::vector<int> path4dbl = {29, 29, 28, 27, 32, 33, 34, 35, 36, 10, 9, 8, 22, 37, 38, 39, 40, 41, 42, 43, 44};	// duplicate 1st waypoint!

    float stepSize = 0.71;

public:

    FixedLagEvalBase() : grid(MiscSettings::gridSize_cm), floors(Helper::getFloors(grid)) {

		// build the grid
		Helper::buildTheGrid(grid, floors);

		// setup the visualisation
		vis.addFloor(floors.f0, floors.h0);
		vis.addFloor(floors.f1, floors.h1);
		vis.addFloor(floors.f2, floors.h2);
		vis.addFloor(floors.f3, floors.h3);

		vis.floors.setColorHex("#666666");
		vis.groundTruth.setCustomAttr("dashtype 3");
		vis.groundTruth.setColorHex("#009900");
        vis.gp << "unset cbrange\n";


	}

	static GridPoint conv(const Point3& p) {
		return GridPoint(p.x, p.y, p.z);
	}

	GroundTruthWay getGroundTruthWay(SensorReader& sr, const std::unordered_map<int, Point3>& waypoints, std::vector<int> ids) {

		// construct the ground-truth-path by using all contained waypoint ids
		std::vector<Point3> path;
		for (int id : ids) {
			auto it = waypoints.find(id);
			if(it == waypoints.end()) {throw "not found";}
			path.push_back(it->second);
		}

		// new created the timed path
		GroundTruthWay gtw;
		int i = 0;
		while (sr.hasNext()) {
			const SensorEntry se = sr.getNext();
			if (se.data.empty()) {continue;}		// why necessary??
			if (se.idx == 99) {
				gtw.add(se.ts, path[i]);
				++i;
			}
		}

		// ensure the sensor-data contained usable timestamps for the ground-truth mapping
		assert(i>0);

		sr.rewind();
		return gtw;

	}

	void run() {

        // sensor numbers
        const int s_wifi = 8; const int s_beacons = 9; const int s_barometer = 5; const int s_orientation = 6;
		const int s_accel = 0;

        //const int s_linearAcceleration = 2;

        std::list<TurnObservation> turn_observations;
        std::list<StepObservation> step_observations;

        //Create an BarometerSensorReader
        BarometerSensorReader baroSensorReader;

		// activity detection
		ActivityDetection actDet;


        //Read all turn Observations
        while(srt->hasNext()) {

            SensorEntryTurn set = srt->getNext();
            TurnObservation to;

            to.ts = set.ts;
            to.delta_heading = set.delta_heading;
            to.delta_motion = set.delta_motion;

            turn_observations.push_back(to);
        }


        //Step Observations
        while(srs->hasNext()) {

            SensorEntryStep ses = srs->getNext();
            StepObservation so;

            so.ts = ses.ts;

            step_observations.push_back(so);
        }



        // the to-be-evaluated observation
        MyObservation obs;
        obs.step = new StepObservation(); obs.step->steps = 0;
        obs.turn = new TurnObservation(); obs.turn->delta_heading = 0; obs.turn->delta_motion = 0;

        // control data
        MyControl ctrl;

        //History of all estimated particles. Used for smoothing
        std::vector<std::vector<K::Particle<MyState>>> pfHistory;
        std::vector<Point3> smoothedEst;
        std::vector<uint64_t> tsHistory;

        std::vector<Point3> pathEst;

        uint64_t lastTransitionTS = 0;
        int64_t start_time = -1;

        K::Statistics<double> statsFiltering;
        K::Statistics<double> statsSmoothing;
		K::Statistics<float> statsDistFiltering;
		K::Statistics<float> statsDistSmoothing;
        int cnt = 0;

		// error per update-step
		std::vector<float> errorsNorm;
		std::vector<float> errorsSmooth;
		std::vector<float> errorsDistNorm;
		std::vector<float> errorsDistSmooth;

		// number of updates to skip before starting the smoothing process
		const int skip = 35;


        //stats file
        std::ofstream statsout("/tmp/unsmoothed_" + runName + ".stats");

        //heading
        double currentHeadingGivenByLukas = 0.0;

        // process each single sensor reading
        while(sr->hasNext()) {

            // get the next sensor reading from the CSV
            const SensorEntry se = sr->getNext();

            //start_time needed for time calculation of steps and turns
            obs.latestSensorDataTS = se.ts;
            if (start_time == -1) {start_time = se.ts;}
            int64_t current_time = se.ts - start_time;

            switch(se.idx) {

                case s_wifi: {
                    obs.wifi = WiFiSensorReader::readWifi(se);
                    break;
                }

                case s_beacons: {
                    BeaconObservationEntry boe = BeaconSensorReader::getBeacon(se);
                    if (!boe.mac.empty()) {
                        obs.beacons.entries.push_back(boe);
                    }			// add the observed beacon
                    obs.beacons.removeOld(obs.latestSensorDataTS);
                    break;
                }

                case s_barometer: {
                    obs.barometer = baroSensorReader.readBarometer(se);
					actDet.addBaro(baroSensorReader.getHPA(se));
                    break;
                }

				case s_accel: {
					float acc[3];
					SensorReaderAccel sre; sre.read(se, acc);
					actDet.addAccel(acc);
					break;
				}

//					case s_linearAcceleration:{
//						baroSensorReader.readVerticalAcceleration(se);
//						break;
//					}

                case s_orientation: {
                    obs.orientation = OrientationSensorReader::read(se);
                    break;
                }

            }

            // process all occurred turns
            while (!step_observations.empty() && current_time > step_observations.front().ts) {
                const StepObservation _so = step_observations.front(); step_observations.pop_front(); (void) _so;
                obs.step->steps++;
                ctrl.walked_m = obs.step->steps * stepSize;
            }

            // process all occurred steps
            while (!turn_observations.empty() && current_time > turn_observations.front().ts) {
                const TurnObservation _to = turn_observations.front(); turn_observations.pop_front();
                obs.turn->delta_heading += _to.delta_heading;
                obs.turn->delta_motion += _to.delta_motion;
                ctrl.headingChange_rad = Angle::degToRad(obs.turn->delta_heading);

                currentHeadingGivenByLukas = obs.turn->delta_heading;

            }

			// currently detected activity
			// TODO: feed sensor values!
            ctrl.currentActivitiy = actDet.getCurrentActivity();

            // this is just for testing purposes
            obs.currentActivity = actDet.getCurrentActivity();

            // time for a transition?
            if (se.ts - lastTransitionTS > MiscSettings::timeSteps) {

                lastTransitionTS = se.ts;

                // timed updates
                ((MyTransition*)pf->getTransition())->setCurrentTime(lastTransitionTS);

                // update the particle filter (transition + eval), estimate a new current position and add it to the estimated path
                const MyState est = pf->update(&ctrl, obs);
                const Point3 curEst = est.pCur;

                //EXPERIMENTAL: Set all Particle Angles to the estimated angle of the particle set
//                if(cnt % 30 == 0){
//                    pf->setAngle(est.avgAngle * 3.14159265359 / 180);
//                }

                // error calculation. compare ground-truth to estimation
                const int offset = 0;
                const Point3 curGT = gtw.getPosAtTime(se.ts - offset);
                const Point3 diff = curEst - curGT;


                pathEst.push_back(curEst);
                const float err = diff.length();
				const float errDist = gtw.getMinDist(curEst);	// minimum distance between estimation and ground-truth



				// count number of transitions
				++cnt;

                 if (cnt > 35) {
                     statsFiltering.add(err);
					 statsDistFiltering.add(errDist);
                     errorsNorm.push_back(err);
					 errorsDistNorm.push_back(errDist);
                     //std::cout << "FilteringTime: " << se.ts << " " << statsFiltering.asString() << std::endl;
                     //std::cout << "FilteringDist: " << se.ts << " " << statsDistFiltering.asString() << std::endl;
                 }

				if(cnt > skip){
                    //save the current estimation for later smoothing.
                    pfHistory.push_back(pf->getNonResamplingParticles());
                    tsHistory.push_back(se.ts);
				}


                //fixed-lag smoothing
                MyState estBF;
                if(pfHistory.size() > MiscSettings::lag){

                    bf->reset();

                    //use every n-th (std = 1) particle set of the history within a given lag (std = 5)
                    for(int i = 0; i <= MiscSettings::lag; i += MiscSettings::fixedLagGap){

                        ((MySmoothingTransitionSimple*)bf->getTransition())->setCurrentTime(tsHistory[(tsHistory.size() - 1) - i]);
                        estBF = bf->update(pfHistory[(pfHistory.size() - 1) - i]);

                    }

                    //use and visualize the smoothed particle set
                    const Point3 curSmoothedEst = estBF.pCur;

                    smoothedEst.push_back(curSmoothedEst);

                    // error calculation. compare ground-truth to estimation
                    //const Point3 curGTSmoothed = gtw.getPosAtTime(se.ts - (MiscSettings::lag * MiscSettings::timeSteps));
                    const Point3 curGTSmoothed = gtw.getPosAtTime(tsHistory[(tsHistory.size() - 1) - MiscSettings::lag]);
                    const Point3 diffSmoothed = curSmoothedEst - curGTSmoothed;


					// skip the first 35 scans due to uniform distribution start


                    const float errSmoothed = diffSmoothed.length();
					const float errDistSmoothed = gtw.getMinDist(curSmoothedEst);	// minimum distance between smoothed-estimation and ground-truth

                    statsSmoothing.add(errSmoothed);
					statsDistSmoothing.add(errDistSmoothed);
					errorsSmooth.push_back(errSmoothed);
					errorsDistSmooth.push_back(errDistSmoothed);

                    //std::cout << "SmoothingTime: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsSmoothing.asString() << std::endl;
                    //std::cout << "SmoothingDist: " << tsHistory[(tsHistory.size() - 1) - MiscSettings::lag] << " " << statsDistSmoothing.asString() << std::endl;

                    //plot
                    vis.clearStates();
                    for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
                        const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
                        vis.addState(p.state.walkState);
                    }
                    //vis.setTimestamp(se.ts);
                    vis.setFilteringMedian(statsFiltering.getMedian());
                    vis.setSmoothingMedian(statsSmoothing.getMedian());
                    vis.addGroundTruth(gtw);
                    vis.addEstPath(pathEst);
                    vis.addSmoothPath(smoothedEst);
                    vis.setEstAndShould(curEst, curGT);
                    vis.setEstAndShould2(curSmoothedEst, curGTSmoothed);

                    if (obs.barometer != nullptr) {
                        vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
                    }
                    vis.gp << "set label 111 '" << ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";
					vis.gp << "set label 112 '" << "Act: " << actDet.toString() << "' at screen 0.1,0.15\n";

                    //double avgAngleRad = estBF.avgAngle * 180/3.14159265359;

                    //std::cout << "Measurement: "<< std::fmod((-(obs.orientation.values[0] * 180/3.14159265359) + 720 + 30), 360)  << std::endl;


                    //std::cout << "Measurement: "<< std::fmod(((obs.orientation.values[0] * 180/3.14159265359) + 720 + 60), 360)  << std::endl;

//                    std::cout << "MeasurementLukas: " << currentHeadingGivenByLukas << std::endl;
//                    std::cout << "EstimationS: " << estBF.avgAngle << std::endl;
//                    std::cout << "EstimationF: " << est.avgAngle << std::endl;
                    //vis.gp << "set label 113 ' EstAngle:" << avgAngleRad << "' at screen 0.1,0.15\n";

                    //vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";

                    Point2 p1(0.1, 0.1);
                    Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
                    //Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
                    vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";

                    //vis.show();

                    // prevent gnuplot errors
                   //usleep(1000*33);
                }
            }

        }
        statsout.close();


		{	// detailled error-description (normally filtered)
			std::ofstream oError("/tmp/err_norm_" + runName + ".dat");
			for (float f : errorsNorm) {oError << f << "\n";}
			oError.close();
		}

		{	// detailled error-description (smothed)
			std::ofstream oError("/tmp/err_smooth_" + runName + ".dat");
			for (float f : errorsSmooth) {oError << f << "\n";}
			oError.close();
		}

		{	// detailled distance-error-description (normally filtered)
			std::ofstream oError("/tmp/err_dist_norm_" + runName + ".dat");
			for (float f : errorsDistNorm) {oError << f << "\n";}
			oError.close();
		}

		{	// detailled distance-error-description (smothed)
			std::ofstream oError("/tmp/err_dist_smooth_" + runName + ".dat");
			for (float f : errorsDistSmooth) {oError << f << "\n";}
			oError.close();
		}

        // plot
        //vis.clearStates();
        vis.gp.setTerminal("png", K::GnuplotSize(2560 * 2.54, 1440 * 2.54) );
        vis.gp.setOutput("/tmp/" + runName + ".png");
        vis.gp << "set view 60," << 40 << "\n"; //For fixed position

        for (int j = 0; j < (int) bf->getbackwardParticles().back().size(); j+=15) {
            const K::Particle<MyState>& p = bf->getbackwardParticles().back()[j];
            vis.addState(p.state.walkState);
        }
        //vis.setTimestamp(se.ts);
        vis.setFilteringMedian(statsFiltering.getMedian());
        vis.setSmoothingMedian(statsSmoothing.getMedian());
        vis.addGroundTruth(gtw);
        vis.addEstPath(pathEst);
        vis.addSmoothPath(smoothedEst);
        //vis.setEstAndShould(curEst, curGT);
        //vis.setEstAndShould2(curSmoothedEst, curGTSmoothed);

        if (obs.barometer != nullptr) {
            vis.gp << "set label 112 'baro: " << obs.barometer->hpa << "' at screen 0.1,0.2\n";
        }
        vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << ctrl.headingChange_rad << "' at screen 0.1,0.1\n";

        //vis.gp << "set label 111 '" <<ctrl.walked_m << ":" << obs.orientation.values[0] << "' at screen 0.1,0.1\n";

        Point2 p1(0.1, 0.1);
        Point2 p2 = p1 + Angle::getPointer(ctrl.headingChange_rad) * 0.05;
        //Point2 p2 = p1 + Angle::getPointer(obs.orientation.values[0]) * 0.05;
        vis.gp << "set arrow 999 from screen " << p1.x<<","<<p1.y << " to screen " << p2.x<<","<<p2.y<<"\n";

        vis.show();
        vis.show();
        vis.show();
        vis.show();
        // prevent gnuplot errors
        usleep(1000*33);


        // append error for each run to a file
        std::ofstream oTError("/tmp/errorsFilter.txt", std::ios_base::app);
        oTError << runName << "\n\t";	statsFiltering.appendTo(oTError); oTError << "\n\n";
        oTError.close();

        // append error for each run to a file
        std::ofstream oSError("/tmp/errorsSmoothing.txt", std::ios_base::app);
        oSError << runName << "\n\t";	statsSmoothing.appendTo(oSError); oSError << "\n\n";
        oSError.close();

        // append error for each run to a file
        std::ofstream oTDError("/tmp/errorsDistFiltering.txt", std::ios_base::app);
        oTDError << runName << "\n\t";	statsDistFiltering.appendTo(oTDError); oTDError << "\n\n";
        oTDError.close();

        // append error for each run to a file
        std::ofstream oSDError("/tmp/errorsDistSmoothing.txt", std::ios_base::app);
        oSDError << runName << "\n\t";	statsDistSmoothing.appendTo(oSDError); oSDError << "\n\n";
        oSDError.close();


        // plot-data
		std::ofstream oPath("/tmp/path_" + runName + ".dat");		vis.groundTruth.addDataTo(oPath); oPath.close();		// ground truth
		std::ofstream oEstN("/tmp/est_norm_" + runName + ".dat");	vis.estPath.addDataTo(oEstN); oEstN.close();			// estimation via filter itself
		std::ofstream oEstS("/tmp/est_smooth_" + runName + ".dat");	vis.smoothPath.addDataTo(oEstS); oEstS.close();			// estimation via smoothing
		std::ofstream oFloor("/tmp/floors.dat");					vis.floors.addDataTo(oFloor); oFloor.close();

        std::ofstream oPlot("/tmp/plot_" + runName + ".gp");

        oPlot << "set terminal eps size 3.4,2\n";
        oPlot << "set output '" << runName << ".eps'\n";
        oPlot << "set termoption dashlength 0.5\n";

        oPlot << "set ticslevel 0\n";
        oPlot << "set view equal xy\n";
        oPlot << "set zrange [0:2200]\n";
        oPlot << "set multiplot layout 1,1 scale 2.7,2.7 offset 0,0.23\n";
        oPlot << "set view 72,33\n";

        oPlot << "unset border\n";
        oPlot << "unset xtics\n";
        oPlot << "unset ytics\n";
        oPlot << "unset ztics\n";

        oPlot << "splot \\\n";
		oPlot << "'floors.dat'							skip 21 notitle with lines lc rgb '#777777', \\\n";
		oPlot << "'path_" << runName << ".dat'			skip 21 notitle with lines lw 2.5 dashtype 2 lc rgb '#007700', \\\n";
		oPlot << "'est_norm_" << runName << ".dat'		skip 21 notitle with lines lw 2.5 lc rgb '#000099', ";
		oPlot << "'est_smooth_" << runName << ".dat'	skip 21 notitle with lines lw 2.5 lc rgb '#000000' ";

        oPlot.close();

		{
			std::ofstream oErrPlot("/tmp/plot_err_" + runName + ".gp");
			oErrPlot << "plot \\\n";
			oErrPlot << "'err_norm_" << runName << ".dat' with lines, \\\n";
			oErrPlot << "'err_smooth_" << runName << ".dat' with lines";
			oErrPlot.close();
		}

		{
			std::ofstream oErrPlot("/tmp/plot_dist_err_" + runName + ".gp");
			oErrPlot << "plot \\\n";
			oErrPlot << "'err_dist_norm_" << runName << ".dat' with lines, \\\n";
			oErrPlot << "'err_dist_smooth_" << runName << ".dat' with lines";
			oErrPlot.close();
		}



    }

};

#endif // EVALBASE_H