#ifndef WALKMODULEHEADINGCONTROL_H
#define WALKMODULEHEADINGCONTROL_H

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

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


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

private:

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

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

	Control* ctrl;

	//std::unordered_map<WalkState*, float> errorTracker;

public:

	/** ctor 3.0 should be OK! */
	WalkModuleHeadingControl(Control* ctrl) : dist(Distribution::VonMises<double>(0.0f, 1.5).getLUT()), draw(dist.getDrawList()), ctrl(ctrl) {
		;
	}


	virtual void updateBefore(WalkState& state) override {

		const float var = draw.get() * 0.15;//0.05;
		//const float var = 0;
		state.startHeading += ctrl->turnAngle + var;

	}

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

	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
		(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 - the last error
		const Heading stateHead = state.startHeading;

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

		// determine probability
		const float prob = dist.getProbability(angularDiff);
		return prob;

	}



};

#endif // WALKMODULEHEADINGCONTROL_H