#ifndef DIJKSTRAPATH_H
#define DIJKSTRAPATH_H

#include "DijkstraStructs.h"

/**
 * describes a dijkstra-generated path between end and start.
 * allows KNN searches for points within this path.
 *
 */
template <typename T> class DijkstraPath {

private:

	/** the constructed path */
	std::vector<DijkstraNode<T>*> path;

public:

	/** ctor from end- to start-node */
	DijkstraPath(DijkstraNode<T>* end, DijkstraNode<T>* start) {

		// follow the path from the end to the start
		DijkstraNode<T>* curNode = end;
		while (curNode != start) {
			path.push_back(curNode);
			curNode = curNode->previous;
		}

	}

	/** NANOFLANN: number of elements in the path */
	inline int kdtree_get_point_count() const {
		return path.size();
	}

	/** NANOFLANN: use default bbox */
	template <class BBOX> inline bool kdtree_get_bbox(BBOX& bb) const {
		(void) bb; return false;
	}

	/** NANOFLANN: get the idx-th elements dim-th dimension */
	inline float kdtree_get_pt(const size_t idx, const int dim) const {
		return (*(path[idx]->element))[dim];
	}

	/** NANOFLANN: get the distance between the given point and the idx-th element */
	inline float kdtree_distance(const float* p1, const size_t idx_p2, size_t) const {
		const DijkstraNode<T>* n = path[idx_p2];
		const float d0 = p1[0] - (*(n->element))[0];
		const float d1 = p1[1] - (*(n->element))[1];
		const float d2 = p1[2] - (*(n->element))[2];
		return (d0*d0) + (d1*d1) + (d2*d2);
	}

};

#endif // DIJKSTRAPATH_H