add tryAdd method that can't create vehicles for the nex algorithm

src/algorithm/DARP/Heuristics/BestInsertionHeuristic.cpp

@@ -15,9 +15,11 @@ bool BestInsertionHeuristic::tryVehicleBestInsertion(size_t requestId, size_t ve
     BestInsertionQueue bestInsertionsQueue = route.getBestFeasibleInsertionsQueue(requestId, vehicleId); //TODO: check perfs between BestInsertionsQueue vs BestFeasibleInsertionsQueue
     bool bestInsertionFound = false;
     BestRequestInsertion currentBestInsertion;
-    DEBUG_MSG("Trying to insert request " + std::to_string(requestId) + " in vehicle " + std::to_string(vehicleId));
+
+    DEBUG_MSG("Trying to insert request " + std::to_string(requestId) + " in vehicle " + std::to_string(vehicleId) + " queue size : " + std::to_string(bestInsertionsQueue.size()));
     while(!bestInsertionsQueue.empty() && !bestInsertionFound) {
         currentBestInsertion = bestInsertionsQueue.topAndPop();
+        DEBUG_MSG("Trying insertion " + currentBestInsertion.to_string() + ", remaining : " + std::to_string(bestInsertionsQueue.size()));
         SAEVRouteChangelist lastInsertionChangelist = route.tryAddRequest(requestId,
                                                                           *currentBestInsertion.getOriginInsertionKp(),
                                                                           *currentBestInsertion.getDestinationInsertionKp());
@@ -49,3 +51,17 @@ size_t BestInsertionHeuristic::doBestRequestInsertionForRoute(size_t requestId,
 
     return vehicleId;
 }
+
+size_t BestInsertionHeuristic::tryBestRequestInsertionInActiveVehicle(size_t requestId, SAEVRoute &route) {
+    size_t vehicleId = 0;
+    bool insertionSuccess{false};
+    //Iteratively try inserting in every active vehicle and the first inactive vehicle
+    while(!insertionSuccess && ++vehicleId <= route.getLastActiveVehicleId() + 1) {
+        insertionSuccess = tryVehicleBestInsertion(requestId, vehicleId, route);
+    }
+
+    if(insertionSuccess)
+        return vehicleId;
+    else
+        return route.getLastActiveVehicleId() + 1;
+}

src/algorithm/DARP/Heuristics/BestInsertionHeuristic.h

@@ -18,6 +18,16 @@ public:
      * @return ID of the vehicle in which the request has been
      */
     static size_t doBestRequestInsertionForRoute(size_t requestId, SAEVRoute& route);
+    /**
+     * Iteratively attempts insertions in the currently active vehicles in two steps :
+     * 1) creates a global best insertion list for all active vehicles
+     * 2) tries every insertions from best to worst
+     * 3) return the ID of the vehicle in which the request was inserted if an active vehicle was viable, return the most favourable inactive vehicle's ID otherwise
+     * @param requestId ID of the request to insert in the route
+     * @param route the route structure in which the request will be inserted
+     * @return The ID of the active vehicle in which our request was inserted, or the ID of the most favourable inactive vehicle
+     */
+    static size_t tryBestRequestInsertionInActiveVehicle(size_t requestId, SAEVRoute& route);
     /**
      * Iteratively tests best insertions wrt scoring function (detour) in the given vehicle and route
      * @param requestId ID of the request to insert in the vehicle
@@ -27,6 +37,7 @@ public:
      */
     static bool tryVehicleBestInsertion(size_t requestId, size_t vehicleId, SAEVRoute& route);
 
+
     /** TODO Implement those to prevent trying every single best insertion
     static bool vehicle_K_BestInsertion(size_t requestId, size_t vehicleId, SAEVRoute route);
     static bool vehicleScoreThresholdBestInsertion(size_t requestId, size_t vehicleId, SAEVRoute& route); */