#ifndef GRIDIMPORTANCE_H
#define GRIDIMPORTANCE_H

#include "../Grid.h"
#include "GridFactory.h"
#include "../../misc/KNN.h"

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

class GridImportance {

public:

	/** attach importance-factors to the grid */
	template <int gridSize_cm, typename T> void addImportance(Grid<gridSize_cm, T>& g, const float 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<float, Grid<gridSize_cm, T>, T, 3> knn(inv);

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

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

//			// get its nearest neighbor
//			size_t idxNear;
//			float distSquared;
//			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
//			knn.getNearest(point, idxNear, distSquared);

//			// calculate importante
//			const float imp = importance( Units::cmToM(std::sqrt(distSquared)) );
//			n1.imp = imp;

			size_t indices[10];
			float squaredDist[10];
			float point[3] = {n1.x_cm, n1.y_cm, n1.z_cm};
			knn.get(point, 10, indices, squaredDist);

			const float imp1 = importance( Units::cmToM(std::sqrt(squaredDist[0])) );
			const float imp2 = door( indices );
			n1.imp = (imp1 + imp2)/2;

		}



	}

	float door( size_t* indices ) {

		// build covariance

		//if (dist1_m > 1.0) {return 1;}
		//return 1.0 - std::abs(dist1_m - dist2_m);
		return 1;
	}

	float importance(float dist_m) {

		static K::NormalDistribution d1(0.0, 0.5);
		//if (dist_m > 1.5) {dist_m = 1.5;}
		return 1.0 - d1.getProbability(dist_m) * 0.5;

//		static K::NormalDistribution d1(1.0, 0.75);
//		//static K::NormalDistribution d2(3.0, 0.75);

//		if (dist_m > 3.0) {dist_m = 3.0;}
//		return 0.8 + d1.getProbability(dist_m);// + d2.getProbability(dist_m);

//		if (dist_m < 0.5)	{return 0.8;}
//		if (dist_m < 1.5)	{return 1.2;}
//		if (dist_m < 2.5)	{return 0.8;}
//		else				{return 1.2;}
	}

};

#endif // GRIDIMPORTANCE_H