#ifndef WALKMODULEHEADING_H
#define WALKMODULEHEADING_H

#include "WalkModule.h"
#include "WalkStateHeading.h"

#include "../../../../Assertions.h"

#include "../../../../geo/Heading.h"
#include "../../../../math/distribution/Normal.h"
#include "../../../../math/distribution/LUT.h"
#include "../../../../math/distribution/VonMises.h"


#include "../../../../geo/Heading.h"




/** keep the state's heading */
template <typename Node, typename WalkState> class WalkModuleHeading : public WalkModule<Node, WalkState> {

private:

	/** van-Mises distribution */
	Distribution::LUT<double> dist;

	/** van-Mises draw list */
	DrawList<double> draw;

public:

	/** ctor */
	WalkModuleHeading() : dist(Distribution::VonMises<double>(0.0f, 1.0f).getLUT()), draw(dist.getDrawList()) {

		// ensure the template WalkState inherits from 'WalkStateHeading'!
		StaticAssert::AinheritsB<WalkState, WalkStateHeading>();

	}

	virtual void updateBefore(WalkState& state, const Node& startNode) override {

		(void) startNode;

		// add noise
		state.heading.direction += draw.get();

	}

	virtual void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) override {

		(void) state;
		(void) startNode;
		(void) endNode;
//		if (startNode.x_cm != endNode.x_cm || startNode.y_cm != endNode.y_cm) {
//			Heading head(startNode.x_cm, startNode.y_cm, endNode.x_cm, endNode.y_cm);
//			state.startHeading = head;
//		}

	}

	/** one step (edge) is taken */
	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {

		// TODO
		(void) state;
		(void) curNode;
		(void) nextNode;

	}

	double getProbability(const WalkState& state, const Node& startNode, const Node& curNode, const Node& potentialNode) const override {

		(void) startNode;

		// get the heading between curNode and potentialNode
		const Heading head(curNode.x_cm, curNode.y_cm, potentialNode.x_cm, potentialNode.y_cm);

		// compare the heading against the state's heading
		const Heading stateHead = state.heading.direction;

		// get the difference
		const float angularDiff = head.getDiffHalfRAD(stateHead);//head.getRAD() - stateHead.getRAD();

		// determine probability
		return dist.getProbability(angularDiff);

	}


};

#endif // WALKMODULEHEADING_H