Implemented optimizeRoute function and added a test case for it that passes