Indoor/grid/walk/v2/GridWalker.h

#ifndef GRIDWALKER_H
#define GRIDWALKER_H

#include "../../../data/Timestamp.h"
#include "../../Grid.h"
#include "../../../math/DrawList.h"
#include "modules/WalkModule.h"
#include "../../../math/Distributions.h"
#include "../../../math/Stats.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;

	RandomGenerator rnd;

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, const float _dist_m) {
		double tmp;	// ignored
		return getDestination(grid, _startState, _dist_m, tmp);
	}


	/**
	 * perform the walk based on the configured setup
	 * returns the walks overall probability using the out-parameter
	 */
	WalkState getDestination(Grid<Node>& grid, const WalkState& _startState, const float _dist_m, double& probability) {

		Assert::isTrue(_dist_m >= 0, "walk distance must not be negative!");
		Assert::isTrue(_dist_m < 10.0, "walking more than 10.0 meters at once does not make sense!");

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

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

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

		// perform initial update
		updateBefore(currentState, *curNode);


		// add the previous walked-distance-error to the desired distance (is usually negative, thus dist_m is reduced)
		float dist_m = _dist_m + _startState.distance.error_m;

		Stats::Maximum<double> maxEdgeProb;

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

			// only needed for a global (no-thread-safe) drawer
			//drawer.reset();

			// the rnd-generator is shared among several threads which should be fine
			// the draw-list, however, is per-thread, which is mandatory!
			// alternative to the generator would be seeding the interal one which prooved very unstable!
			DrawList<const Node*> drawer(rnd); drawer.reserve(10);

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

			// pick a neighbor and get its probability
			//double nodeProbability;
			const Node* nextNode = drawer.get();
			//if ( nodeProbability < probability ) {probability = nodeProbability;}	// keep the smallest one
			//probability += nodeProbability;
			//++cnt;

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

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

		}

		//if (cnt != 0) {probability /= cnt;} else {probability = 1.0;}
		probability = 1.0;
		//probability  = (maxEdgeProb.isValid()) ? (maxEdgeProb.get()) : (1.0);		// dist_m might be zero -> no edges -> no maximum

		// add the walk importance to the probabiliy [each node has a to-be-walked-probability depending on its distance to walls, etc...]
		const float walkImportance = curNode->getWalkImportance();
		Assert::isNotNaN(walkImportance, "grid-node's walk-importance is NaN. Did you forget to calculate the importance values after building the grid?");
		Assert::isBetween(walkImportance, 0.0f, 2.5f, "grid-node's walk-importance is out of range. Did you forget to calculate the importance values after building the grid?");
		probability *= walkImportance;// < 0.4f ? (0.1) : (1.0);			// "kill" particles that walk near walls (most probably trapped ones)

		// sanity check
		Assert::isNotNaN(probability, "detected NaN grid-walk probability");

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

		// calculate the walked-distance-error (is usually negative, as we walked a little too far)
		currentState.distance.error_m = dist_m;

		// done
		return currentState;

	}

private:

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

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

	}

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