fixed AStar and added visualization!