Prog2 A* algorithm