diff --git a/lib/graph/graph.hpp b/lib/graph/graph.hpp
index 01d730c..08d58e7 100644
--- a/lib/graph/graph.hpp
+++ b/lib/graph/graph.hpp
@@ -1,5 +1,5 @@
-#ifndef GRAPHWD_HPP
-#define GRAPHWD_HPP
+#ifndef GRAPH_HPP
+#define GRAPH_HPP
 
 #include <unordered_map>
 #include <vector>
@@ -322,4 +322,4 @@ inline void Graph<V>::eraseEdge(edge_container& v, const_reference data) {
 }
 
 
-#endif // GRAPHWD_HPP
+#endif // GRAPH_HPP
diff --git a/lib/graph/graph_algorithms.hpp b/lib/graph/graph_algorithms.hpp
index 671a3a7..6271d9b 100644
--- a/lib/graph/graph_algorithms.hpp
+++ b/lib/graph/graph_algorithms.hpp
@@ -32,6 +32,8 @@ std::vector<V> pathFromPrevList(const V& dest, std::unordered_map<V, V> prev)
   std::vector<V> 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);
@@ -48,10 +50,9 @@ std::vector<V> pathFromPrevList(const V& dest, std::unordered_map<V, V> prev)
 template <typename V, typename W>
 std::vector<V>
 dijkstra_shortest_path_to(const Graph<V>& graph,
-                             const V& source,
-                             const V& dest,
-                             std::function<W(V, V)> distanceCompute
-                            )
+                          const V& source,
+                          const V& dest,
+                          std::function<W(V, V)> distanceCompute)
 {
   std::unordered_map<V, W> dist; /// @todo into std::priority_queue<std::pair<V< W>>
   std::unordered_map<V, V> prev;
@@ -71,14 +72,15 @@ dijkstra_shortest_path_to(const Graph<V>& graph,
       break;
 
     for (V v : graph.neighboursOf(u)) {
-      const bool newNode = dist.find(v) == dist.end();
+
       const W d = distanceCompute(u, v);
+      const W alt = dist.at(u) + d;
+      const bool newNode = dist.find(v) == dist.end();
       if (newNode) {
-        dist.emplace(v, d);
+        dist.emplace(v, alt);
         prev.emplace(v, u);
         q.insert(v);
       } else {
-        const W alt = dist.at(u) + d;
         const bool betterRoute = alt < dist.at(v);
         if (betterRoute) {
           dist[v] = alt;
diff --git a/test/graph/fixture.hpp b/test/graph/fixture.hpp
new file mode 100644
index 0000000..0adf3c1
--- /dev/null
+++ b/test/graph/fixture.hpp
@@ -0,0 +1,168 @@
+#ifndef GRAPH_TEST_FIXTURE_HPP
+#define GRAPH_TEST_FIXTURE_HPP
+
+#include <cmath>
+
+struct float2 {
+  float x, y;
+
+  inline float2() : x(0.0), y(0.0) {}
+  inline float2(float f1, float f2) : x(f1), y(f2) {}
+};
+
+inline bool operator ==(const float2& v1, const float2& v2) { return v1.x == v2.x && v1.y == v2.y; }
+inline float pow2(float f) { return f*f; }
+inline float dist(const float2& v1, const float2& v2) { return sqrt(pow2(v2.x - v1.x) + pow2(v2.y - v1.y)); }
+// inline float dist(const float2& v1, const float2& v2) { return abs(v2.x - v1.x) + abs(v2.y - v1.y); }
+
+namespace std {
+  template <>
+  struct hash<float2>
+  {
+    std::size_t operator()(const float2& f2) const
+    {
+      std::size_t h1 = std::hash<float>()(f2.x);
+      std::size_t h2 = std::hash<float>()(f2.y);
+      return h1 ^ (h2 << 1);
+    }
+  };
+
+  class distanceOf2float2s : public std::function<float(float2, float2)>
+  {
+  public:
+    float operator()(float2 a, float2 b) { return dist(a, b); }
+  };
+}
+
+constexpr std::size_t numberOfEdges(std::size_t number_of_rows, std::size_t number_of_columns) {
+  return (number_of_rows-2)*(number_of_columns-2) *8                // inside vertices have 8
+          + 4 *3                                                    // corners have 3
+          + ((number_of_rows-2)*2 + (number_of_columns-2)*2) *5;    // sides has 5
+}
+
+template <typename V>
+std::vector<V>* createVertices(std::size_t number_of_rows, std::size_t number_of_columns) {
+  std::vector<V>* retval = new std::vector<V>();
+  const std::size_t number_of_edges = number_of_rows * number_of_columns;
+  retval->reserve(number_of_edges);
+
+  for (std::size_t c = 0; c < number_of_columns; ++c)
+    for (std::size_t r = 0; r < number_of_rows; ++r)
+      retval->push_back(V(r, c));
+
+  return retval;
+}
+
+template <typename V>
+std::vector<typename Graph<V>::Edge>* createEdges(std::size_t number_of_rows, std::size_t number_of_columns) {
+  std::vector<typename Graph<V>::Edge>* retval = new std::vector<typename Graph<V>::Edge>();
+  const std::size_t number_of_edges = numberOfEdges(number_of_rows, number_of_columns);
+  retval->reserve(number_of_edges);
+
+  // inside
+  for (std::size_t c = 1; c < number_of_columns-1; ++c)
+    for (std::size_t r = 1; r < number_of_rows-1; ++r) {
+      retval->push_back(typename Graph<V>::Edge(float2(r, c), float2(r-1, c-1)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, c), float2(r-1, c)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, c), float2(r-1, c+1)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, c), float2(r,   c-1)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, c), float2(r,   c+1)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, c), float2(r+1, c-1)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, c), float2(r+1, c)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, c), float2(r+1, c+1)));
+    }
+
+    // top & bottom row
+    for (std::size_t r = 1; r < number_of_rows-1; ++r) {
+      retval->push_back(typename Graph<V>::Edge(float2(r, 0), float2(r-1, 0)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, 0), float2(r+1, 0)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, 0), float2(r-1, 1)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, 0), float2(r,   1)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, 0), float2(r+1, 1)));
+
+      retval->push_back(typename Graph<V>::Edge(float2(r, number_of_columns-1), float2(r-1, number_of_columns-1)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, number_of_columns-1), float2(r+1, number_of_columns-1)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, number_of_columns-1), float2(r-1, number_of_columns-1-1)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, number_of_columns-1), float2(r,   number_of_columns-1-1)));
+      retval->push_back(typename Graph<V>::Edge(float2(r, number_of_columns-1), float2(r+1, number_of_columns-1-1)));
+    }
+
+    // left & right column
+    for (std::size_t c = 1; c < number_of_columns-1; ++c) {
+      retval->push_back(typename Graph<V>::Edge(float2(0, c), float2(0, c-1)));
+      retval->push_back(typename Graph<V>::Edge(float2(0, c), float2(0, c+1)));
+      retval->push_back(typename Graph<V>::Edge(float2(0, c), float2(1, c-1)));
+      retval->push_back(typename Graph<V>::Edge(float2(0, c), float2(1, c)));
+      retval->push_back(typename Graph<V>::Edge(float2(0, c), float2(1, c+1)));
+
+      retval->push_back(typename Graph<V>::Edge(float2(number_of_rows-1, c), float2(number_of_rows-1,   c-1)));
+      retval->push_back(typename Graph<V>::Edge(float2(number_of_rows-1, c), float2(number_of_rows-1,   c+1)));
+      retval->push_back(typename Graph<V>::Edge(float2(number_of_rows-1, c), float2(number_of_rows-1-1, c-1)));
+      retval->push_back(typename Graph<V>::Edge(float2(number_of_rows-1, c), float2(number_of_rows-1-1, c)));
+      retval->push_back(typename Graph<V>::Edge(float2(number_of_rows-1, c), float2(number_of_rows-1-1, c+1)));
+    }
+
+  // corners
+  retval->push_back(typename Graph<V>::Edge(float2(0, 0), float2(0, 1)));
+  retval->push_back(typename Graph<V>::Edge(float2(0, 0), float2(1, 0)));
+  retval->push_back(typename Graph<V>::Edge(float2(0, 0), float2(1, 1)));
+
+  retval->push_back(typename Graph<V>::Edge(float2(number_of_rows-1, 0), float2(number_of_rows-2, 0)));
+  retval->push_back(typename Graph<V>::Edge(float2(number_of_rows-1, 0), float2(number_of_rows-2, 1)));
+  retval->push_back(typename Graph<V>::Edge(float2(number_of_rows-1, 0), float2(number_of_rows-1,   1)));
+
+  retval->push_back(typename Graph<V>::Edge(float2(0, number_of_columns-1), float2(0, number_of_columns-2)));
+  retval->push_back(typename Graph<V>::Edge(float2(0, number_of_columns-1), float2(1, number_of_columns-2)));
+  retval->push_back(typename Graph<V>::Edge(float2(0, number_of_columns-1), float2(1, number_of_columns-1)));
+
+  retval->push_back(typename Graph<V>::Edge(float2(number_of_rows-1, number_of_columns-1), float2(number_of_rows-1, number_of_columns-2)));
+  retval->push_back(typename Graph<V>::Edge(float2(number_of_rows-1, number_of_columns-1), float2(number_of_rows-2, number_of_columns-1)));
+  retval->push_back(typename Graph<V>::Edge(float2(number_of_rows-1, number_of_columns-1), float2(number_of_rows-2, number_of_columns-2)));
+
+  return retval;
+}
+
+/** Creating a big (number_of_rows rows and number_of_columns columns) grid.
+ *  Every vertex is connexted to it's 8 neighbours.
+ *
+ *  +-+-+-+
+ *  |x+x+x|
+ *  +x+x+x+
+ *  |x+x+x|
+ *  +-+-+-+
+ */
+template <typename V>
+class Fixture {
+public:
+
+  static void initOnce(std::size_t number_of_rows, std::size_t number_of_columns) {
+    if (m_initialized)
+      return;
+
+    m_vertices = createVertices<float2>(number_of_rows, number_of_columns);
+    m_edges = createEdges<float2>(number_of_rows, number_of_columns);
+    m_initialized = true;
+  }
+
+  static void tearDown() {
+    delete m_vertices;
+    m_vertices = 0;
+    delete m_edges;
+    m_edges = 0;
+    m_initialized = false;
+  }
+
+  static const std::vector<V> getVertices() { return *m_vertices; }
+  static const std::vector<typename Graph<V>::Edge> getEdges() { return *m_edges; }
+
+private:
+  static bool m_initialized;
+  static std::vector<V>* m_vertices;
+  static std::vector<typename Graph<V>::Edge>* m_edges;
+};
+
+template<class V> bool Fixture<V>::m_initialized = false;
+template<class V> std::vector<V>* Fixture<V>::m_vertices = 0;
+template<class V> std::vector<typename Graph<V>::Edge>* Fixture<V>::m_edges = 0;
+
+#endif // GRAPH_TEST_FIXTURE_HPP
diff --git a/test/graph/test_graph.hpp b/test/graph/test_graph.hpp
index 08adff2..15fe8cf 100644
--- a/test/graph/test_graph.hpp
+++ b/test/graph/test_graph.hpp
@@ -2,6 +2,8 @@
 
 #include "../catch.hpp"
 
