Corrected A* implementation bugs