A* path finding algorithm implementation