/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef WALKMODULEABSOLUTEHEADINGCONTROL_H
#define WALKMODULEABSOLUTEHEADINGCONTROL_H

/**
 * compare the state's absolute heading against a given compass [control]
 */

#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 WalkModuleAbsoluteHeadingControl : public WalkModule<Node, WalkState> {

	const float sigma_rad;
	const Control* ctrl;

public:

	/** ctor. 180 should be OK! */
	WalkModuleAbsoluteHeadingControl(const Control* ctrl, const float sensorNoiseDegreesSigma) :
		sigma_rad(Angle::degToRad(sensorNoiseDegreesSigma)),
		ctrl(ctrl) {

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

	}


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

	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;

		// NOTE: ctrl->turnAngle is cumulative SINCE the last transition!
		// reset this one after every transition!
		Assert::isBetween(ctrl->compassAzimuth_rad, 0.0f, (float)(2*M_PI), "the given absolute heading is out of bounds");

		// ignore for stairs?
		//if (potentialNode.getType() == GridNode::TYPE_STAIR)	{return 1.0;}

		// for elevator edges [same (x,y) but different z] just return 1
		if (potentialNode.getType() == GridNode::TYPE_ELEVATOR)	{return 1.0;}
		if (curNode.getType() == GridNode::TYPE_ELEVATOR)		{return 1.0;}
		//if (curNode.x_cm == potentialNode.x_cm && curNode.y_cm == potentialNode.y_cm && curNode.z_cm != potentialNode.z_cm) {return 1.0;}

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

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

		if (angularDiff > Angle::degToRad(100))	{return 0.10;}
												{return 0.90;}

	}

};

#endif // WALKMODULEABSOLUTEHEADINGCONTROL_H