AStar working, except for infinite loop problem