OTHER2017/plots/PlotWiFiGroundProb.h

#ifndef EVALWIFIGROUNDPROB_H
#define EVALWIFIGROUNDPROB_H

#include <KLib/math/statistics/Statistics.h>

#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
#include "Indoor/sensors/radio/setup/WiFiFingerprint.h"
#include "Indoor/sensors/radio/setup/WiFiFingerprints.h"

#include "Indoor/sensors/radio/setup/WiFiOptimizer.h"
#include "Indoor/sensors/radio/setup/WiFiOptimizerLogDistCeiling.h"

#include "Indoor/sensors/radio/VAPGrouper.h"

#include "Indoor/floorplan/v2/Floorplan.h"
#include "Indoor/floorplan/v2/FloorplanReader.h"
#include "Indoor/floorplan/v2/FloorplanHelper.h"
#include "Indoor/floorplan/v2/FloorplanCeilings.h"

#include "Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h"
#include "Indoor/sensors/offline/FileReader.h"

#include "Indoor/sensors/radio/WiFiProbabilityFree.h"

#include "../Helper.h"

#include <vector>
#include "../plots/Plotty.h"

class PlotWiFiGroundProb {

private:

	const Floorplan::IndoorMap* map;

	const float stepSize_m = 3.0;
	//std::vector<Point3> pos;

	struct Entry {
		Point3 pos;
		K::GnuplotObjectPolygon* poly;
		float max = -1;
		float sum = 0;
		int cnt = 0;
	};

	std::vector<Entry> pos;

	Plotty plot;

public:

	/** ctor */
	PlotWiFiGroundProb(const Floorplan::IndoorMap* map, Plotty::Settings settings = Plotty::Settings()) : map(map), plot(map) {

		plot.settings = settings;

		buildEvalPoints();
		plot.buildFloorplan();



	}

	Plotty& getPlot() {
		return plot;
	}

	/** plot the ground-probability for the given measurement */
	void show(const WiFiObserverFree& prob, const WiFiMeasurements& mes, const Timestamp ts) {

		// determine min/max probability
		double min = +99999;
		double max = -99999;
		for (Entry& e : pos) {
			const double p = prob.getProbability(e.pos, ts, mes);
			if (p < min) {min = p;}
			if (p > max) {max = p;}
		}
		const double diff = max-min;

		// render
		for (Entry& e : pos) {
			const double p = prob.getProbability(e.pos, ts, mes);
			double f = (p-min) / diff * 255;
			if (f < 0) {f = 0;}
			if (f > 255) {f = 255;}

			e.sum += p;
			e.cnt++;

			//if (f > e.max) {
				const K::GnuplotColor c = K::GnuplotColor::fromHSV(0, f, 255);
				e.poly->getFill().setColor(c);
				e.max = f;

				e.poly->setEnabled(f > 32);
			//}
//			if (f < 32) {
//				e.poly->setEnabled(false);
//			} else {
//				e.poly->setEnabled(true);
//			}

			//plot.cpoints.add(K::GnuplotPoint3(pt.x, pt.y, pt.z), p);
		}

	}

	void showStrongest(const WiFiObserverFree& prob, const WiFiMeasurements& mes, const float hue) {

		// determine min/max probability
		double min = +99999;
		double max = -99999;
		for (Entry& e : pos) {
			const double p = prob.getProbability(e.pos, mes.entries.front().getTimestamp(), mes);
			if (p < min) {min = p;}
			if (p > max) {max = p;}
		}
		const double diff = max-min;

		// render
		for (Entry& e : pos) {

			// get probability within [0:255]
			const double p = prob.getProbability(e.pos, mes.entries.front().getTimestamp(), mes);
			const double f = (p-min) / diff * 255;

			if (p != p) {
				throw Exception("nan");
			}

			// overwrite weak entries
			if (f > e.max) {

				const K::GnuplotColor c = K::GnuplotColor::fromHSV(hue, f, 245);
				e.poly->getFill().setColor(c);
				e.max = f;

				// disable unlikely onces to save performance and disk-space
				e.poly->setEnabled(f > 32);

			}

		}

	}

	void update() {

		double min = +99999;
		double max = -99999;
		for (Entry& e : pos) {
			const double p = e.sum / e.cnt;
			if (p < min) {min = p;}
			if (p > max) {max = p;}
		}
		double diff = max-min;

		for (Entry& e : pos) {
			const double p = e.sum / e.cnt;
			const double f = (p-min) / diff * 255;
			const K::GnuplotColor c = K::GnuplotColor::fromHSV(0, f, 255);
			e.poly->getFill().setColor(c);
		}

	}

	void plotMe() {
		plot.plot();
	}

private:

	/** get a list of all points among the outline */
	void buildEvalPoints() {

		BBox3 bbox = FloorplanHelper::getBBox(map);

		for (Floorplan::Floor* f : map->floors) {
			for (float y = bbox.getMin().y; y < bbox.getMax().y; y += stepSize_m) {
				for (float x = bbox.getMin().x; x < bbox.getMax().x; x += stepSize_m) {

					GridFactory<int>::PartOfOutline part = GridFactory<int>::isPartOfFloorOutline(x*100, y*100, f->outline);
					if (part != GridFactory<int>::PartOfOutline::NO) {

						const float s = stepSize_m/2.0f;

						Entry e;

						e.poly = new K::GnuplotObjectPolygon();
						e.poly->add(K::GnuplotCoordinate3(x-s, y-s, f->atHeight, K::GnuplotCoordinateSystem::FIRST));
						e.poly->add(K::GnuplotCoordinate3(x+s, y-s, f->atHeight, K::GnuplotCoordinateSystem::FIRST));
						e.poly->add(K::GnuplotCoordinate3(x+s, y+s, f->atHeight, K::GnuplotCoordinateSystem::FIRST));
						e.poly->add(K::GnuplotCoordinate3(x-s, y+s, f->atHeight, K::GnuplotCoordinateSystem::FIRST));
						e.poly->close();
						e.poly->getFill().setStyle(K::GnuplotFillStyle::SOLID);
						e.poly->setStroke(K::GnuplotStroke::NONE());
						e.poly->setZIndex(f->atHeight - 0.001);	// between outline and obstacles

						e.pos = Point3(x, y, f->atHeight);

						pos.push_back(e);
						plot.splot.getObjects().add(e.poly);

					}

				}
			}
		}

	}


};

#endif // EVALWIFIGROUNDPROB_H