#ifndef BEACONEVALUATION_H
#define BEACONEVALUATION_H

#include <KLib/math/distribution/Normal.h>
#include "BeaconObservation.h"
#include "Settings.h"
#include "../particles/MyState.h"
#include "../particles/MyObservation.h"
#include "PositionedBeacon.h"

class BeaconEvaluation {

private:

	Settings settings;
	//BeaconObservation obs;

public:

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

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

//		const double tx = -74;
		const double waf = 20.0;

//		// 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(tx, distToStrongest_m, relAP->pl) - (strongestFloorDist * waf);

		// process each detected beacon
		for (const BeaconObservationEntry& entry : observation.beacons.entries) {

			// get the AP data from the settings
			const PositionedBeacon* beacon = settings.getBeacon(entry.mac);
			if (!beacon) {continue;}

			// distance (in meter) between particle and AP
			//const double distToBeacon_m = state.getDistance2D(beacon->xCM, beacon->yCM) / 100.0;
			const double distToBeacon_m = state.pCur.getDistance(Point3(beacon->x, beacon->y, beacon->z)) / 100.0;

			// floor difference?
			//const double floorDist = std::abs(beacon->zNr - state.getFloorNr());
			const float floorDist = std::round(std::abs(Helper::getFloorNrFloat(beacon->z) - Helper::getFloorNrFloat(state.pCur.z)));

			// estimate the rssi depending on above distance
			const double mdlRSSI = distanceToRssi(beacon->tx, distToBeacon_m, beacon->pl) - (floorDist * 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) * 2.0;
			const double p = K::NormalDistribution::getProbability(mdlRSSI, sigma, realRSSI);
			//const double p = K::NormalDistribution::getProbability(mdlRelRSSI, sigma, realRelRSSI);

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

		}

		const double lambda = 0.15;
		const double res = lambda * exp(- lambda * (-prob));
		return res;
		//return prob;

	}

//	WiFiObservation filter(const WiFiObservation* obs) const {

//		WiFiObservation out;
//		out.ts = obs->ts;
//		for (const WiFiObservationEntry& entry : obs->entries) {
//			// alter the mac
//			WiFiObservationEntry ne = entry;
//			ne.mac[ne.mac.length()-1] = '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 // BEACONEVALUATION_H