#ifndef GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H
#define GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H

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

#include "../../math/DrawList.h"
#include <KLib/math/distribution/Normal.h>

#include "../../nav/dijkstra/Dijkstra.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 {

private:

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

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

public:

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

		// build all shortest path to reach th target
		dijkstra.build(target, 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, GridWalkState<T> start, float distance_m) {

		int retries = 2;
		GridWalkState<T> res;

		// try to walk the given distance from the start
		// if this fails (reached a dead end) -> restart (maybe the next try finds a better path)
		do {
			res = walk(grid, start, distance_m);
		} while (res.node == nullptr && --retries);

		// still reaching a dead end?
		// -> try a walk in the opposite direction instead
		if (res.node == nullptr) {
			res = walk(grid, GridWalkState<T>(start.node, start.heading.getInverted()), distance_m);
		}

		// still nothing found? -> keep the start as-is
		return (res.node == nullptr) ? (start) : (res);

	}

private:

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

		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 = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);

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

			drawer.add(neighbor, prob);

		}

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

		// 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 = hDir;
		next.heading += headingChangeDist(gen);

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

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

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

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

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

	}

};


#endif // GRIDWALKLIGHTATTHEENDOFTHETUNNEL_H