/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef GRIDWALKRANDOMHEADINGUPDATEADV_H
#define GRIDWALKRANDOMHEADINGUPDATEADV_H


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

#include "../../math/DrawList.h"
#include "../../math/Distributions.h"
#include "../../nav/dijkstra/Dijkstra.h"
#include "../../math/Random.h"

#include "GridWalk.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
 *  - simple
 *  - fixes the issues of GridWalkRandomHeadingUpdate by incorporating floor information
 *  - adds additional randomness which should be more stable
 *
 */
template <typename T> class GridWalkRandomHeadingUpdateAdv : 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 */
	Random::RandomGenerator gen;

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

public:

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

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

		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, float headChange = -1) {

		drawer.reset();

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

		// change the heading at the beginning of each walk
		if (headChange == -1) {headChange = headingChangeDist(gen);}

		// get a new random heading
		next.heading = cur.heading + headChange;

		// 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 = Distribution::Normal<float>::getProbability(0, Angle::degToRad(40), diffRad);

			// add the node's importance factor into the calculation
			//const float prob2 = Distribution::Logistic<float>::getCDF(neighbor.imp, 1.0, 0.05);
			const float prob2 = std::pow(neighbor.imp, 10);

			// final importance
			const float prob = prob1 * prob2;

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

		}

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

		// pick the neighbor best matching this new heading
		next.node = drawer.get();

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

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

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

	}

};

#endif // GRIDWALKRANDOMHEADINGUPDATEADV_H