Cleaned up Astar code