#ifndef GROUNDTRUTHWAY_H
#define GROUNDTRUTHWAY_H

#include <Indoor/math/Interpolator.h>
#include <Indoor/geo/Point3.h>

/**
 * interpolated ground-trouth based on timed check-points
 */
class GroundTruthWay : public Interpolator<uint64_t, Point3> {

public:

	Point3 getPosAtTime(const uint64_t ts) const {
		return get(ts);
	}

	/** get the ground truth way */
	const std::vector<InterpolatorEntry>& getWay() const {return entries;}

	/** get the time where the ground-truth is nearest to p */
	uint64_t getNearestTime(const Point3 p) const {

		// TODO only an approximation (100ms) -> minor errors possible

		float minD = INFINITY;
		uint64_t minTS = 0;
		for (uint64_t ts = getMinKey(); ts <= getMaxKey(); ts += 100) {
			const Point3 cur = getPosAtTime(ts);
			const float curD = cur.getDistance(p);
			if (curD < minD) {minD = curD; minTS = ts;}
		}
		return minTS;

	}

	/** get the minimum distance between the given point and the ground-truth (at any time) */
	float getMinDist(const Point3 p1) const {
		const uint64_t minTS = getNearestTime(p1);
		const Point3 p2 = getPosAtTime(minTS);
		return p1.getDistance(p2);
	}

};


#endif // GROUNDTRUTHWAY_H