#ifndef INDOOR_SYNTHETICPATH_H
#define INDOOR_SYNTHETICPATH_H

#include "../math/Interpolator.h"
#include "../floorplan/v2/Floorplan.h"
#include "../floorplan/v2/FloorplanHelper.h"
#include "../floorplan/v2/FloorplanHelper.h"

/** allows interpolation along a synthetic path */
class SyntheticPath : private Interpolator<float, Point3> {

	const Floorplan::IndoorMap* map;

public:

	using Base = Interpolator<float, Point3>;
	using Entry = Base::InterpolatorEntry;

	/** create path using the given ground-truth points from the map */
	void create(const Floorplan::IndoorMap* map, std::vector<int> ids) {

		this->map = map;

		// get all ground-truth points from the map
		auto gtps = FloorplanHelper::getGroundTruthPoints(map);
		float dist = 0;

		// create distance-based entries within the interpolator
		for (int i = 0; i < ids.size(); ++i) {
			const int id = ids[i];
			Point3 gtp = gtps[id];
			if (i >= 1) {
				Point3 gtpPrev = gtps[id-1];
				dist += gtpPrev.getDistance(gtp);
			}
			Base::add(dist, gtp);
		}

	}

	/** get the path's length */
	float getLength() const {
		//return Base::getEntries().back().key;
		float dist = 0;
		for (size_t i = 0; i < getEntries().size()-1; ++i) {
			const auto& e1 = getEntries()[i];
			const auto& e2 = getEntries()[i+1];
			dist += e1.value.getDistance(e2.value);
		}
		return dist;
	}


	/** get all individual entries from the underlying data-structure */
	const std::vector<Entry>& getEntries() const {
		return Base::getEntries();
	}



	/** smooth harsh angles */
	void smooth(float delta = 1, int numRuns = 1) {

		float t = delta*2;

		for (int j = 0; j < numRuns; ++j) {

			t/=2;

			for (int i = 1; i < (int)entries.size()-1; ++i) {

				// the entry to-be-replaced by two others
				const Entry& e = entries[i];
				const float key = e.key;

				const Entry e1(key-t, Base::get(key-t));
				const Entry e2(key+t, Base::get(key+t));

				entries.erase(entries.begin()+i); --i;
				++i; entries.insert(entries.begin()+i, e1);
				++i; entries.insert(entries.begin()+i, e2);

			}

		}

	}

//	/** smooth harsh angles */
//	void smooth(float delta = 1, int numRuns = 1) {

//		float t = delta/numRuns;

//		for (int i = 1; i < (int)entries.size()-1; ++i) {

//			// the entry to-be-replaced by several others
//			const Entry& e = entries[i];
//			const float key = e.key;

//			std::vector<Entry> newEntries;
//			for (int x = -numRuns; x <= +numRuns; x+= 2) {
//				const float percent = (float)x / (float)numRuns;
//				const float keyO = key + percent*t;
//				const Point3 pos1 = Base::get(keyO-t*2);
//				const Point3 pos2 = Base::get(keyO+t*2);
//				const Point3 posO = (pos1+pos2) / 2;
//				Entry e(keyO, posO);
//				newEntries.push_back(e);
//			}

//			entries.erase(entries.begin()+i); --i;

//			for (const Entry& e : newEntries) {
//				++i;
//				entries.insert(entries.begin()+i, e);
//			}

//		}

//	}

	/** get the position along the path after the given walking distance */
	Point3 getPosAfterDistance(const float distance) const {
		return Base::get(distance);
	}

	bool doneAtDistance(const float distance) const {
		return Base::getMaxKey() < distance;
	}

	/** at the given distance: are we walking on a plain surface or up/down? */
	bool isPlain(const float distance) const {
		const Point3 pos1 = getPosAfterDistance(distance);
		const Point3 pos2 = getPosAfterDistance(distance + 0.1);
		const float delta = std::abs(pos1.z - pos2.z);
		return delta < 0.01;
	}

};

#endif // INDOOR_SYNTEHTICPATH_H