Add support for io flags in importOSMNodes function

src/dsm/headers/Agent.hpp

@@ -103,7 +103,7 @@ namespace dsm {
     Id id() const { return m_id; }
     /// @brief Get the agent's itinerary
     /// @return The agent's itinerary
-    Id itineraryId() const { return m_trip[m_itineraryIdx]; }
+    Id itineraryId() const { return m_trip.at(m_itineraryIdx); }
     /// @brief Get the agent's trip
     /// @return The agent's trip
     std::vector<Id> const& trip() const { return m_trip; }

src/dsm/headers/Dynamics.hpp

@@ -93,7 +93,7 @@
           auto path = SparseMatrix<bool>{};
           path.load(file);
           pItinerary->setPath(std::move(path));
-          Logger::info(
+          Logger::debug(
               std::format("Loaded cached path for itinerary {}", pItinerary->id()));
           return;
         }
@@ -147,24 +147,23 @@
                               destinationID,
                               1.));
             }
-          } else if ((nextNodeId != destinationID)) {
-            Logger::warning(std::format(
-                "No path found from node {} to node {}", nextNodeId, destinationID));
           }
         }
       }
-      if (path.size() == 0) {
+
+      if (path.empty()) {
         Logger::error(
-            std::format("Path with id {} and destination {} is empty. Please "
-                        "check the adjacency matrix.",
+            std::format("Path with id {} and destination {} is empty. Please check the "
+                        "adjacency matrix.",
                         pItinerary->id(),
                         pItinerary->destination()));
       }
+
       pItinerary->setPath(path);
       if (m_bCacheEnabled) {
         pItinerary->path().cache(
             std::format("{}it{}.dsmcache", g_cacheFolder, pItinerary->id()));
-        Logger::info(
+        Logger::debug(
             std::format("Saved path in cache for itinerary {}", pItinerary->id()));
       }
     }
@@ -249,6 +248,8 @@
     /// @param itineraries Generic container of itineraries, represented by an std::span
     void addItineraries(std::span<Itinerary> itineraries);
 
+    void enableCache();
+
     /// @brief Reset the simulation time
     void resetTime();
 
@@ -332,20 +333,32 @@
         m_generator{std::random_device{}()} {
     if (seed.has_value()) {
       m_generator.seed(seed.value());
     }
     if (m_bCacheEnabled) {
       if (!std::filesystem::exists(g_cacheFolder)) {
         std::filesystem::create_directory(g_cacheFolder);
       }
       Logger::info(std::format("Cache enabled (default folder is {})", g_cacheFolder));
     }
+    for (const auto& nodeId : this->m_graph.outputNodes()) {
+      addItinerary(Itinerary{nodeId, nodeId});
+      m_updatePath(m_itineraries.at(nodeId));
+    }
+    // updatePaths();
   }
 
   template <typename agent_t>
   void Dynamics<agent_t>::updatePaths() {
+    if (m_bCacheEnabled) {
+      if (!std::filesystem::exists(g_cacheFolder)) {
+        std::filesystem::create_directory(g_cacheFolder);
+      }
+    }
+
     std::vector<std::thread> threads;
     threads.reserve(m_itineraries.size());
     std::exception_ptr pThreadException;
+    Logger::info(std::format("Init computing {} paths", m_itineraries.size()));
     for (const auto& [itineraryId, itinerary] : m_itineraries) {
       threads.emplace_back(std::thread([this, &itinerary, &pThreadException] {
         try {
@@ -362,6 +375,8 @@
     // Throw the exception launched first
     if (pThreadException)
       std::rethrow_exception(pThreadException);
+
+    Logger::info("End computing paths");
   }
 
   template <typename agent_t>
@@ -390,6 +405,10 @@
           std::format("Agent with id {} already exists.", agent->id())));
     }
     m_agents.emplace(agent->id(), std::move(agent));
+    // Logger::debug(std::format("Added agent with id {} from node {} to node {}",
+    //                           m_agents.rbegin()->first,
+    //                           m_agents.rbegin()->second->srcNodeId().value_or(-1),
+    //                           m_agents.rbegin()->second->itineraryId()));
   }
 
   template <typename agent_t>
@@ -435,6 +454,7 @@
   template <typename agent_t>
   void Dynamics<agent_t>::removeAgent(Size agentId) {
     m_agents.erase(agentId);
+    Logger::debug(std::format("Removed agent with id {}", agentId));
   }
 
   template <typename agent_t>
@@ -468,6 +488,12 @@
     });
   }
 
