Indoor/sensors/radio/setup/WiFiOptimizerPerFloor.h

#ifndef WIFIOPTIMIZERPERFLOOR_H
#define WIFIOPTIMIZERPERFLOOR_H

#include "WiFiOptimizerLogDistCeiling.h"
#include "../model/WiFiModelPerFloor.h"
#include "../../../floorplan/v2/FloorplanHelper.h"
#include <mutex>

#define WITH_DEBUG_PLOT
#ifdef WITH_DEBUG_PLOT
	#include <KLib/misc/gnuplot/Gnuplot.h>
	#include <KLib/misc/gnuplot/GnuplotSplot.h>
	#include <KLib/misc/gnuplot/objects/GnuplotObjectPolygon.h>
	#include <KLib/misc/gnuplot/GnuplotSplotElementColorPoints.h>
	#include <KLib/misc/gnuplot/GnuplotSplotElementLines.h>
#endif

namespace WiFiOptimizer {

	/**
	 * uses the log-distance model, but one per floor.
	 * the model is optimized using all fingerprints that belong to this floor
	 */
	class PerFloor {

		WiFiModelPerFloor* mdl = nullptr;

		WiFiFingerprints fps;

		Floorplan::IndoorMap* map;
		std::mutex mtx;

		VAPGrouper vg;
		Mode mode;

		static constexpr const char* name = "WiFiOptFloor";

	#ifdef WITH_DEBUG_PLOT
		K::Gnuplot gp;
		K::GnuplotSplot splot;
		K::GnuplotSplotElementColorPoints pts;
		K::GnuplotSplotElementLines lines;
	#endif

	public:

		PerFloor(Floorplan::IndoorMap* map, const VAPGrouper& vg, Mode mode) : map(map), vg(vg), mode(mode) {

			// the overall model (contains one sub-model per floor)
			mdl = new WiFiModelPerFloor(map);

			#ifdef WITH_DEBUG_PLOT
				splot.add(&pts); pts.setPointSize(1); pts.setPointType(7);
				splot.add(&lines);
				BBox3 bb = FloorplanHelper::getBBox(map);
				splot.getAxisX().setRange(bb.getMin().x, bb.getMax().x);
				splot.getAxisY().setRange(bb.getMin().y, bb.getMax().y);
				splot.getAxisZ().setRange(bb.getMin().z, bb.getMax().z);
			#endif

		}


		/** make the given fingerprint known to the optimizer */
		void addFingerprint(const WiFiFingerprint& fp) {
			fps.add(fp);
		}

		/** add new fingerprints to the optimizer as data-source */
		virtual void addFingerprints(const WiFiFingerprints& fps) {
			for (const WiFiFingerprint& fp : fps.getFingerprints()) {
				addFingerprint(fp);
			}
		}

		struct OptResultStatsSingle {
			K::Statistics<float> avgApErrors;		// contains one average-error per optimized AP
			K::Statistics<float> singleFPErrors;	// contains one error for each fingerprint<->ap SIGNED
			K::Statistics<float> singleFPErrorsAbs;	// contains one error for each fingerprint<->ap ABSOLUTE
		};

		struct OptResultStats {
			OptResultStatsSingle all;
			std::vector<OptResultStatsSingle> perFloor;
		};

