#ifndef GPSPROBABILITY_H
#define GPSPROBABILITY_H

#include "GPSData.h"
#include "../../geo/Point3.h"
#include "../../math/distribution/Region.h"
#include "../../geo/EarthMapping.h"

class GPSProbability {

private:

	/** convert between map and earth */
	const EarthMapping& em;

public:

	/** ctor with the map<->earth translator */
	GPSProbability(const EarthMapping& em) : em(em) {
		;
	}

	/** get the probability for residing at pos_m [in meter, map-coordinates] given some GPS measurement */
	double getProbability(const Point3 pos_m, const GPSData& d) const {

		// pad GPS? -> no gps eval
		if (isBad(d)) {return 1.0;}

		// adjust accuracy [sometimes strange values are provided here!]
		float accuracy = d.accuracy;
		if (accuracy < 3.0) {
			std::cout << "note: adjusting gps accuracy as '" << accuracy << "'' seems invalid" << std::endl;
			accuracy = 3.0;
		}

		// convert GPS to map coordinats
		const Point3 gpsToMap_m = em.worldToMap(d.toEarthPos());

		// distance between given point and GPS's estimation
		const float dist = pos_m.getDistance(gpsToMap_m);

		// calculate probability
		//const double prob = Distribution::Region<double>::getProbability(0, d.accuracy, dist);
		const double prob = Distribution::Normal<double>::getProbability(0, accuracy, dist);

		// sanity checks
		Assert::isNot0(prob, "gps probability is 0.0");
		Assert::isNotNaN(prob, "gps probability is NaN");

		// done
		return prob;

	}

private:

	/** returns true if the given GPS reading is bad [inaccurate, invalid, ...] */
	static inline bool isBad(const GPSData& d) {
		return (!d.isValid()) || (d.accuracy == 0) || (d.accuracy > 25);
	}


};

#endif // GPSPROBABILITY_H