diff --git a/lib/graph/graph_algorithms.hpp b/lib/graph/graph_algorithms.hpp
index 6271d9b..3042a9d 100644
--- a/lib/graph/graph_algorithms.hpp
+++ b/lib/graph/graph_algorithms.hpp
@@ -21,7 +21,8 @@ inline V closestNode(const std::unordered_set<V>& q, const std::unordered_map<V,
   const typename std::unordered_set<V>::const_iterator smallest_it =
     std::min_element(q.begin(), q.end(),
       [&dist](const V& v1, const V& v2)
-      { return !(dist.find(v2) != dist.end() && ( (dist.find(v1) == dist.end()) || (dist.at(v1) > dist.at(v2)))); } );
+      { return !(  dist.find(v2) != dist.end() &&
+                   ((dist.find(v1) == dist.end()) || (dist.at(v1) > dist.at(v2)))  ); } );
 
   return *smallest_it;
 }
@@ -30,15 +31,14 @@ template <typename V>
 std::vector<V> pathFromPrevList(const V& dest, std::unordered_map<V, V> prev)
 {
   std::vector<V> retval;
+  if (prev.find(dest) == prev.end())
+    return retval;
 
-  retval.push_back(dest);
-
-  /// @bug This can be an endless loop
-  for (V it = dest; prev.find(it) != prev.end() ; /*it = prev.at(it)*/) {
-    V v = prev.at(it);
-    retval.push_back(v);
-    it = v;
+  V it = dest;
+  for (; prev.find(it) != prev.end() ; it = prev.at(it)) {
+    retval.push_back(it);
   }
+  retval.push_back(it);
 
   std::reverse(retval.begin(), retval.end());
   return retval;
@@ -61,7 +61,6 @@ dijkstra_shortest_path_to(const Graph<V>& graph,
 
   std::unordered_set<V> q;
   q.insert(source);
-
   const std::vector<V>& s_n = graph.neighboursOf(source);
   std::copy(s_n.begin(), s_n.end(), std::inserter(q, q.end()));
 
@@ -72,20 +71,16 @@ dijkstra_shortest_path_to(const Graph<V>& graph,
       break;
 
     for (V v : graph.neighboursOf(u)) {
-
       const W d = distanceCompute(u, v);
       const W alt = dist.at(u) + d;
-      const bool newNode = dist.find(v) == dist.end();
-      if (newNode) {
+
+      if (dist.find(v) == dist.end()) { // new node
         dist.emplace(v, alt);
         prev.emplace(v, u);
         q.insert(v);
-      } else {
-        const bool betterRoute = alt < dist.at(v);
-        if (betterRoute) {
-          dist[v] = alt;
-          prev[v] = u;
-        }
+      } else if (alt < dist.at(v)) { // better route
+        dist[v] = alt;
+        prev[v] = u;
       }
     }
   }
diff --git a/test/graph/fixture.hpp b/test/graph/fixture.hpp
index 0adf3c1..3581d37 100644
--- a/test/graph/fixture.hpp
+++ b/test/graph/fixture.hpp
@@ -32,6 +32,11 @@ namespace std {
   public:
     float operator()(float2 a, float2 b) { return dist(a, b); }
   };
+  class distanceOf2ints : public std::function<int(int, int)>
+  {
+  public:
+    float operator()(int a, int b) { return abs(a-b); }
+  };
 }
 
 constexpr std::size_t numberOfEdges(std::size_t number_of_rows, std::size_t number_of_columns) {
diff --git a/test/graph/test_graph_algorithms.hpp b/test/graph/test_graph_algorithms.hpp
index 4db961c..3c4a257 100644
--- a/test/graph/test_graph_algorithms.hpp
+++ b/test/graph/test_graph_algorithms.hpp
@@ -37,6 +37,45 @@ void printPath(std::size_t number_of_rows,
 
 TEST_CASE("Graph algorithms, small", "[graph][algorithm][dijkstra]" ) {
 
+  SECTION("distance") {
+      const float2 source(1, 2);
+      const float2 destination(12, 13);
+      const float euclidan_distance = sqrt(pow(source.x - destination.x, 2) + pow(source.y - destination.y, 2));
+      REQUIRE( std::distanceOf2float2s()(source, destination) == euclidan_distance );
+    }
+
+  SECTION("empty graph") {
+    Graph<int> g;
+    const int source(0);
+    const int destination(1);
+    const std::vector<int> shortestPath = dijkstra_shortest_path_to<int, int>(g, source, destination, std::distanceOf2ints());
+    REQUIRE( shortestPath.size() == 0 );
+  }
+
+  SECTION("nonexisting destination") {
+    Graph<int> g = { {1, 2}, {1, 3}, {1, 4}, {2, 4}, {3, 4} };
+    const int source(1);
+    const int destination(10);
+    const std::vector<int> shortestPath = dijkstra_shortest_path_to<int, int>(g, source, destination, std::distanceOf2ints());
+    REQUIRE( shortestPath.size() == 0 );
+  }
+
+  SECTION("nonexisting source") {
+    Graph<int> g = { {1, 2}, {1, 3}, {1, 4}, {2, 4}, {3, 4} };
+    const int source(10);
+    const int destination(1);
+    const std::vector<int> shortestPath = dijkstra_shortest_path_to<int, int>(g, source, destination, std::distanceOf2ints());
+    REQUIRE( shortestPath.size() == 0 );
+  }
+
+  SECTION("not connected source and destination") {
+    Graph<int> g = { {1, 2}, {3, 4} };
+    const int source(1);
+    const int destination(4);
+    const std::vector<int> shortestPath = dijkstra_shortest_path_to<int, int>(g, source, destination, std::distanceOf2ints());
+    REQUIRE( shortestPath.size() == 0 );
+  }
+
   SECTION("Simple") {
     constexpr std::size_t number_of_rows = 3;
     constexpr std::size_t number_of_columns = 3;