Worked on AStar (converted to function)