Indoor/sensors/radio/WiFiProbabilityFree.h

#ifndef WIFIPROBABILITYFREE_H
#define WIFIPROBABILITYFREE_H

#include "WiFiProbability.h"
#include "model/WiFiModel.h"
#include "../../math/distribution/Normal.h"
#include "../../math/distribution/Region.h"
#include "../../math/distribution/Exponential.h"
#include "VAPGrouper.h"
#include "../../floorplan/v2/Floorplan.h"

#include <unordered_map>

/**
 * compare WiFi-Measurements within predictions of a given model.
 * predictions are just based on the distance to the access-point.
 */
class WiFiObserverFree : public WiFiProbability {

public:

	enum class EvalDist {
		NORMAL_DISTIRBUTION,
		CAPPED_NORMAL_DISTRIBUTION,
		EXPONENTIAL,
	};

private:

	const float sigma;

	const float sigmaPerSecond = 3.0f;

	/** the RSSI prediction model */
	WiFiModel& model;

	/** the map's floorplan */
	Floorplan::IndoorMap* map;

	std::vector<AccessPoint> allAPs;

	bool useError = false;

	EvalDist dist;

public:

	WiFiObserverFree(const float sigma, WiFiModel& model, EvalDist dist = EvalDist::NORMAL_DISTIRBUTION) : sigma(sigma), model(model), dist(dist) {
		allAPs = model.getAllAPs();
	}

	void setUseError(const bool useError) {
		this->useError = useError;
	}

	double getProbability(const Point3& pos_m, const Timestamp curTime, const WiFiMeasurements& obs) const {

		double prob = 1.0;
		//double prob = 0;

		int numMatchingAPs = 0;

		// process each measured AP
		for (const WiFiMeasurement& entry : obs.entries) {

			// sanity check
			Assert::isFalse(entry.getTimestamp().isZero(), "wifi measurement without timestamp. coding error?");

			// get the model's RSSI (if possible!)
			const float modelRSSI = model.getRSSI(entry.getAP().getMAC(), pos_m);

			// NaN? -> AP not known to the model -> skip
			if (modelRSSI != modelRSSI) {continue;}

			// the scan's RSSI
			const float scanRSSI = entry.getRSSI();

			// the measurement's age
			const Timestamp age = curTime - entry.getTimestamp();

			Assert::isTrue(age.ms() >= 0, "found a negative wifi measurement age. this does not make sense");
			Assert::isTrue(age.ms() <= 60000, "found a 60 second old wifi measurement. maybe there is a coding error?");

                        Assert::isBetween(scanRSSI, -100.0f, -20.0f, "scan-rssi out of range");
			//Assert::isBetween(modelRSSI, -160.0f, -10.0f, "model-rssi out of range");

			// sigma grows with measurement age
			const float sigma = this->sigma + this->sigmaPerSecond * age.sec();

			// probability for this AP
			double local = NAN;
			switch (dist) {
				case EvalDist::NORMAL_DISTIRBUTION:
					local = Distribution::Normal<double>::getProbability(modelRSSI, sigma, scanRSSI); break;
				case EvalDist::CAPPED_NORMAL_DISTRIBUTION:
					local = Distribution::Region<double>::getProbability(modelRSSI, sigma, scanRSSI); break;
				case EvalDist::EXPONENTIAL:
					local = Distribution::Exponential<double>::getProbability(0.05, std::abs(modelRSSI-scanRSSI)); break;
				default: throw Exception("unsupported distribution");
			}

			// also add the error value? [location is OK but model is wrong]
			if (useError) {
				local = 0.95 * local + 0.05 * (1.0-local);
				//local = 0.95 * local + 0.05;
			}

			// update probability
			prob *= local;

			++numMatchingAPs;

		}

		// sanity check
		//Assert::isTrue(numMatchingAPs > 0, "not a single measured AP was matched against known ones. coding error? model error?");

		return prob;

	}

	/**
	 * for some locations it might make sense to also look at APs
	 * that have NOT been discovered by the smartphone but SHOULD
	 * be very well receivable at a location.
	 * such locations can be opted-out by using this veto-probability
	 */
	double getVeto(const Point3& pos_m, const Timestamp curTime, const WiFiMeasurements& obs) const {

		(void) curTime;

		struct APR {
			AccessPoint ap;
			float rssi;
			APR(const AccessPoint& ap, const float rssi) : ap(ap), rssi(rssi) {;}
		};

		std::vector<APR> all;
		for (const AccessPoint& ap : allAPs) {
			const float rssi = model.getRSSI(ap.getMAC(), pos_m);
			if (rssi != rssi) {continue;}	// unknown to the model
			all.push_back(APR(ap, rssi));
		}

		// stort by RSSI
		auto comp = [&] (const APR& apr1, const APR& apr2) {return apr1.rssi > apr2.rssi;};
		std::sort(all.begin(), all.end(), comp);

		int numVetos = 0;

		for (int i = 0; i < 3; ++i) {
			const APR& apr = all[i];
			const WiFiMeasurement* mes = obs.getForMac(apr.ap.getMAC());
			const float rssiScan = (mes) ? (mes->getRSSI()) : (-100);
			const float rssiModel = apr.rssi;
			const float diff = std::abs(rssiScan - rssiModel);
			if (diff > 20) {++numVetos;}
		}

		if (numVetos == 0)	{return 0.70;}
		if (numVetos == 1)	{return 0.29;}
		else				{return 0.01;}

	}

	template <typename Node> double getProbability(const Node& n, const Timestamp curTime, const WiFiMeasurements& obs, const int age_ms = 0) const {

        return this->getProbability(n.inMeter() + Point3(0,0,1.3), curTime, obs);
	}

};

#endif // WIFIPROBABILITYFREE_H