/*
 * © 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 GRIDWALKWEIGHTED2_H
#define GRIDWALKWEIGHTED2_H

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

#include "../../math/DrawList.h"
#include "../../math/Random.h"

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

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

public:

	struct State {
		const T* node;
		Heading heading;

		State() : node(nullptr), heading(0) {;}
		State(const T* node, Heading heading) : node(node), heading(heading) {;}

	};


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(20);


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

public:

	template <int gridSize_cm> State getDestination(Grid<gridSize_cm, T>& grid, State start, float distance_m) {

		int retries = 2;
		State 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, State(start.node, start.heading.getInverted()), distance_m);
		}

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

	}

private:

	static Heading getHeading(const T& from, const T& to) {
		return Heading(from.x_cm, from.y_cm,  to.x_cm, to.y_cm);
	}

	template <int gridSize_cm> State walk(Grid<gridSize_cm, T>& grid, State cur, float distRest_m) {

		// make the draw-list empty (faster than allocating a new one every time?)
		drawer.reset();

		// calculate probabilites for the neighboring nodes
		for (T& neighbor : grid.neighbors(*cur.node)) {

			// heading when walking from cur to neighbor
			Heading potentialHeading(cur.node->x_cm, cur.node->y_cm, neighbor.x_cm, neighbor.y_cm);

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

			// probability for this change?
			double prob = K::NormalDistribution::getProbability(0, HEADING_DIFF_SIGMA, diff);

			// adjust by the neighbors importance factor
			//prob += std::pow(neighbor.impPath, 2);
			prob *= neighbor.imp;
			//prob *= K::NormalDistribution::getProbability(1.4, 0.2, neighbor.imp);

			drawer.add(neighbor, prob);

		}

		// if there is no likely neighbor at all, we reached a dead end
		// -> start over!
		if (drawer.getCumProbability() <  0.01) {return State(nullptr, 0);}

		// get the next node
		T* nn = &drawer.get();

		//Heading potentialHeading(cur.node->x_cm, cur.node->y_cm, nn->x_cm, nn->y_cm);
		//State next(nn, potentialHeading);
		State next(&nn, cur.heading);
		next.heading += headingChangeDist(gen);

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