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

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

class GridWalkHelper {

public:

	/** get the heading-change between the two given locations */
	template <typename T> static Heading getHeading(const T& from, const T& to) {
		return Heading(from.x_cm, from.y_cm,  to.x_cm, to.y_cm);
	}

	/** get the neighbor of "from" best matching the given heading "h" */
	template <typename T> static T& getBestNeighbor(Grid<T>& grid, const T& from, const Heading h) {

		auto comp = [&] (const T& n1, const T& n2) {
			const Heading h1 = getHeading(from, n1);
			const Heading h2 = getHeading(from, n2);
			const float d1 = h.getDiffHalfRAD(h1);
			const float d2 = h.getDiffHalfRAD(h2);
			//return (h.getDiffHalfRAD(h1) < h.getDiffHalfRAD(h2));
			//return (d1 == d2) ? (rand() < RAND_MAX/2) : (d1 < d2);		// same heading? > random decision
			//return	((from.z_cm != n1.z_cm) && (from.z_cm == n2.z_cm)) ||
				//	(d1 < d2) ||
					//(rand() < RAND_MAX/2);
			if		(d1 < d2)	{return true;}
			else if	(d1 == d2)	{return n1.z_cm != n2.z_cm;}
			return false;
		};

		auto neighbors = grid.neighbors(from);
		return *std::min_element(neighbors.begin(), neighbors.end(), comp);

	}

	/**
	 * try to walk the given distance from the provided node.
	 * if this fails (algorithm cancel walk e.g. due to detected wall collissions)
	 *  - try again from the start
	 * if this also fails several times
	 *  - try to walk in the opposite direction instead (bounce-back)
	 * if this also fails
	 *  - add some randomness and try again
	 */
	template <typename T, typename Walker> static GridWalkState<T> retryOrInvert(Walker& w, const int numRetries, Grid<T>& grid, GridWalkState<T> start, float distance_m, const float headChange_rad) {

		Assert::isTrue(distance_m >= 0, "distance must not be negative!");
		Assert::isNotNull(start.node, "starting node must not be null");

		GridWalkState<T> res;

		//again:

		int retries = numRetries;

		// 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 = w.walk(grid, start, distance_m, headChange_rad);
		} while (res.node == nullptr && --retries);

		// still reaching a dead end?
		// -> try a walk in the opposite direction instead
		if (res.node == nullptr) {
			GridWalkState<T> newState(start.node, start.heading.getInverted());
			res = w.walk(grid, newState, distance_m, headChange_rad);
		}

		// still nothing found? -> modify and try again
		if (res.node == nullptr) {
//			start.node = &(*grid.neighbors(*start.node).begin());
//			start.heading += 0.25;
//			goto again;
			res.node = &(*grid.neighbors(*start.node).begin());
			res.heading = start.heading;
		}

		return res;

	}

};

#endif // GRIDWALKHELPER_H