#ifndef GRIDWALKRANDOMHEADINGUPDATE_H
#define GRIDWALKRANDOMHEADINGUPDATE_H

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

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

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

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

/**
 * for every walked edge: slightly update (scatter) the current heading
 * pick the edge (neighbor) best matching the current heading
 * if this neighbor's heading highly differs from the requested heading: start over
 *
 * PROs
 *	- very simple
 *
 * CONs
 *	- particles are bad at walking out of rooms for small grid sizes as there are too many options
 *		to stay within the room..
 *
 */
template <typename T> class GridWalkRandomHeadingUpdate {

	friend class GridWalkHelper;

private:

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

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

public:

	/** ctor */
	GridWalkRandomHeadingUpdate() {
		;
	}

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

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

	}

private:

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

		GridWalkState<T> next;

		// get a new random heading
		next.heading = cur.heading + headingChangeDist(gen);

		// pick the neighbor best matching this new heading
		next.node = &GridWalkHelper::getBestNeighbor(grid, *cur.node, next.heading);

		// if the best matching neighbor is far of this requested heading
		// (e.g. no good neighbor due to walls) cancel the walk. to force a retry
		const float diff = GridWalkHelper::getHeading(*cur.node, *next.node).getDiffHalfRAD(next.heading);
		if (diff > Angle::degToRad(45)) {
			return GridWalkState<T>();
		}

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