Fix goal problems in A* search