Added printing of optimal route. Not tested