I was largely holding off on doing further work hoping for some discussion of which way we should go. If you think my approach is worth pursuing (I haven't seriously tested the performance, but I'd expect it to be slower than Surafel's - the price you pay for flexibility) then I can look at it further, but figuring out the planner stuff will take some time.
Other alternative can be pushing the existing implementation
which will be open to change in case of better-finished