#ifndef GRIDWALKSHORTESTPATHCONTROL_H
#define GRIDWALKSHORTESTPATHCONTROL_H


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

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

#include "../../nav/dijkstra/Dijkstra.h"
#include "../../nav/dijkstra/DijkstraPath.h"
#include "../../misc/KNN.h"
#include "../../misc/KNNArray.h"

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

template <typename T> class GridWalkShortestPathControl : public GridWalk<T> {


protected:

	/** put dijkstra-nodes into the KNN */
	class Wrapper {

	private:

		DijkstraPath<T>* path;

	public:

		Wrapper() : path(nullptr) {;}

		/** ctor with the underlying data structure */
		Wrapper(DijkstraPath<T>* path) : path(path) {
			;
		}

		/** get the number of elements to search throrugh */
		inline int kdtree_get_point_count() const {
			return path->size();
		}

		/** use nanoflanns default bbox */
		template <class BBOX> inline bool kdtree_get_bbox(BBOX& bb) const {
			(void) bb; return false;
		}

		/** get the idx-th element's dim-th coordinate */
		inline float kdtree_get_pt(const size_t idx, const int dim) const {
			return (*((*path)[idx]).element)[dim];
		}

		/** get the SQUARED distance between the given coordinates and the provided element */
		inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const {
			const float d0 = p1[0] - (*((*path)[idx_p2]).element)[0];
			const float d1 = p1[1] - (*((*path)[idx_p2]).element)[1];
			const float d2 = p1[2] - (*((*path)[idx_p2]).element)[2];
			return (d0*d0) + (d1*d1) + (d2*d2);
		}


	};



	friend class GridWalkHelper;

protected:

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

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

	Dijkstra<T> dijkstra;
	const T& target;

	Point3 centerOfMass = Point3(0,0,0);
	Wrapper wrapper;
	DijkstraPath<T>* path = nullptr;
	KNN<Wrapper,3>* knn = nullptr;


	DrawList<T&> drawer;


public:



	/** ctor with the target you want to reach */
	template <typename Access> GridWalkShortestPathControl(Grid<T>& grid, const Access& acc, const T& target) : target(target) {

		(void) grid;

		gen.seed(1234);

		// build all shortest path to reach th target
		dijkstra.build(target, target, acc);

	}

	int recalc = 0;

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

		// update the center-of-mass
		centerOfMass = (centerOfMass * 0.999) + (((Point3)*start.node) * 0.001);
		if (path == nullptr) {rebuildPath(grid);}

		if (++recalc > 100 * 1000) {
			recalc = 0;
			rebuildPath(grid);
		}

//		if (knn != nullptr) {
//			const float dist = knn->getNearestDistance( {(float)start.node->x_cm, (float)start.node->y_cm, (float)start.node->z_cm} );
//			if (dist > 10000) {
//				rebuildPath(grid);
//			}
//		}

		// proportional change of the heading
		static 	Distribution::Normal<float> dHead(1, 0.01);

		// proportional change of the to-be-walked distance
		static 	Distribution::Normal<float> dWalk(1, 0.10);

		distance_m = distance_m*dWalk.draw()*2;		// TODO: why *2?
		headChange_rad = headChange_rad*dHead.draw();


		return walk(grid, start, distance_m, headChange_rad);

	}

private:

	double getProbability(const T& start, const T& possible, const Heading head) const {

		// TODO: WHY?! not only when going back to the start?
		if (start.x_cm == possible.x_cm && start.y_cm == possible.y_cm) {
			if (start.z_cm == possible.z_cm) {return 0;} // back to the start
			//throw 1;
			return 0.5;// stair start/end TODO: fix
		}

		// get the angle between START and the possible next node
		const Heading possibleHead = GridWalkHelper::getHeading(start, possible);

		// calculate the difference
		const float diff = possibleHead.getDiffHalfRAD(head);

//		// compare this heading with the requested one
		const double angleProb = Distribution::Normal<float>::getProbability(0, Angle::degToRad(25), diff);
//		const double angleProb = (diff <= Angle::degToRad(15)) ? 1 : 0.1;		// favor best 3 angles equally

		// nodes own importance
		double nodeProb = 1;//(possible.distToTarget < start.distToTarget) ? 1 : 0.025;

		if (knn != nullptr) {
			const float pd_m = knn->getNearestDistance( {(float)possible.x_cm, (float)possible.y_cm, (float)possible.z_cm} ) / 100;
			//const float sd = knn->getNearestDistance( {(float)start.x_cm, (float)start.y_cm, (float)start.z_cm} );
			//nodeProb = (pd < sd) ? 1 : 0.0;
			nodeProb = Distribution::Exponential<float>::getProbability(0.15, pd_m);
		}

		// bring it together
		return angleProb * nodeProb;

	}

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

		// try-again distribution
		//static 	Distribution::Normal<float> dHead(0, Angle::degToRad(10));
		//static 	Distribution::Normal<float> dUpdate(0, Angle::degToRad(3));
			static	Distribution::Uniform<float> dChange(Angle::degToRad(0), +Angle::degToRad(359));

		int retries = 5;
		float walked_m = 0;
		GridWalkState<T> cur = start;

		// the desired heading
		Heading reqHeading = start.heading + (headChange_rad);

		// walk until done
		while(walked_m < distance_m) {

			// evaluate all neighbors
			drawer.reset();
			for (T& neighbor : grid.neighbors(*cur.node)) {

				const double prob = getProbability(*start.node, neighbor, reqHeading);
				drawer.add(neighbor, prob);

			}

			// too bad? start over!
			if (drawer.getCumProbability() < 0.1 && (--retries) >= 0) {
				walked_m = 0;
				cur = start;
				//WHAT THE HELL
				if (retries == 0) { reqHeading = dChange.draw(); }
				continue;
			}

			// get the neighbor
			const T& neighbor = drawer.get();

			// update
			walked_m += neighbor.getDistanceInMeter(*cur.node);
			cur.node = &neighbor;

		}

		cur.distanceWalked_m = NAN;
		cur.headingChange_rad = NAN;
		cur.heading = reqHeading;

		return cur;

	}

	/** rebuild the path for the given center point */
	void rebuildPath(Grid<T>& grid) {

		// find the grid node nearest to the current center-of-mass
		auto nearestGridNode = [&] (const T& n1, const T& n2) { return ((Point3)n1).getDistance(centerOfMass) < ((Point3)n2).getDistance(centerOfMass); };
		const T& currentMass = *std::min_element(grid.begin(), grid.end(), nearestGridNode);

		delete path; path = nullptr;
		delete knn; knn = nullptr;

		DijkstraNode<T>* dnTarget = dijkstra.getNode(target);
		DijkstraNode<T>* dnStart =  dijkstra.getNode(currentMass);

		if (dnStart == nullptr) {
			return;
		}

		// calculate the path from there to the target
		try {
			path = new DijkstraPath<T>(dnStart, dnTarget);

			// create k-nn lookup
			wrapper = Wrapper(path);
			knn = new KNN<Wrapper, 3>(wrapper);
		} catch (...) {
			knn = nullptr;
			path = nullptr;
		}

	}


};

#endif // GRIDWALKSHORTESTPATHCONTROL_H