Add pared-down Dikjstra routing code