Fixed car path finding, implemented one test case