#ifndef MYTRANSITION_H
#define MYTRANSITION_H

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


#include <Indoor/grid/Grid.h>
#include <Indoor/grid/walk/GridWalk.h>

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

#include "../toni/barometric.h"

#include "../MyGridNode.h"

inline double sgn(double x){
    return ((x>0)?1 : ((x<0)?-1 : 1));
}

class MyTransition : public K::ParticleFilterTransition<MyState, MyControl> {

private:

	Grid<MyGridNode>& grid;
	GridWalk<MyGridNode>& walker;


	/** a simple normal distribution */
	K::UniformDistribution distWalkStop;
	K::NormalDistribution distWalkPerSec;
	K::NormalDistribution distStop;


    /** normal distribution for barometer */
	K::NormalDistribution distBaro;


public:

	/**
	 * ctor
	 * @param choice the choice to use for randomly drawing nodes
	 * @param fp the underlying floorplan
	 */
	MyTransition(Grid<MyGridNode>& grid, GridWalk<MyGridNode>& walker) :
		grid(grid), walker(walker),
		distWalkStop(0.0, 1.0), distWalkPerSec(1.0, 0.3), distStop(0.0, 0.1), distBaro(0.3, 0.05) {

		distWalkStop.setSeed(1234);
		distWalkPerSec.setSeed(1234);
		distStop.setSeed(1234);

        distBaro.setSeed(5678);

	}

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 = ts - this->ts;
			this->ts = ts;
		}
	}

	virtual void transition(std::vector<K::Particle<MyState>>& particles, const MyControl* control) override {

		for (K::Particle<MyState>& p : particles) {

			// TODO: depending on the time since the last update
			// random distance to move
//			const double distance = (distWalkStop.draw() > 0.2) ? (distWalk.draw()) : (distStop.draw());
//			double dist_m = distance * deltaMS / 1000.0;

//			if (dist_m < 0) {dist_m = -dist_m; p.state.heading = rand() % 360;}

			// update the old heading and the other old values
			//p.state.walkState.heading = p.state.heading;
            if(!(p.state.pOld == p.state.pCur)){
				p.state.cumulativeHeading = Angle::getDEG_360(p.state.pOld.x, p.state.pOld.y, p.state.pCur.x, p.state.pCur.y);
            }
			p.state.pOld = p.state.pCur;


//			// 10% stand still, 90% walk
//			double dist_m;
//			if (distWalkStop.draw() > 0.9) {
//				dist_m = std::abs(distStop.draw() * deltaMS / 1000.0);
//			} else {
//				dist_m = std::abs(distWalkPerSec.draw() * deltaMS / 1000.0);
//			}

			// get new destination
			//const Node3* dst = choice->getTarget(src, p.state, dist_m);
			p.state.walkState = walker.getDestination(grid, p.state.walkState, control->walked_m, control->headingChange_rad, control->currentActivitiy );

			// randomly move the particle within its target grid (box)
			// (z remains unchanged!)
			//const int grid_size_cm = grid.getGridSize_cm();

			// new position (x,y) is randomly distributed within the target node
			Point3 noise = Point3(0,0,0);		// TODO
			p.state.pCur = (Point3) *p.state.walkState.node + noise;


			// update the baromter
			p.state.hPa += (p.state.pOld.z - p.state.pCur.z) / 100.0f * 0.105f;


		}

	}


};

#endif // MYTRANSITION_H