#ifndef GRIDWALKER_H
#define GRIDWALKER_H

#include "../../Grid.h"
#include "../../../math/DrawList.h"
#include "modules/WalkModule.h"

/**
 * modular grid-walker that takes various sub-components to determine
 * p(e) and thus randomly pick edges
 */
template <typename Node, typename WalkState> class GridWalker {

private:

	/** all modules to evaluate */
	std::vector<WalkModule<Node, WalkState>*> modules;

	DrawList<const Node*> drawer;

public:

	/** add the given module */
	void addModule(WalkModule<Node, WalkState>* module) {
		modules.push_back(module);
	}

	/** perform the walk based on the configured setup */
	WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, float dist_m) {

		// keep the starting state for reference
		//const WalkState startState = _startState;

		// the current state that is modified for each step
		WalkState currentState = _startState;
		updateBefore(currentState);

		// get the node that corresponds to start;
		const Node* startNode = grid.getNodePtrFor(currentState.position);
		Assert::isNotNull(startNode, "failed to termine start-node for grid-walk");

		// currently examined node
		const Node* curNode = startNode;

		// until distance is reached
		while (dist_m > 0) {

			drawer.reset();

			// evaluate each neighbor
			for (const Node& neighbor : grid.neighbors(*curNode)) {
				const double prob = getProbability(currentState, *startNode, *curNode, neighbor);
				drawer.add(&neighbor, prob);
			}

			// pick a neighbor
			const Node* nextNode = drawer.get();

			// inform
			step(currentState, *curNode, *nextNode);

			// update distance-to-walk and current position
			dist_m -= nextNode->getDistanceInMeter(*curNode);
			curNode = nextNode;
			currentState.position = *curNode;


		}

		// update after
		updateAfter(currentState, *startNode, *curNode);

		// done
		return currentState;

	}

private:

	/** update the state before starting the random walk (e.g. based on sensor readings, ..) */
	inline void updateBefore(WalkState& state) {

		for (WalkModule<Node, WalkState>* mdl : modules) {
			mdl->updateBefore(state);
		}

	}

	/** update the WalkState after the random walk (if needed) */
	inline void updateAfter(WalkState& state, const Node& startNode, const Node& endNode) {

		for (WalkModule<Node, WalkState>* mdl : modules) {
			mdl->updateAfter(state, startNode, endNode);
		}

	}

	/** one step taken. inform modules */
	inline void step(WalkState& state, const Node& curNode, const Node& nextNode) {

		for (WalkModule<Node, WalkState>* mdl : modules) {
			mdl->step(state, curNode, nextNode);
		}

	}

	/** get the probability for the given random walk (one edge) */
	inline double getProbability(const WalkState& state, const Node& start, const Node& cur, const Node& next) const {

		double prob = 1.0;
		//double prob = 0;

		for (const WalkModule<Node, WalkState>* mdl : modules) {
			const double subProb = mdl->getProbability(state, start, cur, next);
			Assert::isTrue(subProb >= 0, "probability must not be negative!");
			prob *= subProb;
			//prob += std::log( mdl->getProbability(state, start, cur, next) );
		}

		return prob;
		//return std::exp(prob);

	}

};

#endif // GRIDWALKER_H