A* algorithm implementation and demo