Small refactoring
Variation of HSP planner