diff --git a/src/main.cpp b/src/main.cpp
index 035941c..07596cd 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -31,10 +31,11 @@ int main(){
     GraphNode n4 = GraphNode();
     n4.setValue(4);
 
-    n1.addNeighbours(&n2,2);
+    n1.addNeighbours(&n2,7);
     n1.addNeighbours(&n3,3);
-    n2.addNeighbours(&n3,5);
-    n3.addNeighbours(&n4,2);
+    n2.addNeighbours(&n4,2);
+    n3.addNeighbours(&n4,20);
+    n3.addNeighbours(&n2,2);
 
     std::vector<GraphNode*> path = PathFinder::a_star(&n1,&n4);
     std::cout << "Path Size: " << path.size() << std::endl;
diff --git a/src/tools/graph.cpp b/src/tools/graph.cpp
index cc97225..7782502 100644
--- a/src/tools/graph.cpp
+++ b/src/tools/graph.cpp
@@ -48,6 +48,15 @@ int GraphNode::getValue(){
     return m_testvalue;
 }
 
+void GraphNode::setPriority(float a){
+    m_priority = a;
+}
+
+float GraphNode::getPriority(){
+    return m_priority;
+}
+
+
 
 
 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 42f670d..b6abb2f 100644
--- a/src/tools/graph.h
+++ b/src/tools/graph.h
@@ -10,6 +10,8 @@ class GraphNode
 {
     std::vector<GraphEdge> m_neighbours;
     int m_testvalue;  //temp variable
+    float m_priority;
+
 public:
     GraphNode();
     GraphNode(std::vector<GraphEdge>);
@@ -18,10 +20,15 @@ public:
     void addNeighbours(GraphNode*,float);
     void addNeighbours(GraphEdge);
     int getNbNeighbours();
-    virtual float heuristic(GraphNode*){return 1;};
+    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;
+    };
 
     void setValue(int);
     int getValue();
+    void setPriority(float);
+    float getPriority();
     void print(std::string prefix);
 };
 
diff --git a/src/tools/pathfinder.cpp b/src/tools/pathfinder.cpp
index e692459..1099a98 100644
--- a/src/tools/pathfinder.cpp
+++ b/src/tools/pathfinder.cpp
@@ -8,27 +8,26 @@
 PathFinder::PathFinder(){
 }
 
-std::vector<GraphNode*> PathFinder::a_star(GraphNode* start,GraphNode* goal)
+std::vector<GraphNode*> PathFinder::a_star(GraphNode* start,GraphNode* goal,bool debug)
 {
     // Check if priorityqueue sort value in right order.
-    std::priority_queue<PriorityNode*> frontier = std::priority_queue<PriorityNode*>();
+    std::priority_queue<GraphNode*,std::vector<GraphNode*>,ComparePriority> frontier = std::priority_queue<GraphNode*,std::vector<GraphNode*>,ComparePriority>();
     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
 
     // init frontier, cost, and pred with value for start
-    PriorityNode* pn = new PriorityNode();
-    pn->node = std::pair<GraphNode*,float>(start,0);
-    frontier.push(pn);
+    frontier.push(start);
     cost.insert(std::pair<GraphNode*,float>(start,0));
     pred.insert(std::pair<GraphNode*,GraphNode*>(start,NULL));
 
     GraphNode* current;
     while(!frontier.empty()){
         //pick best element from frontier (with priority queue the best is in front)
-        pn = frontier.top();
-        current = pn->node.first;
+        current = frontier.top();
+        //current = pn->node.first;
         frontier.pop();
 
+        if(debug) std::cout << "Exploring node " << current->getValue() << std::endl;
         // goal reached, end of a-star
         if (current == goal){
             break;
@@ -36,17 +35,20 @@ std::vector<GraphNode*> PathFinder::a_star(GraphNode* start,GraphNode* goal)
 
         // for all neighbours of current node
         for (GraphEdge next : current->getNeighbours()){
-             float new_cost = cost[current] + next.getWeight();
+            float new_cost = cost[current] + next.getWeight();
+            if(debug) std::cout << "\tExploring neighbours node " << next.getTarget()->getValue() << " with cost " << new_cost << std::endl;
 
-             if ((cost.count(next.getTarget()) == 0) || (new_cost < cost[next.getTarget()])){
+            bool never_visited = cost.count(next.getTarget()) == 0;
+            if ((never_visited) || (new_cost < cost[next.getTarget()])){
                 // affect processed cost to next node in cost_map
                 cost[next.getTarget()] = new_cost;
 
                 // calcul priority of node with heuristic,and add it to frontier                
-                float priority = new_cost; //+ heuristic(next.getTarget(), goal);
-                PriorityNode* pn = new PriorityNode();
-                pn->node = std::pair<GraphNode*,float>(next.getTarget(),priority);
-                frontier.push(pn);
+                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());
 
                 // memorize predecessor for next
                  pred[next.getTarget()] = current;
@@ -62,6 +64,7 @@ std::vector<GraphNode*> PathFinder::a_star(GraphNode* start,GraphNode* goal)
     }
     path.push_back(start);
     std::reverse(path.begin(),path.end());
+    if(debug) std::cout << "path cost :" << cost[goal] << std::endl;
     return path;
 }
 
diff --git a/src/tools/pathfinder.h b/src/tools/pathfinder.h
index d4a3d94..47e75d6 100644
--- a/src/tools/pathfinder.h
+++ b/src/tools/pathfinder.h
@@ -8,16 +8,14 @@ class PathFinder
 {
 public:
     PathFinder();
-    static std::vector<GraphNode*> a_star(GraphNode* start,GraphNode* goal);
+    static std::vector<GraphNode*> a_star(GraphNode* start,GraphNode* goal,bool debug = false);
 };
 
-struct PriorityNode
-{
-    std::pair<GraphNode*, float> node;
-    bool operator<(PriorityNode other) const
+class ComparePriority{
+public:
+    bool operator() (GraphNode* a,GraphNode* b)
     {
-        return node.second < other.node.second;
+        return a->getPriority() > b->getPriority();
     }
 };
-
 #endif // PATHFINDER_H