Indoor/nav/dijkstra/Dijkstra.h

#ifndef DIJKSTRA_H
#define DIJKSTRA_H


#include <vector>
#include <algorithm>
#include <unordered_set>
#include <list>
#include <set>

#include "DijkstraStructs.h"
#include "../../misc/Debug.h"
#include "../../misc/Time.h"
#include "../../Defines.h"

#include <KLib/Assertions.h>

template <typename T> class Dijkstra {

	/** all allocated nodes for the user-data inputs */
	std::unordered_map<const T*, DijkstraNode<T>*> nodes;

public:

	/** get the dijkstra-pendant for the given user-node */
	DijkstraNode<T>* getNode(const T& userNode) {
		return nodes[&userNode];
	}

	/** build shortest path from start to end using the provided wrapper-class */
	template <typename Access> void build(const T& start, const T& end, const Access& acc) {

		// NOTE: end is currently ignored!
		// runs until all nodes were evaluated

		Log::add("Dijkstra", "calculating dijkstra from " + (std::string)start + " to ALL OTHER nodes", false);
		Log::tick();

		// cleanup previous runs
		nodes.clear();

		// sorted list of all to-be-processed nodes
		ToProcess toBeProcessedNodes;

		// all already processed edges
		std::unordered_set<decltype(getEdge(nullptr,nullptr))> usedEdges;

		// run from start
		const T* cur = &start;

		// create a node for the start element
		DijkstraNode<T>* dnStart = getNode(cur);
		dnStart->cumWeight = 0;

		// add this node to the processing list
		toBeProcessedNodes.add(dnStart);

		// until we are done
		while(unlikely(!toBeProcessedNodes.empty())) {

			// get the next to-be-processed node
			DijkstraNode<T>* dnSrc = toBeProcessedNodes.pop();

			// stop when end was reached??
			//if (dnSrc->element == &end) {break;}

			// process each neighbor of the current element
			for (int i = 0; i < acc.getNumNeighbors(*dnSrc->element); ++i) {

				// get the neighbor itself
				const T* dst = acc.getNeighbor(*dnSrc->element, i);

				// get-or-create a node for the neighbor
				DijkstraNode<T>* dnDst = getNode(dst);

				// get-or-create the edge describing the connection
				//const DijkstraEdge<T> edge = getEdge(dnSrc, dnDst);
				const auto edge = getEdge(dnSrc, dnDst);

				// was this edge already processed? -> skip it
				if (usedEdges.find(edge) != usedEdges.end()) {continue;}

				// otherwise: remember it
				usedEdges.insert(edge);

				// and add the node for later processing
				//toBeProcessedNodes.push_back(dnDst);
				toBeProcessedNodes.add(dnDst);

				// get the distance-weight to the neighbor
				const float weight = acc.getWeightBetween(*dnSrc->element, *dst);
				_assertTrue(weight >= 0, "edge-weight must not be negative!");

				// update the weight to the destination?
				const float potentialWeight = dnSrc->cumWeight + weight;
				if (potentialWeight < dnDst->cumWeight) {
					dnDst->cumWeight = potentialWeight;
					dnDst->previous = dnSrc;
				}

			}

		}

		Log::tock();
		Log::add("Dijkstra", "processed " + std::to_string(nodes.size()) + " nodes");

	}

private:

	/** helper class to sort to-be-processed nodes by their distance from the start */
	class ToProcess {

		/** sort comparator */
		struct setComp {
			bool operator() (const DijkstraNode<T>* dn1, const DijkstraNode<T>* dn2) {
				return dn1->cumWeight < dn2->cumWeight;
			}
		};

		/** sorted list of to-be-processed nodes */
		std::set<DijkstraNode<T>*, setComp> toBeProcessedNodes;

	public:

		/** add a new to-be-processed node */
		void add(DijkstraNode<T>* node) {toBeProcessedNodes.insert(node);}

		/** get the next to-be-processed node (smallest distance) */
		DijkstraNode<T>* pop() {
			DijkstraNode<T>* next = *toBeProcessedNodes.begin();
			toBeProcessedNodes.erase(toBeProcessedNodes.begin());
			return next;
		}

		/** set empty? */
		bool empty() const {return toBeProcessedNodes.empty();}

	};




	/** get (or create) a new node for the given user-node */
	inline DijkstraNode<T>* getNode(const T* userNode) {
		auto it = nodes.find(userNode);
		if (unlikely(it == nodes.end())) {
			DijkstraNode<T>* dn = new DijkstraNode<T>(userNode);
			nodes[userNode] = dn;
			return dn;
		} else {
			return it->second;
		}
	}

	/** get the edge (bi-directional) between the two given nodes */
	inline DijkstraEdge<T> getEdge(const DijkstraNode<T>* n1, const DijkstraNode<T>* n2) const {
		return DijkstraEdge<T>(n1, n2);
	}

};

#endif // DIJKSTRA_H