major progress on findPath function.