#ifndef IMPORTANCE_H
#define IMPORTANCE_H



#include "../../../geo/Units.h"
#include "../../Grid.h"
#include "GridFactoryListener.h"

#include "../../../misc/KNN.h"
#include "../../../misc/KNNArray.h"

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




class Importance {

private:

	static constexpr const char* name = "GridImp";

public:


	template <typename T> static void addOutlineNodes(Grid<T>& dst, Grid<T>& src) {

		for (const T& n : src) {
			if (n.getNumNeighbors() < 8) {
				if (!dst.hasNodeFor(n)) {
					dst.add(n);
				}
			}
		}

	}

	/** attach importance-factors to the grid */
	template <typename T> static void addImportance(Grid<T>& g, GridFactoryListener* l = nullptr) {

		Log::add(name, "adding importance information to all nodes");
		if (l) {
			l->onGridBuildUpdateMajor(2, 0);
			l->onGridBuildUpdateMajor("adding importance information");
			l->onGridBuildUpdateMinor("performing initial setups");
		}

		// get an inverted version of the grid
		Grid<T> inv(g.getGridSize_cm());
		addOutlineNodes(inv, g);
		//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);

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

		// create list of all stair-nodes
		std::vector<T> stairs;


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

			switch(n1.getType()) {
			case GridNode::TYPE_DOOR:		doors.push_back(n1);	break;
			case GridNode::TYPE_STAIR:		stairs.push_back(n1);	break;
			case GridNode::TYPE_ELEVATOR:	stairs.push_back(n1);	break;
			}

		}

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

		// KNN for stairs
		KNNArray<std::vector<T>> knnArrStairs(stairs);
		KNN<KNNArray<std::vector<T>>, 3> knnStairs(knnArrStairs);

		// probability adjustments
		Distribution::Normal<float> avoidWalls(0.0, 0.35);
		Distribution::Normal<float> favorDoors(0.0f, 0.5f);
		Distribution::Normal<float> favorStairs(0.0f, 1.5f);

		if (l) {
			l->onGridBuildUpdateMajor(2, 1);
			l->onGridBuildUpdateMinor("calculating importance for each node");
		}

		std::cout << "dunno why, but the KNN for stairs searches extremely slow!" << std::endl;

		// process each node again
		for (int i = 0; i < g.getNumNodes(); ++i) {


			// log
			if (i % (g.getNumNodes() / 20) == 0) {
				if (l) {
					l->onGridBuildUpdateMinor(g.getNumNodes(), i);
				}
			}

			// get the node
			T& n1 = g[i];

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

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

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

			const bool useNormal = (distToWall_m < distToDoor_m && distToWall_m < distToStair_m);

			// final probability
			n1.navImportance = 1.0f;
			n1.navImportance += favorDoors.getProbability(distToDoor_m) * 1.25f;
			n1.navImportance += favorStairs.getProbability(distToStair_m) * 30.5f;

			// use wall avoidance
			if (useNormal) {
				n1.navImportance -= avoidWalls.getProbability(distToWall_m) * 0.5f;
			}

			// sanity check
			Assert::isTrue(n1.navImportance >= 0, "detected negative importance. does not make sense!");

		}

	}


};

#endif // IMPORTANCE_H