WaypointPlanner exports granular (resampled) gpx

But the .wst export is still the normal respresentation
Fixed simulation timing issue in WaypointPlanner
Cleaned up hierarchical planner...

* Cheap dependency checks happen before expensive A* intermediate
plans..
* Moved some things around to slightly enhance readability