A* algorithm in C