Indoor/grid/walk/GridWalkLightAtTheEndOfTheTunnel.h

#ifndef GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H
#define GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H

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

#include "../../math/DrawList.h"
#include "../../math/distribution/Normal.h"
#include "../../math/DrawList.h"

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

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

/**
 * perform walks on the grid based on some sort of weighting
 * and drawing from the weighted elements
 */
template <typename T> class GridWalkLightAtTheEndOfTheTunnel : public GridWalk<T> {

	friend class GridWalkHelper;

private:

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

	/** per-edge: allowed heading difference */
	static constexpr float HEADING_DIFF_SIGMA = Angle::degToRad(30);


	/** allows drawing elements according to their probability */
	DrawList<T&> drawer;

	/** fast random-number-generator */
	Random::RandomGenerator gen;

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

	Dijkstra<T> dijkstra;

public:

	/** ctor with the target you want to reach */
	template <typename Access> GridWalkLightAtTheEndOfTheTunnel(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, const float distance_m, const float headChange_rad, Activity act) override {

		return GridWalkHelper::retryOrInvert(*this, 2, grid, start, distance_m, headChange_rad);

	}

private:

	GridWalkState<T> walk(Grid<T>& grid, GridWalkState<T>& cur, float distRest_m, const float headChange_rad) {

		drawer.reset();

		// calculate the weight for all possible destinations from "cur"
		for (T& neighbor : grid.neighbors(*cur.node)) {

			// heading when walking from cur to neighbor
			const Heading potentialHeading = GridWalkHelper::getHeading(*cur.node, neighbor);

			// angular difference
			const float diff = cur.heading.getDiffHalfRAD(potentialHeading);

			// probability for this direction change?
			double prob = Distribution::Normal<float>::getProbability(0, HEADING_DIFF_SIGMA, diff);

			// perfer locations reaching the target
			const double shortening = cur.node->distToTarget - neighbor.distToTarget;
			if (shortening >= 0) {prob *= 5;} // << importance factor!!

//			prob = 0.1;
//			if		(diff < Angle::degToRad(40)) {prob += 0.2;}
//			else if	(diff < Angle::degToRad(20)) {prob += 0.5;}

//			if (shortening >= 0) {prob += 0.5;}

			//prob *= std::pow(neighbor.imp, 5);

			//prob = (shortening >= 0) ? (2) : (0.75);

			drawer.add(neighbor, prob);

		}

		//GridWalkState<T> next(nullptr, cur.heading);

		// create a copy (to keep heading and walked distance)
		GridWalkState<T> next = cur;

		// pick a random destination
		T& nDir = drawer.get();
		const Heading hDir = GridWalkHelper::getHeading(*cur.node, nDir);
		//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.5;
		//next.heading = Heading( cur.heading.getRAD() * 0.2 + hDir.getRAD() * 0.8 );
		next.heading = hDir;
		next.heading += headingChangeDist(gen);
		next.node = &nDir;


////		// compare two neighbors according to their implied heading change
////		auto compp = [&] (const T& n1, const T& n2) {
////			Heading h1 = GridWalkHelper::getHeading(*cur.node, n1);
////			Heading h2 = GridWalkHelper::getHeading(*cur.node, n2);
////			const float d1 = next.heading.getDiffHalfRAD(h1);
////			const float d2 = next.heading.getDiffHalfRAD(h2);
////			// same heading -> prefer nodes nearer to the target. needed for stairs!!!
////			// BAD: leads to straight lines in some palces. see solution B (below)
////			//return (d1 < d2) && (n1.distToTarget < n2.distToTarget);

////			// VERY IMPORTANT!
////			// pick the node with the smallest heading change.
////			// if the heading change is the same for two nodes, pick a random one!
////			return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2);
////		};

////		// pick the neighbor best matching the new heading
////		auto it = grid.neighbors(*cur.node);
////		T& nn = *std::min_element(it.begin(), it.end(), compp);
////		next.node = &nn;

//		next.node = &GridWalkHelper::getBestNeighbor(grid, *cur.node, next.heading);

////		// pervent dramatic heading changes. instead: try again
////		if (cur.heading.getDiffHalfRAD(GridWalkHelper::getHeading(*cur.node, nn)) > Angle::degToRad(60)) {
////			return GridWalkState<T>(nullptr, 0);
////		}

		// get the distance up to this neighbor
		const float walked_m = next.node->getDistanceInMeter(*cur.node);
		distRest_m -= walked_m;

		// update the heading-change and walked-distance
		next.headingChange_rad += next.heading.getRAD() - cur.heading.getRAD();
		next.distanceWalked_m += walked_m;
		++((T*)next.node)->cnt;		// TODO: eval only

//		if (next.node->z_cm != cur.node->z_cm) {
//			int i = 0;
//		}



		// done?
		if (distRest_m <= 0) {return next;}

		// another round..
		return walk(grid, next, distRest_m, headChange_rad);

	}

};


#endif // GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H