Fixed A* heuristic to use distance function rather than manhattan distance