IPIN2016/competition/src/competition/Optimizer.h

#ifndef OPTIMIZER_H
#define OPTIMIZER_H

#include "Structs.h"
#include "APParams.h"
#include "Scaler.h"
#include "FileReader.h"

#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/floorplan/v2/FloorplanHelper.h>

#include <Indoor/sensors/radio/VAPGrouper.h>
#include <Indoor/geo/BBox3.h>

#include <Indoor/sensors/radio/model/WiFiModelLogDistCeiling.h>

#include <KLib/math/optimization/NumOptAlgoDownhillSimplex.h>
#include <KLib/math/optimization/NumOptAlgoGenetic.h>
#include <KLib/math/optimization/NumOptAlgoRangeRandom.h>


template <typename Scalar> class Outlier {

private:

	std::vector<Scalar> data;

public:

	/** add the given scalar */
	void add(const Scalar& s) {
		auto it = std::lower_bound(data.begin(), data.end(), s);
		data.insert(it, s);
	}

	/** get the sum of all values between start-percent, and end-percent */
	Scalar getSum(const float pStart, const float pEnd) {
		const int i1 = (data.size()) * pStart;
		const int i2 = (data.size()) * pEnd;
		Scalar sum = 0;
		for (int i = i1; i < i2; ++i) {sum += data[i];}
		return sum;
	}

	/** get the average of all values between start-percent, and end-percent */
	Scalar getAvg(const float pStart, const float pEnd) {
		const int i1 = (data.size()) * pStart;
		const int i2 = (data.size()) * pEnd;
		const int cnt = i2-i1+1;
		return getSum(pStart, pEnd) / cnt;
	}

	Outlier operator += (const Scalar& s) {add(s); return *this;}

};


struct Optimizer {

public:

	Floorplan::IndoorMap* map;
	Scaler scaler;
	VAPGrouper::Mode vapMode;
	std::vector<FileReader> records;

//	/** debug only */
//	struct WiFiEntry {
//		std::string mac;
//		int rssi;
//		WiFiEntry(const std::string& mac, const int rssi) : mac(mac), rssi(rssi) {;}
//	};

//	/** group entries by mac+timestamp */
//	struct WiFiEntries {
//		FileReader::Position pos;			// ground-truth position at time of scan
//		std::vector<WiFiEntry> debug;		// all VAP entries at this time
//		int rssiSum;						// sum of all VAP rssis
//		int cnt;							// number of all VAP rssis
//		float getRSSI() const {return (float) rssiSum / (float) cnt;}
//	};

	/** group a wifi-scan with the best-matching ground-truth position */
	struct WiFiOptBase {
		WiFiMeasurement apScan;
		FileReader::Position pos;
		WiFiOptBase(const WiFiMeasurement& apScan, const FileReader::Position pos) : apScan(apScan), pos(pos) {;}
	};

	/** group entries by mac (faster access) */
	std::unordered_map<MACAddress, std::vector<WiFiOptBase>> wifiMap;

//	/** group entries by mac+timestamp */
//	std::unordered_map<MACAddress, std::unordered_map<float, WiFiEntries>> wifiMap;



	//std::vector<WiFiMeasurementsAtGroundTruthPosition> wifiPos;

public:

	Optimizer(Floorplan::IndoorMap* map, Scaler scaler, VAPGrouper::Mode vapMode) : map(map), scaler(scaler), vapMode(vapMode) {;}

	/** add the given record */
	void addRecord(const std::string& file) {

		records.push_back(FileReader(file));

		buildWiFiMap();

	}


	/** add all of the given records */
	void addRecords(const std::vector<std::string>& files) {
		for (const std::string& file : files) {
			addRecord(file);
		}
	}


//	std::vector<std::string> getAllMACs() {

//		std::unordered_set<std::string> macs;
//		for (const FileReader& record : records) {
//			for (const FileReader::TS<FileReader::WiFi>& scan : record.getWiFi()) {
//				macs.insert(scan.data.mac);
//			}
//		}

//		auto comp = [] (const std::string& a, const std::string& b) {return a < b;};
//		std::vector<std::string> sorted;
//		for (std::string mac : macs) {sorted.push_back(mac);}
//		std::sort(sorted.begin(), sorted.end(), comp);

//		return sorted;

//	}

	std::vector<MACAddress> getAllMACs() {
		//buildWiFiMap();
		std::vector<MACAddress> res;
		for (auto it : wifiMap) {res.push_back(it.first);}
		return res;
//		for (const WiFiMeasurementsAtGroundTruthPosition& e : wifiPos) {
//			for (const WiFiMeasurement& m : e.measurements.entries) {
//				res.insert(m.getAP().getMAC());
//			}
//		}
//		std::vector<MACAddress> vec;
//		vec.insert(vec.end(), res.begin(), res.end());
//		return vec;
	}


	void buildWiFiMap() {

		wifiMap.clear();

		// how to combine VAPs
		const VAPGrouper vg(vapMode, VAPGrouper::Aggregation::MAXIMUM);


		// parse each training data file
		for (const FileReader& record : records) {

			// ground-truth interpolation for the given record-data-set
			const Interpolator<float, FileReader::Position> path = record.getPath();

			// process each wifi-scan within the record-data-set
			for (const FileReader::TS<WiFiMeasurements>& scan : record.getWiFiGroupedByTime()) {

				for (const WiFiMeasurement& m : scan.data.entries) {
					std::cout << m.getAP().getMAC().asString() << ":" << m.getRSSI() << std::endl;
				}
				std::cout << "----------------------------------" << std::endl;

				// group all VAPs within one scan
				const WiFiMeasurements vapGrouped = vg.group(scan.data);

				for (const WiFiMeasurement& m : vapGrouped.entries) {
					std::cout << m.getAP().getMAC().asString() << ":" << m.getRSSI() << std::endl;
				}

				// add entries grouped by MAC (faster access for the optimizer)
				for (const WiFiMeasurement& m : vapGrouped.entries) {

					// ombine scanned AP with ground-truth position during scan and add to the map
					const FileReader::Position pos = path.get(m.getTimestamp());
					wifiMap[m.getAP().getMAC()].push_back( WiFiOptBase(m, pos) );

				}

			}

		}

	}