+  template <typename agent_t>
+  void Dynamics<agent_t>::enableCache() {
+    m_bCacheEnabled = true;
+    Logger::info(std::format("Cache enabled (default folder is {})", g_cacheFolder));
+  }
+
   template <typename agent_t>
   void Dynamics<agent_t>::resetTime() {
     m_time = 0;

src/dsm/headers/Graph.hpp

@@ -50,6 +50,8 @@
     std::unordered_map<Id, std::unique_ptr<Node>> m_nodes;
     std::unordered_map<Id, std::unique_ptr<Street>> m_streets;
     std::unordered_map<std::string, Id> m_nodeMapping;
+    std::vector<Id> m_inputNodes;
+    std::vector<Id> m_outputNodes;
     SparseMatrix<bool> m_adjacency;
     unsigned long long m_maxAgentCapacity;
 
@@ -80,6 +82,8 @@
           });
       m_nodeMapping = other.m_nodeMapping;
       m_adjacency = other.m_adjacency;
+      m_inputNodes = other.m_inputNodes;
+      m_outputNodes = other.m_outputNodes;
     }
 
     Graph& operator=(const Graph& other) {
@@ -94,6 +98,8 @@
           });
       m_nodeMapping = other.m_nodeMapping;
       m_adjacency = other.m_adjacency;
+      m_inputNodes = other.m_inputNodes;
+      m_outputNodes = other.m_outputNodes;
 
       return *this;
     }
@@ -261,6 +267,9 @@
     /// @return unsigned long long The maximum agent capacity of the graph
     unsigned long long maxCapacity() const { return m_maxAgentCapacity; }
 
+    std::vector<Id> const& inputNodes() const { return m_inputNodes; }
+    std::vector<Id> const& outputNodes() const { return m_outputNodes; }
+
     /// @brief Get the shortest path between two nodes using dijkstra algorithm
     /// @param source The source node
     /// @param destination The destination node
@@ -287,7 +296,7 @@
              std::constructible_from<node_t, Id, TArgs...>)
   node_t& Graph::addNode(Id id, TArgs&&... args) {
     addNode(std::make_unique<node_t>(id, std::forward<TArgs>(args)...));
-    return dynamic_cast<node_t&>(*m_nodes[id]);
+    return dynamic_cast<node_t&>(*m_nodes.at(id));
   }
   template <typename T1, typename... Tn>
     requires is_node_v<std::remove_reference_t<T1>> &&
@@ -302,7 +311,7 @@
              std::constructible_from<edge_t, Id, TArgs...>)
   edge_t& Graph::addEdge(Id id, TArgs&&... args) {
     addStreet(std::make_unique<edge_t>(id, std::forward<TArgs>(args)...));
-    return dynamic_cast<edge_t&>(*m_streets[id]);
+    return dynamic_cast<edge_t&>(*m_streets.at(id));
   }
 
   template <typename T1>

src/dsm/headers/RoadDynamics.hpp

@@ -129,6 +129,8 @@
                            const TContainer& dst_weights,
                            const size_t minNodeDistance = 0);
 
+    void addAgentsRandomly(Size nAgents, const size_t minNodeDistance = 0);
+
     /// @brief Evolve the simulation
     /// @details Evolve the simulation by moving the agents and updating the travel times.
     /// In particular:
@@ -235,7 +237,10 @@
         }
       }
     }
-    assert(possibleMoves.size() > 0);
+    if (possibleMoves.empty()) {
+      Logger::error(
+          std::format("No possible moves from node {} for agent {}", nodeId, agentId));
+    }
     std::uniform_int_distribution<Size> moveDist{
         0, static_cast<Size>(possibleMoves.size() - 1)};
     uint8_t p{0};
@@ -319,9 +324,7 @@
         }
       }
       if (bArrived) {
-        if (pStreet->dequeue(queueIndex) == std::nullopt) {
-          continue;
-        }
+        pStreet->dequeue(queueIndex);
         m_travelDTs.push_back({pAgent->distance(), static_cast<double>(pAgent->time())});
         if (reinsert_agents) {
           // reset Agent's values
@@ -331,14 +334,14 @@
         }
         continue;
       }
