planner performance improvements

[K0zka]

There are some performance issues with the planner that should be investigated and improved if any better solution is found:

• Planner is only able to calculate the benefits of a step once the step is taken, this may be a bit too expensive
• Also the planner is working with depth first backtrack algorithm, a paralell version of dijkstra could possibly improve on responsiveness