Astar calculation NOTE! Note tested