Changing Astar algorithm to work with Node