public void FindPathATest()
{
PointPolygon pointToGo = (11, 5);
//SquarePolygon pointToGo = ((11, 5), 2);
Index pointA = (28, 10);
SquarePolygon wall1 = ((19, 8), 4);
SquarePolygon wall2 = ((11, 17), 4);
SquarePolygon wall3 = ((23, 17), 4);
var graph = new PathGraph(30, 30);
graph.AddWalls(wall1, wall2, wall3);
graph.FindPath1(pointToGo, pointA);
Debug.WriteLine(graph.ToPath());
}