Add a distance metric to turn the routing into an A* search

The original straightforward implementation exploded in complexity much
sooner than I expected.  The reasons are not entirely clear, but the
inability to converge states and drop those where the same position was
already reached through a faster path may have something to do with it.