+#include "fixture.hpp"
+
 TEST_CASE( "Graph creation", "[graph][data_structure]" ) {
 
   SECTION("Initial state") {
@@ -184,6 +186,16 @@ TEST_CASE( "Graph adding vertices", "[graph][data_structure]" ) {
     }
     REQUIRE( found.size() == 3 );
   }
+
+  SECTION("neighboursOf") {
+    Graph<int> g = { {1, 2}, {1, 3}, {1, 4}, {2, 3}, {2, 4}, {3, 4} };
+    REQUIRE( numberOfVertices(g) == 4 );
+    REQUIRE( numberOfEdges(g) == 4*3 );
+    const std::vector<int> n = g.neighboursOf(3);
+    REQUIRE( std::find(n.begin(), n.end(), 1) != n.end() );
+    REQUIRE( std::find(n.begin(), n.end(), 2) != n.end() );
+    REQUIRE( std::find(n.begin(), n.end(), 4) != n.end() );
+  }
 }
 
 TEST_CASE( "Graph adding edges", "[graph][data_structure]" ) {
@@ -523,3 +535,30 @@ TEST_CASE( "Graph adding/removing with more data", "[graph][data_structure]" ) {
   }
 }
 
+
+TEST_CASE_METHOD(Fixture<float2>, "Graph performance", "[graph][data_structure][performance]" ) {
+
+  constexpr std::size_t number_of_rows = 100;
+  constexpr std::size_t number_of_columns = 100;
+  constexpr std::size_t number_of_vertices = number_of_rows * number_of_columns;
+  constexpr std::size_t number_of_edges = numberOfEdges(number_of_rows, number_of_columns);
+
+  Fixture<float2>::initOnce(number_of_rows, number_of_columns);
+
+
+  SECTION("Adding vertices") {
+    const std::vector<float2> vertices = Fixture<float2>::getVertices();
+    Graph<float2> g(vertices);
+    REQUIRE( numberOfVertices(g) == number_of_vertices );
+  }
+
+  SECTION("Adding edges") {
+    const std::vector<typename Graph<float2>::Edge> edges = Fixture<float2>::getEdges();
+    Graph<float2> g(edges);
+  }
+
+  SECTION("teardown") {
+    Fixture<float2>::tearDown();
+  }
+}
+
diff --git a/test/graph/test_graph_algorithms.hpp b/test/graph/test_graph_algorithms.hpp
new file mode 100644
index 0000000..4db961c
--- /dev/null
+++ b/test/graph/test_graph_algorithms.hpp
@@ -0,0 +1,89 @@
+#include <graph/graph.hpp>
+#include <graph/graph_algorithms.hpp>
+
+#include "../catch.hpp"
+
+#include "fixture.hpp"
+
+void printPath(std::size_t number_of_rows,
+               std::size_t number_of_columns,
+               float2 source,
+               float2 destination,
+               const std::vector<float2>& path) {
+
+  std::cout << "Path size: " << path.size()  << std::endl;
+
+  std::cout << "Edges: ";
+  for (const auto& e : path)
+    std::cout << "(" << e.x << "," << e.y << ") ";
+  std::cout << std::endl;
+
+  for (std::size_t c = 0; c < number_of_columns; ++c) {
+    for (std::size_t r = 0; r < number_of_rows; ++r) {
+      const float2 p(r, c);
+      if (p == source)
+        std::cout << "s ";
+      else if (p == destination)
+        std::cout << "d ";
+      else {
+        const bool found = std::find(path.begin(), path.end(), p) != path.end();
+        std::cout << (found ? "1 " : ". ");
+      }
+    }
+    std::cout << std::endl;
+  }
+}
+
+
+TEST_CASE("Graph algorithms, small", "[graph][algorithm][dijkstra]" ) {
+
+  SECTION("Simple") {
+    constexpr std::size_t number_of_rows = 3;
+    constexpr std::size_t number_of_columns = 3;
+    const std::vector<typename Graph<float2>::Edge>* edges = createEdges<float2>(number_of_rows, number_of_columns);
+    Graph<float2> g(*edges);
+
+    const float2 source(0, 0);
+    const float2 destination(2, 2);
+    const std::vector<float2> shortestPath = dijkstra_shortest_path_to<float2, float>(g, source, destination, std::distanceOf2float2s());
+
+    const float euclidan_distance = sqrt(pow(source.x - destination.x, 2) + pow(source.y - destination.y, 2));
+    REQUIRE( std::distanceOf2float2s()(source, destination) == euclidan_distance );
+
+    REQUIRE( shortestPath.size() == 3 );
+    REQUIRE( shortestPath[0] == float2(0, 0) );
+    REQUIRE( shortestPath[1] == float2(1, 1) );
+    REQUIRE( shortestPath[2] == float2(2, 2) );
+
+    delete edges;
+  }
+
+}
+
+TEST_CASE_METHOD(Fixture<float2>, "Graph algorithms, big graph", "[graph][algorithm][dijkstra]" ) {
+
+  constexpr std::size_t number_of_rows = 100;
+  constexpr std::size_t number_of_columns = number_of_rows;
+  constexpr std::size_t number_of_vertices = number_of_rows * number_of_columns;
+  constexpr std::size_t number_of_edges = numberOfEdges(number_of_rows, number_of_columns);
+
+  Fixture<float2>::initOnce(number_of_rows, number_of_columns);
+
+  SECTION("Simple") {
+    const std::vector<typename Graph<float2>::Edge> edges = Fixture<float2>::getEdges();
+    Graph<float2> g(edges);
+
+    const float2 source(0, 0);
+    const float2 destination(number_of_rows-1, number_of_columns-1);
+    const std::vector<float2> shortestPath = dijkstra_shortest_path_to<float2, float>(g, source, destination, std::distanceOf2float2s());
+
+    REQUIRE( shortestPath.size() == number_of_rows);
+    for (std::size_t i = 0; i < number_of_rows; ++i)
+      REQUIRE( shortestPath[i] == float2(i, i) );
+  }
+
+  SECTION("teardown") {
+    Fixture<float2>::tearDown();
+  }
+
+}
diff --git a/test/test_main.cpp b/test/test_main.cpp
index 38c1097..f465b9d 100644
--- a/test/test_main.cpp
+++ b/test/test_main.cpp
@@ -1,4 +1,5 @@
 #define CATCH_CONFIG_MAIN
 #include "catch.hpp"
 
-#include "graph/test_graph.hpp"
\ No newline at end of file
+#include "graph/test_graph.hpp"
+#include "graph/test_graph_algorithms.hpp"
\ No newline at end of file