/*
 * © Copyright 2014 – Urheberrechtshinweis
 * Alle Rechte vorbehalten / All Rights Reserved
 *
 * Programmcode ist urheberrechtlich geschuetzt.
 * Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
 * Keine Verwendung ohne explizite Genehmigung.
 * (vgl. § 106 ff UrhG / § 97 UrhG)
 */

#ifndef DIJKSTRAPATH_H
#define DIJKSTRAPATH_H

#include "DijkstraStructs.h"
#include <vector>

#include "../../Assertions.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<const DijkstraNode<T>*> path;

public:

	/** empty ctor */
	DijkstraPath() {
		;
	}

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

		// sanity checks
		Assert::isNotNull(end, "end-node must not be null");
		Assert::isNotNull(start, "start-node must not be null");

		// follow the path from the end to the start
		const DijkstraNode<T>* curNode = end;
		while (curNode != start) {

			// sanity check in case no path between start and end exists
			Assert::isNotNull(curNode, "there is not path between start and end. did you accidentially swap start and end?");

			// append
			path.push_back(curNode);
			curNode = curNode->previous;

		}

	}

	/** allow iteration */
	decltype(path.begin()) begin()								{return path.begin();}
	decltype(path.end()) end()									{return path.end();}
//	decltype(path.begin()) begin() const						{return ((std::vector<const DijkstraNode<T>*>)path).begin();}
//	decltype(path.end()) end() const							{return ((std::vector<const DijkstraNode<T>*>)path).end();}
	const DijkstraNode<T>& operator [] (const int idx) const	{return *(path[idx]);}
	size_t size() const											{return path.size();}

	/** get the idx-th element from the starting-node. if this is beyond the end, the last node is returned */
	const DijkstraNode<T>& getFromStart(const int idx) const	{return (idx >= (int) path.size()) ? (*path.back()) : (*path[idx]);}

	const std::vector<const DijkstraNode<T>*>& getVector() const {return path;}

	/** 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