#ifndef FSTRUCTS_H
#define FSTRUCTS_H

#include <Indoor/grid/Grid.h>
#include <Indoor/sensors/radio/WiFiGridNode.h>
#include <Indoor/math/Distributions.h>
#include <Indoor/sensors/radio/WiFiMeasurements.h>
#include <Indoor/sensors/beacon/BeaconMeasurements.h>
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>
#include <Indoor/grid/factory/v2/GridNodeImportance.h>

#include <Indoor/grid/walk/v2/GridWalker.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleHeading.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleSpread.h>
#include <Indoor/grid/walk/v2/modules/WalkModuleFavorZ.h>
#include <Indoor/grid/walk/v2/modules/WalkModulePreventVisited.h>

struct MyState : public WalkState, public WalkStateHeading, public WalkStateSpread, public WalkStateFavorZ {

	static Floorplan::IndoorMap* map;

    float relativePressure = 0;
    GridPoint positionOld;

    MyState() : WalkState(GridPoint(0,0,0)), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}

    MyState(GridPoint pos) : WalkState(pos), WalkStateHeading(Heading(0), 0), positionOld(0,0,0), relativePressure(0) {;}

	MyState& operator += (const MyState& o) {
		this->position += o.position;
		return *this;
	}
	MyState& operator /= (const double d) {
		this->position /= d;
		return *this;
	}
	MyState operator * (const double d) const {
		return MyState(this->position*d);
	}
	bool belongsToRegion(const MyState& o) const {
		return position.inMeter().getDistance(o.position.inMeter()) < 3.0;
	}

};

struct MyControl {

    /** turn angle (in radians) since the last transition */
    float turnSinceLastTransition_rad = 0;

    /** number of steps since the last transition */
    int numStepsSinceLastTransition = 0;

    /** reset the control-data after each transition */
    void resetAfterTransition() {
        turnSinceLastTransition_rad = 0;
        numStepsSinceLastTransition = 0;
    }

};

struct MyObs {

    /** relative pressure since t_0 */
    float relativePressure = 0;

    /** current estimated sigma for pressure sensor */
    float sigmaPressure = 0.10f;

    /** wifi measurements */
    WiFiMeasurements wifi;

    /** beacon measurements */
    BeaconMeasurements beacons;

    /** gps measurements */
    //GPSData gps;

    /** time of evaluation */
    Timestamp currentTime;

};

struct MyNode : public GridPoint, public GridNode, public GridNodeImportance, public WiFiGridNode<20> {

    float navImportance;
    float getNavImportance() const { return navImportance; }

    float walkImportance;
    float getWalkImportance() const { return walkImportance; }


    /** empty ctor */
    MyNode() : GridPoint(-1, -1, -1) {;}

    /** ctor */
    MyNode(const int x, const int y, const int z) : GridPoint(x,y,z) {;}

    static void staticDeserialize(std::istream& inp) {
        WiFiGridNode::staticDeserialize(inp);
    }

    static void staticSerialize(std::ostream& out) {
        WiFiGridNode::staticSerialize(out);
    }
};


#endif // FSTRUCTS_H