#ifndef WALKMODULEFOLLOWDESTINATION_H
#define WALKMODULEFOLLOWDESTINATION_H

#include "WalkModule.h"
#include "../../../../nav/dijkstra/Dijkstra.h"

/**
 * favour edges p(e) that approach the destination
 */
template <typename Node, typename WalkState> class WalkModuleFollowDestination : public WalkModule<Node, WalkState> {

private:

	Dijkstra<Node> dijkstra;

	struct DijkstraMapper {
		const Grid<Node>& grid;
		DijkstraMapper(const Grid<Node>& grid) : grid(grid) {;}
		int getNumNeighbors(const Node& n) const {return n.getNumNeighbors();}
		const Node* getNeighbor(const Node& n, const int idx) const {return &grid.getNeighbor(n, idx);}
		float getWeightBetween(const Node& n1, const Node& n2) const {
			return n1.getDistanceInCM(n2) * n2.navImportance;
		}
	};

public:

	/** ctor */
	WalkModuleFollowDestination(Grid<Node>& grid, const Node& destination) {

		// shortest path calculation
		dijkstra.build(&destination, DijkstraMapper(grid));

	}

	virtual void updateBefore(WalkState& state) override {
		(void) state;
	}

	virtual void step(WalkState& state, const Node& curNode, const Node& nextNode) override {
		(void) state;
		(void) curNode;
		(void) nextNode;
	}

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

		(void) state;
		(void) startNode;

		const float kappa = 0.8;
		const DijkstraNode<Node>* dnCur = dijkstra.getNode(curNode);
		const DijkstraNode<Node>* dnNext = dijkstra.getNode(potentialNode);

		// probability
		return (dnNext->cumWeight < dnCur->cumWeight) ? (kappa) : (1.0 - kappa);

	}

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

};

#endif // WALKMODULEFOLLOWDESTINATION_H