A-star (A*) path finding algorithm in C