clean up findPath some, and do some minor testing on the findPath algorithm to verify correctness.