Added test for path finding
Work on path finding..