/*
 * © 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 GRIDWALKWEIGHTED3_H
#define GRIDWALKWEIGHTED3_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(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 */
	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) {

		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 = getHeading(*cur.node, neighbor);

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

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

			prob *= std::pow(neighbor.impPath, 5);

			drawer.add(neighbor, prob);

		}

		State next(nullptr, cur.heading);

		// pick a random destination
		T& nDir = drawer.get();
		const Heading hDir = getHeading(*cur.node, nDir);
		//next.heading += (cur.heading.getRAD() - hDir.getRAD()) * -0.95;
		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 = getHeading(*cur.node, n1);
			Heading h2 = getHeading(*cur.node, n2);
			const float d1 = next.heading.getDiffHalfRAD(h1);
			const float d2 = next.heading.getDiffHalfRAD(h2);
			return 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 // GRIDWALKWEIGHTED3_H