	// optimize the given AP
	APParams optimize(const MACAddress& mac, float& errResult) {

		// starting parameters do not matter for the current optimizer!
		APParams params(0,0,0, -40, 2.5, 0.0);

//		// construct vector of all readings: signal-strength -> position
//		std::vector<FileReader::WiFiPos> readings;
//		for (const FileReader& record : records) {
//			const std::vector<FileReader::WiFiPos> tmp = record.getWiFiPos(mac);
//			readings.insert(readings.end(), tmp.begin(), tmp.end());
//		}


		//std::cout << "use different errors: diff diff² diff³ and compare the result?" << std::endl;

		const float hugeError = 1e10;

		auto func = [&] (const float* data) {

			const APParams* params = (APParams*) data;

			if (params->waf > 0) {return hugeError;}

			if (params->txp < -50) {return hugeError;}
			if (params->txp > -30) {return hugeError;}

			if (params->exp > 4) {return hugeError;}
			if (params->exp < 1) {return hugeError;}

			// current position guess for the AP;
			const Point3 apPos(params->x, params->y, params->z);

			// signal-strength-prediction-model...
			WiFiModelLogDistCeiling model(map);

			// ... with the current parameter guess: position, txp, exp, waf
			model.addAP( mac, WiFiModelLogDistCeiling::APEntry(apPos, params->txp, params->exp, params->waf));

			//Outlier<float> out;
			float err = 0;
			int cnt = 0;

			// fetch all measurements (with ground-truth) available for THE REQUESTED AP
			const std::vector<WiFiOptBase>& entries = wifiMap[mac];

			// process each measurement
			for (const WiFiOptBase& reading : entries) {

				// get the corresponding ground truth ESTIMATION
				const FileReader::Position worldPos = reading.pos;

				// convert it from world(lat,lon) to map(x,y)
				//const float fixedFloorHeight = 4.0;
				const Point3 mapPos = scaler.convert3D(worldPos.lat, worldPos.lon, worldPos.floorNr);
				//const float z = fixedFloorHeight * worldPos.floor;
				//const Point3 mapPos = Point3(mapPos2.x, mapPos2.y, z);		// TODO! z coordinate

				// model estimation for the AP
				const float rssiModel = model.getRSSI(mac, mapPos);

				// difference between guess and measurement?
				const float diff = std::abs(rssiModel - reading.apScan.getRSSI());

				// append
				err += diff*diff;
				++cnt;

				// max distance penality
				if (apPos.getDistance(mapPos) > 120) {	// unlikely!
					err += 999999;
				}


			}


			//float err1 = out.getAvg(0.0, 1.0);
			//float err = std::sqrt(out.getAvg(0.0, 0.99));

			err /= cnt;
			err = std::sqrt(err);


			if (params->txp < -50) {err += 999999;}
			if (params->txp > -35) {err += 999999;}

			if (params->exp > 3.5) {err += 999999;}
			if (params->exp < 1.0) {err += 999999;}

//			if (params->x < -100) {err += 999999;}
//			if (params->y < -100) {err += 999999;}

			return err;


		};


		//
		const BBox3 mapBBox = FloorplanHelper::getBBox(map);

		using LeOpt = K::NumOptAlgoRangeRandom<float>;
		const std::vector<LeOpt::MinMax> valRegion = {
			 LeOpt::MinMax(mapBBox.getMin().x - 20, mapBBox.getMax().x + 20),	// x
			 LeOpt::MinMax(mapBBox.getMin().y - 20, mapBBox.getMax().y + 20),	// y
			 LeOpt::MinMax(mapBBox.getMin().z -  5, mapBBox.getMax().z +  5),	// z
			 LeOpt::MinMax(-50,-30),	// txp
			 LeOpt::MinMax(1,3),		// exp
			 LeOpt::MinMax(-10,-4),		// waf
		 };

		std::cout << "use more rounds for production" << std::endl;

		LeOpt opt(valRegion);
		opt.setPopulationSize(500);	// USE MORE FOR PRODUCTION
		opt.setNumIerations(100);
		opt.calculateOptimum(func, (float*) &params);

//		using LeOpt = K::NumOptAlgoGenetic<float>;
//		LeOpt opt(6);
//		opt.setPopulationSize(750);
//		opt.setMaxIterations(50);
//		opt.setElitism(0.05f);
//		opt.setMutation(0.75f);
//		//opt.setValRange({0.5, 0.5, 0.5, 0.1, 0.1, 0.1});
//		opt.setValRegion(valRegion);

//		K::NumOptAlgoDownhillSimplex<float, 6> opt;
//		opt.setMaxIterations(100);
//		opt.setNumRestarts(10);

		opt.calculateOptimum(func, (float*) &params);
		std::cout << params.x << "," << params.y << "," << params.z << " txp: " << params.txp << " exp: " << params.exp << " waf: " << params.waf << " err: " << func((float*)&params) << "dB" << std::endl;


		errResult = func((float*)&params);
		return params;

	}


};


#endif // OPTIMIZER_H