Simple used AStar (A*) implementation on C#