-      auto const& nextStreet{this->m_graph.streetSet()[m_agentNextStreetId[agentId]]};
+      auto const& nextStreet{this->m_graph.street(m_agentNextStreetId.at(agentId))};
       if (nextStreet->isFull()) {
         continue;
       }
-      if (pStreet->dequeue(queueIndex) == std::nullopt) {
-        continue;
+      pStreet->dequeue(queueIndex);
+      if (destinationNode->id() != nextStreet->u()) {
+        Logger::error(std::format("Agent {} is going to the wrong street", agentId));
       }
-      assert(destinationNode->id() == nextStreet->nodePair().first);
       if (destinationNode->isIntersection()) {
         auto& intersection = dynamic_cast<Intersection&>(*destinationNode);
         auto const delta{nextStreet->deltaAngle(pStreet->angle())};
@@ -360,7 +363,7 @@
         return false;
       }
       for (auto const [angle, agentId] : intersection.agents()) {
-        auto const& nextStreet{this->m_graph.streetSet()[m_agentNextStreetId[agentId]]};
+        auto const& nextStreet{this->m_graph.street(m_agentNextStreetId.at(agentId))};
         if (nextStreet->isFull()) {
           if (m_forcePriorities) {
             return false;
@@ -383,7 +386,7 @@
         return false;
       }
       auto const agentId{roundabout.agents().front()};
-      auto const& nextStreet{this->m_graph.streetSet()[m_agentNextStreetId[agentId]]};
+      auto const& nextStreet{this->m_graph.street(m_agentNextStreetId.at(agentId))};
       if (!(nextStreet->isFull())) {
         if (this->agents().at(agentId)->streetId().has_value()) {
           const auto streetId = this->agents().at(agentId)->streetId().value();
@@ -494,7 +497,7 @@
                  !m_agentNextStreetId.contains(agentId)) {
         Id srcNodeId = agent->srcNodeId().has_value() ? agent->srcNodeId().value()
                                                       : nodeDist(this->m_generator);
-        const auto& srcNode{this->m_graph.nodeSet()[srcNodeId]};
+        const auto& srcNode{this->m_graph.node(srcNodeId)};
         if (srcNode->isFull()) {
           continue;
         }
@@ -596,6 +599,13 @@
                                                 const TContainer& src_weights,
                                                 const TContainer& dst_weights,
                                                 const size_t minNodeDistance) {
+    Logger::debug(
+        std::format("Init addAgentsRandomly for {} agents from {} nodes to {} nodes with "
+                    "minNodeDistance {}",
+                    nAgents,
+                    src_weights.size(),
+                    dst_weights.size(),
+                    minNodeDistance));
     if (src_weights.size() == 1 && dst_weights.size() == 1 &&
         src_weights.begin()->first == dst_weights.begin()->first) {
       throw std::invalid_argument(Logger::buildExceptionMessage(
@@ -630,6 +640,7 @@
     if (!this->agents().empty()) {
       agentId = this->agents().rbegin()->first + 1;
     }
+    Logger::debug(std::format("Adding {} agents at time {}.", nAgents, this->time()));
     while (nAgents > 0) {
       Id srcId{0}, dstId{0};
       if (dst_weights.size() == 1) {
@@ -683,6 +694,20 @@
     }
   }
 
+  template <typename delay_t>
+    requires(is_numeric_v<delay_t>)
+  void RoadDynamics<delay_t>::addAgentsRandomly(Size nAgents,
+                                                const size_t minNodeDistance) {
+    std::unordered_map<Id, double> src_weights, dst_weights;
+    for (auto const& id : this->m_graph.inputNodes()) {
+      src_weights[id] = 1.;
+    }
+    for (auto const& id : this->m_graph.outputNodes()) {
+      dst_weights[id] = 1.;
+    }
+    addAgentsRandomly(nAgents, src_weights, dst_weights, minNodeDistance);
+  }
+
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
   void RoadDynamics<delay_t>::evolve(bool reinsert_agents) {
@@ -691,7 +716,7 @@
         m_dataUpdatePeriod.has_value() && this->m_time % m_dataUpdatePeriod.value() == 0;
     for (const auto& [streetId, pStreet] : this->m_graph.streetSet()) {
       if (bUpdateData) {
-        m_streetTails[streetId] += pStreet->nExitingAgents();
+        m_streetTails.at(streetId) += pStreet->nExitingAgents();
       }
       for (auto i = 0; i < pStreet->transportCapacity(); ++i) {
         this->m_evolveStreet(pStreet, reinsert_agents);
@@ -736,11 +761,11 @@
 
       double inputGreenSum{0.}, inputRedSum{0.};
       for (const auto& [streetId, _] : this->m_graph.adjMatrix().getCol(nodeId, true)) {
-        auto const& pStreet{this->m_graph.streetSet()[streetId]};
+        auto const& pStreet{this->m_graph.street(streetId)};
         if (streetPriorities.contains(streetId)) {
-          inputGreenSum += m_streetTails[streetId] / pStreet->nLanes();
+          inputGreenSum += m_streetTails.at(streetId) / pStreet->nLanes();
         } else {
-          inputRedSum += m_streetTails[streetId] / pStreet->nLanes();
+          inputRedSum += m_streetTails.at(streetId) / pStreet->nLanes();
         }
       }
       inputGreenSum /= meanGreenFraction;
@@ -780,11 +805,11 @@
         //    - If the previous check fails, do nothing
         double outputGreenSum{0.}, outputRedSum{0.};
         for (const auto& [streetId, _] : this->m_graph.adjMatrix().getRow(nodeId, true)) {
-          auto const& pStreet{this->m_graph.streetSet()[streetId]};
+          auto const& pStreet{this->m_graph.street(streetId)};
           if (streetPriorities.contains(streetId)) {
-            outputGreenSum += m_streetTails[streetId] / pStreet->nLanes();
+            outputGreenSum += m_streetTails.at(streetId) / pStreet->nLanes();
           } else {
-            outputRedSum += m_streetTails[streetId] / pStreet->nLanes();
+            outputRedSum += m_streetTails.at(streetId) / pStreet->nLanes();
           }
         }
         auto const outputDifference{(outputGreenSum - outputRedSum) / nCycles};

src/dsm/headers/SparseMatrix.hpp

@@ -178,6 +178,8 @@ namespace dsm {
     /// @return number of non zero elements
     Id size() const { return _matrix.size(); };
 
+    bool empty() const { return _matrix.empty(); }
+
     /// @brief get the maximum number of elements in the matrix
     /// @return maximum number of elements
     Id max_size() const { return _rows * _cols; }

src/dsm/headers/Street.hpp

@@ -105,7 +105,8 @@ namespace dsm {
     /// @throw std::runtime_error If the street's queue is full
     void enqueue(Id agentId, size_t index);
     /// @brief Remove an agent from the street's queue
-    virtual std::optional<Id> dequeue(size_t index);
+    /// @return Id The id of the agent removed from the street's queue
+    virtual Id dequeue(size_t index);
     /// @brief Check if the street is a spire
     /// @return bool True if the street is a spire, false otherwise
     virtual bool isSpire() const { return false; };
@@ -160,8 +161,8 @@ namespace dsm {
     ///     Notice that this flow is positive iff the input flow is greater than the output flow.
     int meanFlow();
     /// @brief Remove an agent from the street's queue
-    /// @return std::optional<Id> The id of the agent removed from the street's queue
-    std::optional<Id> dequeue(size_t index) final;
+    /// @return Id The id of the agent removed from the street's queue
+    Id dequeue(size_t index) final;
     /// @brief Check if the street is a spire
     /// @return bool True if the street is a spire, false otherwise
     bool isSpire() const final { return true; };
@@ -182,7 +183,7 @@ namespace dsm {
     int meanFlow();
     /// @brief Remove an agent from the street's queue
     /// @return std::optional<Id> The id of the agent removed from the street's queue
-    std::optional<Id> dequeue(size_t index) final;
+    Id dequeue(size_t index) final;
     /// @brief Check if the street is a spire
     /// @return bool True if the street is a spire, false otherwise
     bool isSpire() const final { return true; };

src/dsm/sources/FirstOrderDynamics.cpp

@@ -28,7 +28,7 @@ namespace dsm {
 
   void FirstOrderDynamics::setAgentSpeed(Size agentId) {
     const auto& agent{this->agents().at(agentId)};
-    const auto& street{this->m_graph.streetSet()[agent->streetId().value()]};
+    const auto& street{this->m_graph.street(agent->streetId().value())};
     double speed{street->maxSpeed() * (1. - m_alpha * street->density(true))};
     if (m_speedFluctuationSTD > 0.) {
       std::normal_distribution<double> speedDist{speed, speed * m_speedFluctuationSTD};