#ifndef WALKMODULEHEADING_H
#define WALKMODULEHEADING_H

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

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

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


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

/**
 * base-class e.g. needed for GridWalkHeading and GridWalkHeadingControl to work
 */
struct WalkStateHeading {

	/** used for better naming: heading.error instead of headingError */
	struct _Heading {

		/**
		 * the direction [0:2pi] the walk should move to
		 * e.g. indiciated by:
		 *		compass
		 *		integration over gyroscope values
		 */
		Heading direction;

		/**
		 * (cumulative) error between walked edges and requested direction (above).
		 * is used to ensure that (even though the grid contains only 45° edges) we
		 * approximately walk into the requested direction.
		 */
		float error = 0;

		/** ctor */
		_Heading(const Heading direction, const float error) : direction(direction), error(error) {;}

	} heading;




	/** ctor */
	explicit WalkStateHeading(const Heading& direction, const float error) : heading(direction, error) {;}

};



/** 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) override {

		// 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