#ifndef MYTRANSITIONSIMPLE_H
#define MYTRANSITIONSIMPLE_H

#include <KLib/math/filter/particles/ParticleFilterTransition.h>
#include <KLib/math/distribution/Normal.h>
#include "MyState.h"
#include "MyControl.h"

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

private:

	/** a simple normal distribution */
	K::NormalDistribution distX;
	K::NormalDistribution distY;
	K::NormalDistribution distZ;
	K::NormalDistribution distBaro;

public:

	/** ctor */
	MyTransitionSimple() : distX(0, 1.0), distY(0, 1.0), distZ(0, 1.0), distBaro(0.3, 0.05) {
		distX.setSeed(1234);
		distY.setSeed(1235);
		distZ.setSeed(1236);
		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) {

            p.state.heading_old = p.state.heading;
            p.state.x_cm_old = p.state.x_cm;
            p.state.y_cm_old = p.state.y_cm;
            p.state.z_nr_old = p.state.z_nr;

			p.state.x_cm += (distX.draw() * deltaMS / 1000.0) * 250.0;
			p.state.y_cm += (distY.draw() * deltaMS / 1000.0) * 250.0;
			p.state.z_nr += (distZ.draw() * deltaMS / 1000.0) * 0.25;
			p.state.heading = Helper::angleBetween(p.state.x_cm_old, p.state.y_cm_old,  p.state.x_cm, p.state.y_cm);

//            if (p.state.z_nr < 0.5) {p.state.z_nr = 0.5;}
//            if (p.state.z_nr > 3.5) {p.state.z_nr = 3.5;}
//            if (p.state.x_cm < 0) {p.state.x_cm = 0;}
//            if (p.state.y_cm < 0) {p.state.y_cm = 0;}



            //update barometer
			p.state.hPa += (p.state.z_nr_old - p.state.z_nr) * distBaro.draw();

			// update walked distance (2D)
			const double dx = p.state.x_cm_old - p.state.x_cm;
			const double dy = p.state.y_cm_old - p.state.y_cm;
			p.state.distanceWalkedCM = std::sqrt((dx*dx) + (dy*dy));


		}

	}


};


#endif // MYTRANSITIONSIMPLE_H