More infrastructure on planning multiple paths
Moved path planner
Refactoring