#ifndef WIFIEVALUATION_H
#define WIFIEVALUATION_H

#include "../particles/MyState.h"
#include "WiFiObservation.h"
#include "PositionedWiFiAP.h"
#include "Settings.h"
#include "../particles/MyObservation.h"

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



class WiFiEvaluation {

private:

	Settings settings;
	WiFiObservation obs;
	WiFiObservationEntry strongest;

public:

	void nextObservation(const WiFiObservation& _obs) {

		if (_obs.entries.empty()) {return;}

		obs = filter(_obs);
		strongest = getStrongest(&obs);

	}

	double getProbability(const MyState& state, const MyObservation& observation) const {

		if (obs.entries.empty()) {return 1.0;}
		double prob = 0;//1.0;

		//const double tx = -48;		// tablet
		//const double pl = 3.15;
		const float waf = 8;//10.0;		// was 7 before?! has something todo with the floor heights / levels

		// get the ap the client had the strongest measurement for
		//const PositionedWifiAP* relAP = settings.getAP(strongest.mac); assert(relAP);
		//const double distToStrongest_m = state.getDistance2D(relAP->xCM, relAP->yCM) / 100.0;
		//const double strongestFloorDist = std::abs(relAP->zNr - state.z_nr);
		//const double mdlStrongestRSSI = distanceToRssi(relAP->tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf);

		// process each detected AP
		for (const WiFiObservationEntry& entry : obs.entries) {

			// get the AP data from the settings
			const PositionedWifiAP* ap = settings.getAP(entry.mac); assert(ap);

			// distance (in meter) between particle and AP
			const float distToAP_m = state.pCur.getDistance(*ap) / 100.0;

			// floor difference?
			const float floorDiff = std::abs(Helper::getFloorNrFloat(ap->z) - Helper::getFloorNrFloat(state.pCur.z));
			//const float floorDiff = std::round(std::abs(Helper::getFloorNr(ap->z) - Helper::getFloorNr(state.pCur.z)));

			//const float floorDiff = std::abs(ap->z - state.pCur.z) / 340;

			// estimate the rssi depending on above distance
			const double mdlRSSI = distanceToRssi(ap->tx, distToAP_m, ap->pl) - (floorDiff * waf);

			// the measured rssi
			const double realRSSI = entry.rssi;

			// the measured relative rssi
			//const double realRelRSSI = strongest.rssi - realRSSI;
			//const double mdlRelRSSI = mdlStrongestRSSI - mdlRSSI;

			// probability? (sigma grows with measurement's age)
			const double sigma = (8) + ((observation.latestSensorDataTS - entry.ts) / 1000.0) * 3.5;
			const double p = K::NormalDistribution::getProbability(mdlRSSI, sigma, realRSSI);			// absolute
			//const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI);	// relative

			//prob *= p;
			prob += std::log(p);

		}

		//const double lambda = 0.05;//0.15; //0.12;
		//return lambda * exp(- lambda * (-prob));
		return std::pow(std::exp(prob), 0.1);
//		if (prob < -30)		{return 0.01;}
//		if (prob < -15)		{return 0.50;}
//		else				{return 1.00;}


		//return prob;

	}

	WiFiObservation filter(const WiFiObservation& obs) const {

		WiFiObservation out;
		for (const WiFiObservationEntry& entry : obs.entries) {
			// alter the mac
			WiFiObservationEntry ne = entry;
			//ne.mac[ne.mac.length()-1] = '0';		// enabled = VAP grouping. also adjust settings to use ending "0"
			if (settings.getAP(ne.mac)) {out.entries.push_back(ne);}
		}
		return out;

	}

	/** get the strongest AP within all measurements */
	WiFiObservationEntry getStrongest(const WiFiObservation* obs) const {
		WiFiObservationEntry max = obs->entries.front();
		for (const WiFiObservationEntry& entry : obs->entries) {
			if (entry.rssi > max.rssi) {max = entry;}
		}
		return max;
	}

	static double rssiToDistance(double txPower, double rssi, double pathLoss) {
		return pow(10, (txPower - rssi) / (10 * pathLoss));
	}

	static double distanceToRssi(double txPower, double distance, double pathLoss) {
		if (distance <= 1) {return txPower;}
		return (txPower - (10 * pathLoss * log10(distance)));
	}

};

#endif // WIFIEVALUATION_H