#ifndef WALKMODULEFOLLOWDESTINATION_H
#define WALKMODULEFOLLOWDESTINATION_H

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

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

#include "../../../../math/Distributions.h"
#include "../../../../Assertions.h"


/**
 * favor nodes that approach a known destination
 */
template <typename Node, typename WalkState> class WalkModuleFollowDestination : public WalkModule<Node, WalkState> {

private:

	const Grid<Node>& grid;
	Dijkstra<Node> dijkstra;
	const DijkstraNode<Node>* dnDest;

	struct DijkstraAccess {
		const Grid<Node>& grid;
		DijkstraAccess(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.getDistanceInMeter(n2) / n2.getNavImportance();
		}
	};

public:

	/** ctor WITHOUT known destination*/
	WalkModuleFollowDestination(const Grid<Node>& grid) : grid(grid) {

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

	}

	/** ctor WITH known destination*/
	WalkModuleFollowDestination(const Grid<Node>& grid, const Node& destination) : grid(grid) {
		setDestination(destination);
	}


	/** set the desired destination node */
	void setDestination(const Node& dest) {
		DijkstraAccess acc(grid);
		dijkstra.build(&dest, acc);
		dnDest = dijkstra.getNode(dest);
	}


	/** get the shortest path from the given start to the configured destination */
	DijkstraPath<Node> getShortestPath(const Node& start) {
		const DijkstraNode<Node>* dnStart = dijkstra.getNode(start);
		const DijkstraPath<Node> path(dnStart, dnDest);
		return path;
	}

	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) state;
		(void) startNode;

		const DijkstraNode<Node>* dnCur = dijkstra.getNode(curNode);
		const DijkstraNode<Node>* dnPot = dijkstra.getNode(potentialNode);

		if (dnCur == nullptr) {return 1.0;}
		if (dnPot == nullptr) {return 1.0;}

		constexpr double kappa = 0.70;

		const float curDistToTarget = dnCur->cumWeight;
		const float potDistToTarget = dnPot->cumWeight;

		return (potDistToTarget < curDistToTarget) ? (kappa) : (1.0-kappa);

	}


};


#endif // WALKMODULEFOLLOWDESTINATION_H