Wrote two simple tests to varify that the pathfinding algorithm worked