fixing A* implementation - cost function priority queue