A* best path algorithm implemented