#ifndef GRIDWALKPUSHFORWARD_H
#define GRIDWALKPUSHFORWARD_H

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

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

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

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

/**
 * keeps something like an "average position within the last X steps"
 * and tries to move away from this point as fast as possible
 *
 */
template <typename T> class GridWalkPushForward {

	friend class GridWalkHelper;

private:

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

	static constexpr float HEADING_ALLOWED_SIGMA = Angle::degToRad(20);

	/** 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 */
	GridWalkPushForward() {
		;
	}

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

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

	}

private:

	// NOTE: allocate >>ONCE<<! otherwise random numbers will NOT work!
	DrawList<T*> drawer;

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

		drawer.reset();

		// weight all neighbors based on this heading
		for (T& neighbor : grid.neighbors(*cur.node)) {

			// get the heading between the current node and its neighbor
			const Heading potentialHeading = GridWalkHelper::getHeading(*cur.node, neighbor);

			// calculate the difference from the requested heading
			const float diffRad = potentialHeading.getDiffHalfRAD(cur.heading);

			// weight this change
			const float prob1 = K::NormalDistribution::getProbability(0, HEADING_ALLOWED_SIGMA, diffRad);


			// distance from average? and previous distance from average
			const float distToAvg = Point3(neighbor.x_cm, neighbor.y_cm, neighbor.z_cm).getDistance(cur.avg);
			const float prevDistToAvg = Point3(cur.node->x_cm, cur.node->y_cm, cur.node->z_cm).getDistance(cur.avg);
			const float increase = distToAvg - prevDistToAvg;

			// the distance from the average MUST increase
			const float prob2 = (increase > 0) ? (1) : (0.1);

			// add floorplan importance information
			const float prob3 = std::pow(neighbor.imp, 1);

			const float prob = prob1*prob2*prob3;

			// add for drawing
			drawer.add(&neighbor, prob);

		}


		// all neighbors are unlikely? -> start over
		if (drawer.getCumProbability() < 0.01) {return GridWalkState<T>();}

		GridWalkState<T> next;

		// pick the neighbor best matching this new heading
		next.node = drawer.get();
		next.heading = GridWalkHelper::getHeading(*cur.node, *next.node) + headingChangeDist(gen);

		// weighted average.. moves over time
		next.avg = (cur.avg * 0.9) + (Point3(next.node->x_cm, next.node->y_cm, next.node->z_cm) * 0.1);

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