Extended the new routing algorithm and created more tests