A* algorithm A star algorithm C language version