Route planning algorithm - draft, to be tested!