Final astar algortith; we still need to standarlize its name