#ifndef MYSMOOTHINGTRANSITIONEXPERIMENTAL_H
#define MYSMOOTHINGTRANSITIONEXPERIMENTAL_H

#include <KLib/math/filter/particles/ParticleFilterTransition.h>
#include <KLib/math/filter/smoothing/BackwardFilterTransition.h>
#include <KLib/math/distribution/Normal.h>
#include <KLib/math/distribution/Uniform.h>

#include <Indoor/geo/Angle.h>

#include "../MyState.h"
#include "../MyControl.h"
#include "../../Helper.h"
#include "../../toni/barometric.h"

class MySmoothingTransitionExperimental : public K::BackwardFilterTransition<MyState> {

private:

	/** a simple normal distribution */
	K::NormalDistribution distWalk;


public:

	/**
	 * ctor
	 * @param choice the choice to use for randomly drawing nodes
	 * @param fp the underlying floorplan
	 */
    MySmoothingTransitionExperimental() :
        distWalk(smoothing_walk_mu, smoothing_walk_sigma)
    {
        distWalk.setSeed(4321);
	}

public:

	uint64_t ts = 0;
	uint64_t deltaMS = 0;

	/** set the current time in millisconds */
	void setCurrentTime(const uint64_t ts) {
		if (this->ts == 0) {
			this->ts = ts;
			deltaMS = 0;
		} else {
            deltaMS = this->ts - ts;
			this->ts = ts;
		}
	}

    /**
     * smoothing transition starting at T with t, t-1,...0
     * @param particles_new p_t (Forward Filter) p2
     * @param particles_old p_t+1 (Smoothed Particles from Step before) p1
     * q(p1 | p2) is calculated
     */
    std::vector<std::vector<double>> transition(std::vector<K::Particle<MyState>>const& particles_new,
                                                std::vector<K::Particle<MyState>>const& particles_old ) override {


        // calculate alpha(m,n) = p(q_t+1(m) | q_t(n))
        // this means, predict all possible states q_t+1 with all passible states q_t
        // e.g. p(q_490(1)|q_489(1));p(q_490(1)|q_489(2)) ... p(q_490(1)|q_489(N)) and
        // p(q_490(1)|q_489(1)); p(q_490(2)|q_489(1)) ... p(q_490(M)|q_489(1))
        std::vector<std::vector<double>> predictionProbabilities;

        omp_set_dynamic(0);     // Explicitly disable dynamic teams
        omp_set_num_threads(7);
        #pragma omp parallel for shared(predictionProbabilities)
        for (int i = 0; i < particles_old.size(); ++i) {
            std::vector<double> innerVector;
            auto p1 = &particles_old[i];

            for(int j = 0; j < particles_new.size(); ++j){

                auto p2 = &particles_new[j];

                const double distance_m = p2->state.pCur.getDistance(p1->state.pCur) / 100.0;

                double muDistance = 0.8;
                double sigmaDistance = 0.5;
                double muFloor = 0.0;
                double sigmaFloor = 0.1;

                switch (p2->state.currentActivity) {
                    case Activity::ELEVATOR:
                        muDistance = 0.0;
                        sigmaDistance = 0.3;

                        muFloor = 0.6;
                        sigmaFloor = 0.25;
                        break;

                    case Activity::STAIRS_DOWN:
                        muDistance = 0.5;
                        sigmaDistance = 0.3;

                        muFloor = 0.3;
                        break;

                    case Activity::STAIRS_UP:
                        muDistance = 0.4;
                        sigmaDistance = 0.2;

                        muFloor = -0.3;
                        break;

                    case Activity::STANDING:
                        muDistance = 0.0;
                        sigmaDistance = 0.2;

                        muFloor = 0.0;
                        break;

                    case Activity::WALKING:
                        muDistance = 0.8;
                        sigmaDistance = 0.5;

                        muFloor = 0.0;
                        break;

                    default:
                        muDistance = 0.8;
                        sigmaDistance = 0.5;

                        muFloor = 0.0;
                        break;
                }

                const double distProb = K::NormalDistribution::getProbability(muDistance, sigmaDistance, distance_m);

                // is the heading change similiar to the measurement?
                double diffRad = Angle::getDiffRAD_2PI_PI(p2->state.walkState.heading.getRAD(), p1->state.walkState.heading.getRAD());
                double diffDeg = Angle::radToDeg(diffRad);
                double angularChangeZeroToPi = std::fmod(std::abs(p1->state.angularHeadingChange), 360.0);

                const double headingProb = K::NormalDistribution::getProbability(angularChangeZeroToPi, smoothing_heading_sigma, diffDeg);


                //check how near we are to the measurement
                double diffZ = (p2->state.pCur.z - p1->state.pCur.z) / 100.0;
                const double floorProb = K::NormalDistribution::getProbability(muFloor, sigmaFloor, diffZ);


                //combine the probabilities
                double prob = distProb * headingProb * floorProb;
                innerVector.push_back(prob);

                //error checks
//                if(distance_m != distance_m) {throw "detected NaN";}
//                if(distProb != distProb) {throw "detected NaN";}
//                if(headingProb != headingProb) {throw "detected NaN";}
//                if(floorProb != floorProb) {throw "detected NaN";}
//                if(floorProb == 0) {throw "detected zero prob in floorprob";}
//                if(prob != prob) {throw "detected NaN";}
                //if(prob == 0) {++zeroCounter;}


            }
            #pragma omp critical
            predictionProbabilities.push_back(innerVector);
        }

        return predictionProbabilities;

    }


};

#endif // MYTRANSITION_H