Indoor/grid/factory/GridImportance.h

#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 "../../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 <typename T> void addImportance(Grid<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<T> inv(g.getGridSize_cm());
		GridFactory<T> fac(inv);
		fac.addInverted(g, z_cm);

		// sanity check
		Assert::isFalse(inv.getNumNodes() == 0, "inverted grid is empty!");

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

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

		// create list of all doors
		std::vector<T> doors;

		// process each node
		for (T& n1 : g) {

			// skip nodes on other than the requested floor-level
			if (n1.z_cm != z_cm) {continue;}

			// 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]]);
			}

			n1.imp = 1.0f;

			n1.imp += getWallImportance( Units::cmToM(std::sqrt(squaredDist[0])) );
			//addDoor(n1, neighbors);

			// is the current node a door?
			if (isDoor(n1, neighbors)) {doors.push_back(n1);}

			// favor stairs just like doors
			if (isStaircase(g, n1)) {doors.push_back(n1);}

		}

		KNNArray<std::vector<T>> knnArrDoors(doors);
		KNN<KNNArray<std::vector<T>>, 3> knnDoors(knnArrDoors);

		// process each node again
		for (T& n1 : g) {

			static Distribution::Normal<float> favorDoors(0.0f, 1.0f);

			// get the distance to the nearest door
			const float dist_m = Units::cmToM(knnDoors.getNearestDistance( {n1.x_cm, n1.y_cm, n1.z_cm} ));

			// importance for this node (based on the distance from the next door)
			//n1.imp += favorDoors.getProbability(dist_m) * 0.30;
			n1.imp += favorDoors.getProbability(dist_m);

		}

	}

	/** is the given node connected to a staircase? */
	template <typename T> bool isStaircase(Grid<T>& g, T& node) {

		// if this node has a neighbor with a different z, this is a stair
		for (T& neighbor : g.neighbors(node)) {
			if (neighbor.z_cm != node.z_cm) {return true;}
		}
		return false;

	}

	/** attach importance-factors to the grid */
	template <typename T> void addDistanceToTarget(Grid<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 <typename T> void addImportance(Grid<T>& g, const DijkstraNode<T>* start, const 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 + Distribution::Normal<float>::getProbability(0, 1.0, dist_m) * 0.8;


		}



	}


	/** is the given node (and its inverted neighbors) a door? */
	template <typename T> bool isDoor( T& nSrc, std::vector<T*> neighbors ) {

		MiniMat2 m;
		Point3 center = nSrc.inCentimeter();

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

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

		// build covariance of the nearest-neighbors
		int used = 0;
		for (const T* n : neighbors) {
			Point3 d = n->inCentimeter() - 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 false;}

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

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

		// door?
		return ((ev.e2/ev.e1) < 0.15) ;

	}

	/** get the importance of the given node depending on its nearest wall */
	float getWallImportance(float dist_m) {

		// avoid sticking too close to walls (unlikely)
		static Distribution::Normal<float> avoidWalls(0.0, 0.5);

		// favour walking near walls (likely)
		static Distribution::Normal<float> stickToWalls(0.9, 0.5);

		// favour walking far away (likely)
		static Distribution::Normal<float> farAway(2.2, 0.5);
		if (dist_m > 2.0) {dist_m = 2.0;}

		// overall importance
//		return	- avoidWalls.getProbability(dist_m) * 0.30		// avoid walls
//				+ stickToWalls.getProbability(dist_m) * 0.15	// walk near walls
//				+ farAway.getProbability(dist_m) * 0.15			// walk in the middle
		return	- avoidWalls.getProbability(dist_m)		// avoid walls
				//+ stickToWalls.getProbability(dist_m)	// walk near walls
				//+ farAway.getProbability(dist_m)			// walk in the middle
		;


	}

};

#endif // GRIDIMPORTANCE_H