#ifndef GRIDIMPORTANCE_H
#define GRIDIMPORTANCE_H

#include "../Grid.h"
#include "GridFactory.h"
#include "../../misc/KNN.h"
#include "../../misc/KNNArray.h"
#include "../../math/MiniMat2.h"

#include "../../misc/Debug.h"
#include "../../nav/dijkstra/Dijkstra.h"
#include "../../nav/dijkstra/DijkstraPath.h"

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




/**
 * add an importance factor to each node within the grid.
 * the importance is calculated based on several facts:
 * - nodes that belong to a door or narrow path are more important
 * - nodes directly located at walls are less important
 */
class GridImportance {

private:

	static constexpr const char* name = "GridImp";

public:

	/** attach importance-factors to the grid */
	template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, const float z_cm) {

		Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));

		// get an inverted version of the grid
		Grid<gridSize_cm, T> inv;
		GridFactory<gridSize_cm, T> fac(inv);
		fac.addInverted(g, z_cm);

		// construct KNN search
		KNN<Grid<gridSize_cm, T>, 3> knn(inv);

		// the number of neighbors to use
		static constexpr int numNeighbors = 8;

		for (int idx = 0; idx < g.getNumNodes(); ++idx) {

			// process each point
			T& n1 = (T&) g[idx];

			// get the 10 nearest neighbors and their distance
			size_t indices[numNeighbors];
			float squaredDist[numNeighbors];
			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
			knn.get(point, numNeighbors, indices, squaredDist);

			// get the neighbors
			std::vector<T*> neighbors;
			for (int i = 0; i < numNeighbors; ++i) {
				neighbors.push_back(&inv[indices[i]]);
			}

			addImportance(n1, Units::cmToM(std::sqrt(squaredDist[0])) );
			addDoor(n1, neighbors);

		}

	}

	/** attach importance-factors to the grid */
	template <int gridSize_cm, typename T> void addDistanceToTarget(Grid<gridSize_cm, T>& g, Dijkstra<T>& d) {

		//Log::add(name, "adding importance information to all nodes at height " + std::to_string(z_cm));

		for (T& node : g) {

			DijkstraNode<T>* dn = d.getNode(node);
			if (dn != nullptr) {
				node.distToTarget = dn->cumWeight / 2000;
			}

		}


	}

	template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, DijkstraNode<T>* start, DijkstraNode<T>* end) {

		// routing path
		DijkstraPath<T> path(end, start);

		// knn search within the path
		KNN<DijkstraPath<T>, 3> knn(path);

		// update each node from the grid using its distance to the path
		for (T& n : g) {

			//const int idx = knn.getNearestIndex( {n.x_cm, n.y_cm, n.z_cm} );
			//T& node = g[idx];
			const float dist_cm = knn.getNearestDistance( {n.x_cm, n.y_cm, n.z_cm} );
			const float dist_m = Units::cmToM(dist_cm);
			n.impPath = 1.0 + K::NormalDistribution::getProbability(0, 1.0, dist_m) * 0.8;


		}



	}


	/** add importance to nSrc if it is part of a door */
	template <typename T> void addDoor( T& nSrc, std::vector<T*> neighbors ) {

		MiniMat2 m;
		Point3 center = nSrc;

		// calculate the centroid of the nSrc's nearest-neighbors
		Point3 centroid(0,0,0);
		for (const T* n : neighbors) {
			centroid = centroid + (Point3)*n;
		}
		centroid /= neighbors.size();

		// if nSrc is too far from the centroid, this does not make sense
		if ((centroid-center).length() > 60) {return;}

		// build covariance of the nearest-neighbors
		int used = 0;
		for (const T* n : neighbors) {
			Point3 d = (Point3)*n - center;
			if (d.length() > 100) {continue;}	// radius search
			m.addSquared(d.x, d.y);
			++used;
		}

		// we need at least two points for the covariance
		if (used < 2) {return;}

		// check eigenvalues
		MiniMat2::EV ev = m.getEigenvalues();

		// ensure e1 > e2
		if (ev.e1 < ev.e2) {std::swap(ev.e1, ev.e2);}

		// door?
		if ((ev.e2/ev.e1) < 0.15) { nSrc.imp *= 1.3; }

	}

	/** get the importance of the given node depending on its nearest wall */
	template <typename T> void addImportance(T& nSrc, float dist_m) {

		// avoid sticking too close to walls (unlikely)
		static K::NormalDistribution avoidWalls(0.0, 0.3);

		// favour walking near walls (likely)
		static K::NormalDistribution sticToWalls(0.9, 0.5);

		// favour walking far away (likely)
		static K::NormalDistribution farAway(2.2, 0.5);
		if (dist_m > 2.0) {dist_m = 2.0;}

		// overall importance
		nSrc.imp *=  1.0
				- avoidWalls.getProbability(dist_m) * 0.35		// avoid walls
				+ sticToWalls.getProbability(dist_m) * 0.15		// walk near walls
				+ farAway.getProbability(dist_m) * 0.20			// walk in the middle
		;


	}

};

#endif // GRIDIMPORTANCE_H