Test for finding correct path
Refactor findPath()