diff --git a/src/main.cpp b/src/main.cpp
index 07596cd..e4007cf 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -30,14 +30,17 @@ int main(){
     n3.setValue(3);
     GraphNode n4 = GraphNode();
     n4.setValue(4);
+    GraphNode n5 = GraphNode();
+    n5.setValue(5);
 
-    n1.addNeighbours(&n2,7);
+    n1.addNeighbours(&n2,6);
     n1.addNeighbours(&n3,3);
     n2.addNeighbours(&n4,2);
     n3.addNeighbours(&n4,20);
     n3.addNeighbours(&n2,2);
+    n3.addNeighbours(&n5,10);
 
-    std::vector<GraphNode*> path = PathFinder::a_star(&n1,&n4);
+    std::vector<GraphNode*> path = PathFinder::a_star(&n1,&n4,true);
     std::cout << "Path Size: " << path.size() << std::endl;
     for(GraphNode* gn: path){
         std::cout << gn->getValue() << std::endl;
diff --git a/src/tools/graph.cpp b/src/tools/graph.cpp
index 7782502..84ef650 100644
--- a/src/tools/graph.cpp
+++ b/src/tools/graph.cpp
@@ -57,8 +57,6 @@ float GraphNode::getPriority(){
 }
 
 
-
-
 GraphEdge::GraphEdge(GraphNode* gn,float v):m_target(gn),m_weight(v)
 {}
 
diff --git a/src/tools/graph.h b/src/tools/graph.h
index b6abb2f..0ac4c37 100644
--- a/src/tools/graph.h
+++ b/src/tools/graph.h
@@ -22,7 +22,7 @@ public:
     int getNbNeighbours();
     virtual float heuristic(GraphNode*){
         std::cout << "WARNING : You're using the default heuristic. For better result, redefine the heuristic function in GraphNode." << std::endl;
-        return 1;
+        return 0;
     };
 
     void setValue(int);
diff --git a/src/tools/pathfinder.cpp b/src/tools/pathfinder.cpp
index 1099a98..dcbd6a1 100644
--- a/src/tools/pathfinder.cpp
+++ b/src/tools/pathfinder.cpp
@@ -3,6 +3,8 @@
 #include <map>
 #include <queue>
 #include <algorithm>
+#include <set>
+
 //#include "fiboheap.h"
 
 PathFinder::PathFinder(){
@@ -15,6 +17,8 @@ std::vector<GraphNode*> PathFinder::a_star(GraphNode* start,GraphNode* goal,bool
     std::map<GraphNode*, float> cost = std::map<GraphNode*, float>(); //cost of visited node
     std::map<GraphNode*, GraphNode*> pred = std::map<GraphNode*, GraphNode*>(); //pred of visited node
 
+    std::set<GraphNode*> visited = std::set<GraphNode*>();
+
     // init frontier, cost, and pred with value for start
     frontier.push(start);
     cost.insert(std::pair<GraphNode*,float>(start,0));
@@ -26,6 +30,10 @@ std::vector<GraphNode*> PathFinder::a_star(GraphNode* start,GraphNode* goal,bool
         current = frontier.top();
         //current = pn->node.first;
         frontier.pop();
+        if (visited.count(current) == 0)
+            visited.insert(current);
+        else
+            continue;
 
         if(debug) std::cout << "Exploring node " << current->getValue() << std::endl;
         // goal reached, end of a-star
@@ -38,8 +46,7 @@ std::vector<GraphNode*> PathFinder::a_star(GraphNode* start,GraphNode* goal,bool
             float new_cost = cost[current] + next.getWeight();
             if(debug) std::cout << "\tExploring neighbours node " << next.getTarget()->getValue() << " with cost " << new_cost << std::endl;
 
-            bool never_visited = cost.count(next.getTarget()) == 0;
-            if ((never_visited) || (new_cost < cost[next.getTarget()])){
+            if ((cost.count(next.getTarget()) == 0) || (new_cost < cost[next.getTarget()])){
                 // affect processed cost to next node in cost_map
                 cost[next.getTarget()] = new_cost;
 
@@ -47,11 +54,10 @@ std::vector<GraphNode*> PathFinder::a_star(GraphNode* start,GraphNode* goal,bool
                 float priority = new_cost + next.getTarget()->heuristic(goal);
                 if(debug) std::cout << "\t\t Priority: " << priority << std::endl;
                 next.getTarget()->setPriority(priority);
-                if(never_visited)
-                    frontier.push(next.getTarget());
-
+                visited.erase(next.getTarget());
+                frontier.push(next.getTarget());
                 // memorize predecessor for next
-                 pred[next.getTarget()] = current;
+                pred[next.getTarget()] = current;
              }
         }
     }
@@ -69,3 +75,9 @@ std::vector<GraphNode*> PathFinder::a_star(GraphNode* start,GraphNode* goal,bool
 }
 
 
+bool ComparePriority::operator ()(GraphNode* a, GraphNode* b)
+{
+        return a->getPriority() > b->getPriority();
+}
+
+
diff --git a/src/tools/pathfinder.h b/src/tools/pathfinder.h
index 47e75d6..faeaf94 100644
--- a/src/tools/pathfinder.h
+++ b/src/tools/pathfinder.h
@@ -13,9 +13,6 @@ public:
 
 class ComparePriority{
 public:
-    bool operator() (GraphNode* a,GraphNode* b)
-    {
-        return a->getPriority() > b->getPriority();
-    }
+    bool operator() (GraphNode*,GraphNode*s);
 };
 #endif // PATHFINDER_H