Indoor/grid/walk/v2/GridWalkerMulti.h

#ifndef GRIDWALKERMULTI_H
#define GRIDWALKERMULTI_H

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

// FOR TESTING
#include "../../../geo/Heading.h"

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

private:

	struct Path {
		std::vector<const Node*> nodes;
		const Node* getSrc() const {return nodes.front();}
		const Node* getDst() const {return nodes.back();}
	};


private:

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

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) {

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

		const uint64_t seed = (uint64_t) startNode + rand();
		DrawList<Path> drawPath;
		drawPath.setSeed(seed);

		for (int i = 0; i < 1; ++i) {

			float dist = dist_m;

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

			Path path;
			path.nodes.push_back(curNode);

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

				DrawList<const Node*> drawEdge;
				drawEdge.setSeed(seed+1);

				//if (curNode->getNumNeighbors() < 6) {break;}

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

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

				dist -= nextNode->getDistanceInMeter(*curNode);

				curNode = nextNode;

				path.nodes.push_back(curNode);

			}

			const double prob = getProb(path);
			drawPath.add(path, prob);

		}

		const Path& bestPath = drawPath.get();

		WalkState nextState = _startState;
		nextState.startPos = *(bestPath.getDst());
		return nextState;


	}

private:

	inline double getProb(const Node& n0, const Node& n1, const Node& n2) {
		static float angle = 0; angle += 0.05;
		angle = std::fmod(angle, 6);
		if (&n1 == &n2) {return 1;}
		const Heading head1(angle);
		const Heading head2(n1.x_cm, n1.y_cm, n2.x_cm, n2.y_cm);
		const float diff = head1.getDiffHalfRAD(head2);
		return rand() % 32;
	}

	inline double getProb(const Path& path) {
		const Point3 p1 = path.getSrc()->inMeter();
		const Point3 p2 = path.getDst()->inMeter();
		const float dist = p1.getDistance(p2);
		return 1/dist;
	}

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

		for (const WalkModule<Node, WalkState>* mdl : modules) {
			prob *= mdl->getProbability(state, start, cur, next);
		}

		return prob;

	}

};

#endif // GRIDWALKERMULTI_H