A* Pathfinding algorithm for C#