#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:

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

	/** random noise */
	Distribution::Normal<float> distNoise;

	Control* ctrl;

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

public:

	/** ctor 3.0 should be OK! */
	WalkModuleHeadingControl(Control* ctrl, const float sensorNoiseDegreesSigma) :
		dist(Distribution::VonMises<double>(0.0f, 2.0).getLUT()),
		distNoise(0, Angle::degToRad(sensorNoiseDegreesSigma)),
		ctrl(ctrl) {
		;
	}


	virtual void updateBefore(WalkState& state) override {

		// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
		// reset this one after every transition!
		Assert::isBetween(ctrl->turnAngle, -3.0f, +3.0f, "the given turn angle is too high to make sense.. did you forget to set ctrl->turnAngle = 0 after each transition?");

		// sensor noise
		const float var = distNoise.draw();

		// adjust the state's heading using the control-data
		state.heading.direction += 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;

		// get the heading denoted by the way from curNode to nextNode
		const Heading head(curNode.x_cm, curNode.y_cm, nextNode.x_cm, nextNode.y_cm);

		// get the heading requested by the state
		const Heading stateHead = state.heading.direction;

		// get the error (signed difference) between both
		const float angularDiff = stateHead.getSignedDiff(head);

		// adjust the error.
		// note:	the error may get > +/- 2PI but this is not an issue!
		//			when the error is added to the current heading within getProbability(),
		//			it is ensured their sum is within [0:2pi]
		state.heading.error += angularDiff;


	}

	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.heading.direction + state.heading.error;

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

		if (angularDiff > Angle::degToRad(135))	{return 0.01;}
		if (angularDiff > Angle::degToRad(90))	{return 0.02;}
		if (angularDiff > Angle::degToRad(45))	{return 0.07;}
												{return 0.90;}

//		add error to allow stronger deviation with respect to the "BIG GLOBAL SCOPE"

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

	}



};

#endif // WALKMODULEHEADINGCONTROL_H