Add javadoc
Fix classes so MapPathFinder can be reused