Indoor/grid/walk/GridWalkPathControl.h

#ifndef GRIDWALKPATHCONTROL_H
#define GRIDWALKPATHCONTROL_H

#include "../../geo/Heading.h"
#include "../Grid.h"

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

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

#include "GridWalkState.h"
#include "GridWalkHelper.h"
#include "GridWalk.h"

template <typename T> class GridWalkPathControl : public GridWalk<T> {

	friend class GridWalkHelper;

private:

	/** per-edge: change heading with this sigma */
	static constexpr float HEADING_CHANGE_SIGMA = Angle::degToRad(10);

	/** fast random-number-generator */
	std::minstd_rand gen;

	/** 0-mean normal distribution */
	std::normal_distribution<float> headingChangeDist = std::normal_distribution<float>(0.0, HEADING_CHANGE_SIGMA);

	Dijkstra<T> dijkstra;

	DrawList<T&> drawer;

public:

	float pOther = 0.10;

public:


	/** ctor with the target you want to reach */
	template <typename Access> GridWalkPathControl(Grid<T>& grid, const Access& acc, const T& target) {

		gen.seed(1234);

		// build all shortest path to reach th target
		dijkstra.build(&target, acc);

		// attach a corresponding weight-information to each user-grid-node
		for (T& node : grid) {

			const DijkstraNode<T>* dn = dijkstra.getNode(node);

			// should never be null as all nodes were evaluated
			if (dn != nullptr) {
				node.distToTarget = dn->cumWeight/2000;
			}

		}

	}

	GridWalkState<T> getDestination(Grid<T>& grid, const GridWalkState<T>& start, float distance_m, float headChange_rad, Activity act) {

		// proportional change of the heading
        static 	Distribution::Normal<float> dHead(1, 0.01);

		// proportional change of the to-be-walked distance
		static 	Distribution::Normal<float> dWalk(1, 0.10);

		distance_m = distance_m*dWalk.draw()*1.4;		// TODO: why *2?
		headChange_rad = headChange_rad*dHead.draw();

		static 	Distribution::Normal<float> sWalk(0, 0.15);
		if (distance_m == 0) { distance_m = std::abs( sWalk.draw() ); }

		return walk(grid, start, distance_m, headChange_rad, act);

	}

private:

	double getProbability(const T& start, const T& prev, const T& possible, const Heading head, Activity act) const {

		// TODO: WHY?! not only when going back to the start?
		if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {
			if (start.z_cm == possible.z_cm) {return 0;} // back to the start
			throw 1;
			return 0.5;// stair start/end TODO: fix
		}

		// get the angle between START and the possible next node
		const Heading possibleHead = GridWalkHelper::getHeading(start, possible);

		// calculate the difference
		const float diff = possibleHead.getDiffHalfRAD(head);

//		// compare this heading with the requested one
        const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(25), diff);
//		const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1;		// favor best 3 angles equally

		// nodes own importance
		//const double nodeProb = (possible.distToTarget < start.distToTarget) ? 1 : 0.025;		// from start
		const double nodeProb = (possible.distToTarget < prev.distToTarget) ? 1 : pOther;		// from previous node

		double actProb = 1.0;
		if (act == Activity::STAIRS_UP)		{actProb = (prev.z_cm < possible.z_cm) ? (0.8) : (0.2);}
		if (act == Activity::STAIRS_DOWN)	{actProb = (prev.z_cm > possible.z_cm) ? (0.8) : (0.2);}
		if (act == Activity::WALKING) {actProb = (prev.z_cm == possible.z_cm) ? (0.8) : (0.2);}

		// bring it together
		return angleProb * nodeProb * actProb;

	}

	GridWalkState<T> walk(Grid<T>& grid, const GridWalkState<T>& start, const float distance_m, const float headChange_rad, Activity act) {

		// try-again distribution
		//static 	Distribution::Normal<float> dHead(0, Angle::degToRad(10));
		//static 	Distribution::Normal<float> dUpdate(0, Angle::degToRad(3));
			static	Distribution::Uniform<float> dChange(Angle::degToRad(0), +Angle::degToRad(359));

		int retries = 5;
		float walked_m = 0;
		GridWalkState<T> cur = start;

		// the desired heading
		Heading reqHeading = start.heading + (headChange_rad);

		// walk until done
		while(walked_m < distance_m) {

			// evaluate all neighbors
			drawer.reset();
			for (T& neighbor : grid.neighbors(*cur.node)) {

				const double prob = getProbability(*start.node, *cur.node, neighbor, reqHeading, act);
				drawer.add(neighbor, prob);

			}

			// too bad? start over!
			if (drawer.getCumProbability() < 0.1 && (--retries) >= 0) {
				walked_m = 0;
				cur = start;
				//WHAT THE HELL
				if (retries == 0) { reqHeading = dChange.draw(); }
				continue;
			}

			// get the neighbor
			const T& neighbor = drawer.get();

			// update
			walked_m += neighbor.getDistanceInMeter(*cur.node);
			cur.node = &neighbor;

		}

		cur.distanceWalked_m = NAN;
		cur.headingChange_rad = NAN;
		cur.heading = reqHeading;

		return cur;

	}

};



#endif // GRIDWALKPATHCONTROL_H