Revised implementation of routing algorithm