diff --git a/lib/astar.py b/lib/astar.py
index 2c11e08..99b5ee0 100644
--- a/lib/astar.py
+++ b/lib/astar.py
@@ -37,11 +37,13 @@ def search(start, is_goal, neighbors, heuristic=None):
             return cost_so_far, this, path
 
         for c, n in neighbors(this):
-            if n not in done:
-                # calculate the "reduced cost"
-                c = cost_so_far + c + heuristic(n)
+            c = cost_so_far + c
+            if n not in done or c < done[n][0]:
+                h = heuristic(n)
+                if h is None:
+                    continue
                 i += 1
-                heappush(q, (c, i, n, this))
+                heappush(q, (c+h, i, n, this))
 
     return float('inf'), None, []