Indoor/nav/dijkstra/Dijkstra.h

#ifndef DIJKSTRA_H
#define DIJKSTRA_H

#include <cmath>
#include <vector>
#include <algorithm>
#include <unordered_set>
#include <unordered_map>

#include <list>
#include <set>

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

#include "../../Assertions.h"

template <typename T> class Dijkstra {

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

public:

	/** dtor: cleanup */
	~Dijkstra() {
		for (auto it : nodes) {delete it.second;}
	}

	/** get the dijkstra-pendant for the given user-node. null if none matches */
	inline const DijkstraNode<T>* getNode(const T& userNode) const {
		return getNode(&userNode);
	}

	/** get the dijkstra-pendant for the given user-node. null if none matches */
	inline const DijkstraNode<T>* getNode(const T* userNode) const {
		auto it = nodes.find(userNode);
		return (unlikely(it == nodes.end())) ? (nullptr) : (it->second);
	}

	/** get all constructed dijkstra-nodes and their original pendant */
	inline const std::unordered_map<const T*, DijkstraNode<T>*>& getNodes() const {
		return nodes;
	}

	/** calculate all shortest paths from ANY node to the given destination */
	template <typename Access> void build(const T* end, const Access& acc) {
		build(end, nullptr, acc, NAN);
	}

	/**
	 * build the shortest path from start to end using the provided access-wrapper-class.
	 * if end is null, the algorithm will terminate only if every possible node was checked.
	 * if given, the algorithm will also terminate if the current distance is already > the given maximum
	 */
	template <typename Access> void build(const T* start, const T* end, const Access& acc, const float maxWeight = 0) {

		// NOTE: end is currently ignored!
		// runs until all nodes were evaluated
		(void) end;

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

		// cleanup previous runs
		nodes.clear();

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

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

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

		// add this node to the processing list (and mark it as "enqueued")
		toBeProcessedNodes.addOnce(dnStart, dnStart->cumWeight);

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

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

			// when an end is given, stop when end was reached
			if (end != nullptr && dnSrc->element == end) {Log::add("Dijkstra", "reached target node"); break;}

			// when a maximum weight is given, stop when current cum-dist > maxWeight
			if (maxWeight != 0 && dnSrc->cumWeight > maxWeight) {Log::add("Dijkstra", "reached weight limit: " + std::to_string(maxWeight)); break;}

			// visit (and maybe update) 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 DijkstraNode for the neighbor
				DijkstraNode<T>* dnDst = getOrCreateNode(dst);

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

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

					// re-sort (weight has changed) within the list of to-be-processed nodes
					toBeProcessedNodes.addOrUpdate(dnDst, dnDst->cumWeight, potentialWeight);
					dnDst->cumWeight = potentialWeight;
					dnDst->previous = dnSrc;

				} else {

					// if this neighbor was encountered for the first time, add it (and mark it as "enqueued")
					toBeProcessedNodes.addOnce(dnDst, dnDst->cumWeight);

				}

			}

		}

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

	}

private:

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

		/** assign a weight to a node and provide the corresponding comparator */
		struct WeightedNode {

			DijkstraNode<T>* dn;
			const float weight;

			/** ctor */
			WeightedNode(DijkstraNode<T>* dn, const float weight) : dn(dn), weight(weight) {;}

			/** compare by weight. same weight? : compare by pointer */
			bool operator < (const WeightedNode& wn) const	{
				return (weight != wn.weight) ? (weight < wn.weight) : (dn < wn.dn);
			}

		};

		/** sorted list of to-be-processed nodes */
		std::set<WeightedNode> nodes;

	public:

		/** add a new to-be-processed node */
		void addOnce(DijkstraNode<T>* node, const float weight) {

			// skip nodes that were already enqueued
			if (node->enqueued) {return;}

			// add the combination (node+weight)
			nodes.insert(WeightedNode(node, weight));

			// mark the node as processed
			node->enqueued = true;

		}

		/** add a new to-be-processed node or update its old weight to the new one */
		void addOrUpdate(DijkstraNode<T>* node, const float oldWeight, const float newWeight) {

			// find and remove the previous combination (node+weight) if any
			const auto old = nodes.find(WeightedNode(node, oldWeight));
			if (old != nodes.end()) {nodes.erase(old);}

			// add the new combination (node+weight)
			nodes.insert(WeightedNode(node, newWeight));

			// mark the node as processed
			node->enqueued = true;

		}

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

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

	};




	/** get (or create) a new node for the given user-node */
	inline DijkstraNode<T>* getOrCreateNode(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