add best insertion heuristic functions

CMakeLists.txt

@@ -113,6 +113,8 @@ add_executable(GraphGeneration
         "src/utils/Instance Generation/Graph/OSRMGraphGenerator.h"
         "src/utils/Instance Generation/Graph/GraphGenerator.h"
         "src/utils/Instance Generation/Graph/PTLineGenerationParameters.h"
+        src/algorithm/DARP/Heuristics/BestInsertionHeuristic.cpp
+        src/algorithm/DARP/Heuristics/BestInsertionHeuristic.h
 )
 
 target_include_directories(GreedyAlgorithm PRIVATE ${PYTHON_INCLUDE_DIRS})

src/algorithm/DARP/Heuristics/BestInsertionHeuristic.cpp

+//
+// Created by romain on 20/06/24.
+//
+
+#include "BestInsertionHeuristic.h"
+
+#ifdef DEBUG_TRANSIT_PRECOMPUTE
+#include <iostream>
+#define DEBUG_MSG(str) do { std::cout << str << std::endl; } while( false )
+#else
+#define DEBUG_MSG(str) do { } while ( false )
+#endif
+
+bool BestInsertionHeuristic::tryVehicleBestInsertion(size_t requestId, size_t vehicleId, SAEVRoute& route) {
+    BestInsertionQueue bestInsertionsQueue = route.getBestFeasibleInsertionsQueue(requestId, vehicleId); //TODO: check perfs between BestInsertionsQueue vs BestFeasibleInsertionsQueue
+    bool bestInsertionFound = false;
+    BestRequestInsertion currentBestInsertion;
+
+    while(!bestInsertionsQueue.empty() && !bestInsertionFound) {
+        currentBestInsertion = bestInsertionsQueue.topAndPop();
+        SAEVRouteChangelist lastInsertionChangelist = route.tryAddRequest(requestId,
+                                                                          *currentBestInsertion.getOriginInsertionKp(),
+                                                                          *currentBestInsertion.getDestinationInsertionKp());
+        //If insertion worked, signal it, otherwise revert changes
+        if(lastInsertionChangelist.getStatus() == SAEVRouteChangelist::InsertionStatus::SUCCESS) {
+            DEBUG_MSG("Best valid insertion found !\n\t" + currentBestInsertion.to_string());
+            bestInsertionFound = true;
+        } else {
+            lastInsertionChangelist.revertChanges();
+        }
+    }
+    return bestInsertionFound;
+}
+
+size_t BestInsertionHeuristic::doRouteBestInsertion(size_t requestId, SAEVRoute route) {
+    size_t vehicleId = 0;
+    bool insertionSuccess{false};
+    while(vehicleId <= route.getLastActiveVehicleId() + 1 && !insertionSuccess) {
+        insertionSuccess = tryVehicleBestInsertion(requestId, vehicleId, route);
+    }
+    return vehicleId;
+}

src/algorithm/DARP/Heuristics/BestInsertionHeuristic.h

+//
+// Created by romain on 20/06/24.
+//
+
+#ifndef GREEDYALGORITHM_BESTINSERTIONHEURISTIC_H
+#define GREEDYALGORITHM_BESTINSERTIONHEURISTIC_H
+
+
+#include <cstdlib>
+#include "../../../routes/vehicle/SAEVRoute.h"
+
+class BestInsertionHeuristic {
+    /**
+     * Automatically inserts the given request in a vehicle, potentially creating a new one if no active vehicle works
+     * @param requestId ID of the request to insert in the route
+     * @param route the route structure in which the request will be inserted
+     * @return ID of the vehicle in which the request has been
+     */
+    static size_t doRouteBestInsertion(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
+     * @param requestId ID of the vehicle in which to insert the vehicle
+     * @param route
+     * @return true iff the request was inserted in the vehicle, false if no best insertion yielded a possible insertion
+     */
+    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); */
+};
+
+
+#endif //GREEDYALGORITHM_BESTINSERTIONHEURISTIC_H