		WiFiModelPerFloor* optimizeAll(LogDistCeiling::APFilter filter, OptResultStats* dst = nullptr) {

			// be sure we got stats to write to
			OptResultStats tmp;
			if (!dst) {dst = &tmp;}

			// alloc place for per-floor stats
			dst->perFloor.resize(map->floors.size());

			// optimize each floor on its own
			//for (WiFiModelPerFloor::ModelForFloor& mdlForFloor : mdl->getFloorModels()) {
			for (size_t i = 0; i < map->floors.size(); ++i) {

				WiFiOptimizer::LogDistCeiling::OptResultStats floorStats;

				const Floorplan::Floor* floor = map->floors[i];

				// 1) create a new optimizer for the current floor
				WiFiOptimizer::LogDistCeiling opt(map, vg, mode);

				// 2) create the model for this floor
				mtx.lock();
					WiFiModelLogDistCeiling* mdlForFloor = new WiFiModelLogDistCeiling(map);
					mdl->add(mdlForFloor, floor);
				mtx.unlock();

				// 3) get the floor's bbox and adjust the z-region (needed for museum in Rothenburg)
				BBox3 bb = FloorplanHelper::getBBox(floor);
				bb.setMinZ(floor->atHeight+0.25);
				bb.setMaxZ(floor->atHeight+2);

				// 4) find all fingerprints that belong to the floor/model and add them to the optimizer
				for (const WiFiFingerprint& fp : fps.getFingerprints()) {
					//if (mdlForFloor.matches(fp.pos_m.z)) {
					//	std::cout << fp.pos_m.z << std::endl;
					//	opt.addFingerprint(fp);
					//}
					if (bb.contains(fp.pos_m)) {
					//if (fp.pos_m.z >= floor->atHeight && fp.pos_m.z < floor->atHeight+floor->height) {
						std::cout << fp.pos_m.z << std::endl;
						opt.addFingerprint(fp);

						#ifdef WITH_DEBUG_PLOT
							pts.add(K::GnuplotPoint3(fp.pos_m.x, fp.pos_m.y, fp.pos_m.z), i);
						#endif

					}
				}

				#ifdef WITH_DEBUG_PLOT
					for (Floorplan::FloorOutlinePolygon* poly : floor->outline) {
						for (const Point2 pt : poly->poly.points) {
							lines.add(K::GnuplotPoint3(pt.x, pt.y, floor->atHeight));
						}
						lines.splitFace(); lines.splitFace();
						for (const Point2 pt : poly->poly.points) {
							lines.add(K::GnuplotPoint3(pt.x, pt.y, floor->atHeight+floor->height));
						}
						lines.splitFace(); lines.splitFace();
					}
					gp.draw(splot);
					gp.flush();
					pts.clear();
					lines.clear();
					sleep(1);
				#endif

				// 5) run the optimizer
				const WiFiOptimizer::LogDistCeiling::APParamsList res = opt.optimizeAll(filter, &floorStats);

				// 6) add all optimized APs to the floor's model
				for (const WiFiOptimizer::LogDistCeiling::APParamsMAC& ap : res.get()) {
					// model is per-floor. so model cant optimize waf.. set to VERY HIGH manually
					const WiFiModelLogDistCeiling::APEntry entry(ap.params.getPos(), ap.params.txp, ap.params.exp, ap.params.waf);
					mdlForFloor->addAP(ap.mac, entry);
				}

				// overall stats
				dst->all.avgApErrors.add(floorStats.avgApErrors);
				dst->all.singleFPErrors.add(floorStats.singleFPErrors);
				dst->all.singleFPErrorsAbs.add(floorStats.singleFPErrorsAbs);

				// per-floor stats
				dst->perFloor[i].avgApErrors.add(floorStats.avgApErrors);
				dst->perFloor[i].singleFPErrors.add(floorStats.singleFPErrors);
				dst->perFloor[i].singleFPErrorsAbs.add(floorStats.singleFPErrorsAbs);

			}

			// overall stats
			Log::add(name, "optimization result: ");
			Log::add(name, " - AvgPerAP   " + dst->all.avgApErrors.asString());
			Log::add(name, " - Single:    " + dst->all.singleFPErrors.asString());
			Log::add(name, " - SingleAbs: " + dst->all.singleFPErrorsAbs.asString());

			// per-floor stats
			for (size_t i = 0; i < dst->perFloor.size(); ++i) {
				Log::add(name, "----------------------------------------------------------------------------------------------------");
				std::string fn = std::to_string(i);
				Log::add(name, " - F"+fn+" AvgPerAP   " + dst->perFloor[i].avgApErrors.asString());
				Log::add(name, " - F"+fn+" Single:    " + dst->perFloor[i].singleFPErrors.asString());
				Log::add(name, " - F"+fn+" SingleAbs: " + dst->perFloor[i].singleFPErrorsAbs.asString());

			}

			return mdl;

		}

	};

}

#endif // WIFIOPTIMIZERPERFLOOR_H