Move finishSolve() from DiffusingPlanner to PathPlanner, and add debug outputs
Cosmetic changes

  - remove trailing white spaces.