#ifndef MYSTATE_H
#define MYSTATE_H

#include <KLib/math/distribution/Normal.h>
#include <KLib/math/optimization/NumOptVector.h>

#include <Indoor/grid/walk/GridWalkState.h>

#include "../MyGridNode.h"

/**
 * one possible state for the pedestrian
 * 3D position (x, y, floor-nr)
 */
struct MyState {

	// current position
	Point3 pCur;

	// previous position
	Point3 pOld;

	// the grid-walk state
	GridWalkState<MyGridNode> walkState;

	int distanceWalkedCM;


//    double heading_old;

//	//double transHeading;
//	float numZChanges;

//	// cumulative distance (in cm) this particle has taken. to-be-reset by the step detector whenever needed!
//	double distanceWalkedCM;

	double hPa; //relative Pressure given by a history with size defined in BarometerSensorReader.h

//    double vertical_acc; //vertical acceleration

//	/** the pedestrian's current heading */
//	double heading;

	/** empty ctor */
	MyState() : pCur(0,0,0), pOld(0,0,0), walkState(nullptr, Heading(0)) {
		;
	}

//	/** get the 2D distance between this state and the given x,y (in centimter) */
//	double getDistance2D(const double x_cm, const double y_cm) const {
//		const double dx = (x_cm - this->x_cm);
//		const double dy = (y_cm - this->y_cm);
//		return std::sqrt( (dx*dx) + (dy*dy) );
//	}

//	/** get the 3D distance between this state and the given x,y,floor (in centimter) */
//	double getDistance3D(const double x_cm, const double y_cm, const double floor_height_cm) const {
//		const double dx = (x_cm - this->x_cm);
//		const double dy = (y_cm - this->y_cm);
//		const double dz = (z_nr - this->z_nr) * floor_height_cm;
//		return std::sqrt( (dx*dx) + (dy*dy) + (dz*dz) );
//	}

	/** -------- METHODS FOR THE PARTICLE FILTER -------- */

	MyState& operator += (const MyState& o) {
		pCur += o.pCur;
		//hPa += o.hPa;
		//distanceWalked += o.distanceWalked;
		return *this;
	}

	MyState& operator /= (const double d) {
		pCur /= d;
		//hPa /= d;
		//distanceWalked /= d;
		return *this;
	}

	MyState operator * (const double d) const {
		MyState s = MyState(*this);
		s.pCur *= d;
		//s.hPa *= d;
		//distanceWalked *= d;
		return s;
	}


	// use the default one
//	MyState& operator = (const MyState& o) {
//		x_cm = o.x_cm;
//		y_cm = o.y_cm;
//		z_nr = o.z_nr;
//		x_cm_old = o.x_cm_old;
//		y_cm_old = o.y_cm_old;
//		z_nr_old = o.z_nr_old;
//		hPa = o.hPa;
//		heading_old = o.heading_old;
//		heading = o.heading;
//		distanceWalkedCM = o.distanceWalkedCM;
//		return *this;
//	}

//	/** rejection for the regional estimator. reject after 150cm distance */
//	bool belongsToRegion(const MyState& o) const {

////		// do NOT group particles in distinct floors!
////		if (z_nr != o.z_nr) {return false;}

////		// get the 2D distance
////		double d =	(x_cm - o.x_cm)*(x_cm - o.x_cm) +
////					(y_cm - o.y_cm)*(y_cm - o.y_cm);
////		d = std::sqrt(d);

////		// 2D distance below grouping threshold?
////		return d < 350.0;

//		const double dx = (x_cm - o.x_cm);
//		const double dy = (y_cm - o.y_cm);
//		const double dz = (z_nr - o.z_nr) * 3000;

//		// get the 2D distance
//		double d = dx*dx + dy*dy + dz*dz;
//		d = std::sqrt(d);
//		return d < 350.0;

//	}



//	MyState(K::NumOptVector<3>& params) : x_cm(params[0]), y_cm(params[1]), z_cm(params[2]) {;}


};

#endif // MYSTATE_H