C# A* algorithm implementation