Adjust heuristic (#19)

Also need to adjust heuristic since
costs are now in real coordinate terms