diff --git a/examples/slow_charge_rb.cpp b/examples/slow_charge_rb.cpp
index 568024f17..7aef0ebbc 100644
--- a/examples/slow_charge_rb.cpp
+++ b/examples/slow_charge_rb.cpp
@@ -71,7 +71,7 @@ int main(int argc, char** argv) {
                                            BASE_OUT_FOLDER,
                                            ERROR_PROBABILITY,
                                            std::to_string(SEED))};  // output folder
-  const auto MAX_TIME{static_cast<unsigned int>(1e6)};  // maximum time of simulation
+  const auto MAX_TIME{static_cast<unsigned int>(5e5)};  // maximum time of simulation
 
   // Clear output folder or create it if it doesn't exist
   if (!fs::exists(BASE_OUT_FOLDER)) {
@@ -205,14 +205,15 @@ int main(int argc, char** argv) {
     dynamics.evolve(false);
 
     if (dynamics.time() % 2400 == 0) {
-      deltaAgents = dynamics.agents().size() - previousAgents;
+      auto const totalDynamicsAgents{dynamics.nAgents()};
+      deltaAgents = totalDynamicsAgents - previousAgents;
       if (deltaAgents < 0) {
         ++nAgents;
         std::cout << "- Now I'm adding " << nAgents << " agents.\n";
         std::cout << "Delta agents: " << deltaAgents << '\n';
         std::cout << "At time: " << dynamics.time() << '\n';
       }
-      previousAgents = dynamics.agents().size();
+      previousAgents = totalDynamicsAgents;
     }
 
     if (dynamics.time() % 300 == 0) {
diff --git a/examples/slow_charge_tl.cpp b/examples/slow_charge_tl.cpp
index 3a9a3e8fe..9ad6a69ed 100644
--- a/examples/slow_charge_tl.cpp
+++ b/examples/slow_charge_tl.cpp
@@ -67,7 +67,7 @@ int main(int argc, char** argv) {
                                      BASE_OUT_FOLDER,
                                      ERROR_PROBABILITY,
                                      std::to_string(SEED))};  // output folder
-  constexpr auto MAX_TIME{static_cast<unsigned int>(1e6)};  // maximum time of simulation
+  constexpr auto MAX_TIME{static_cast<unsigned int>(5e5)};  // maximum time of simulation
 
   std::cout << "-------------------------------------------------\n";
   std::cout << "Input parameters:\n";
@@ -309,14 +309,15 @@ int main(int argc, char** argv) {
     if (dynamics.time() % 2400 == 0 && nAgents > 0) {
       // auto meanDelta = std::accumulate(deltas.begin(), deltas.end(), 0) /
       // deltas.size();
-      deltaAgents = dynamics.agents().size() - previousAgents;
+      auto const totalDynamicsAgents{dynamics.nAgents()};
+      deltaAgents = totalDynamicsAgents - previousAgents;
       if (deltaAgents < 0) {
         ++nAgents;
         std::cout << "- Now I'm adding " << nAgents << " agents.\n";
         std::cout << "Delta agents: " << deltaAgents << '\n';
         std::cout << "At time: " << dynamics.time() << '\n';
       }
-      previousAgents = dynamics.agents().size();
+      previousAgents = totalDynamicsAgents;
       // deltas.clear();
     }
 
diff --git a/src/dsm/dsm.hpp b/src/dsm/dsm.hpp
index ddafb7715..2cb390a08 100644
--- a/src/dsm/dsm.hpp
+++ b/src/dsm/dsm.hpp
@@ -5,8 +5,8 @@
 #include <format>
 
 static constexpr uint8_t DSM_VERSION_MAJOR = 2;
-static constexpr uint8_t DSM_VERSION_MINOR = 5;
-static constexpr uint8_t DSM_VERSION_PATCH = 12;
+static constexpr uint8_t DSM_VERSION_MINOR = 6;
+static constexpr uint8_t DSM_VERSION_PATCH = 0;
 
 static auto const DSM_VERSION =
     std::format("{}.{}.{}", DSM_VERSION_MAJOR, DSM_VERSION_MINOR, DSM_VERSION_PATCH);
diff --git a/src/dsm/headers/Agent.hpp b/src/dsm/headers/Agent.hpp
index 7a21744f3..599cabdad 100644
--- a/src/dsm/headers/Agent.hpp
+++ b/src/dsm/headers/Agent.hpp
@@ -38,22 +38,20 @@ namespace dsm {
   public:
     /// @brief Construct a new Agent object
     /// @param spawnTime The agent's spawn time
-    /// @param id The agent's id
     /// @param itineraryId Optional, The agent's destination node. If not provided, the agent is a random agent
     /// @param srcNodeId Optional, The id of the source node of the agent
     Agent(Time const& spawnTime,
-          Id id,
           std::optional<Id> itineraryId = std::nullopt,
           std::optional<Id> srcNodeId = std::nullopt);
     /// @brief Construct a new Agent object
     /// @param spawnTime The agent's spawn time
-    /// @param id The agent's id
     /// @param itineraryIds The agent's itinerary
     /// @param srcNodeId Optional, The id of the source node of the agent
     Agent(Time const& spawnTime,
-          Id id,
           std::vector<Id> const& trip,
           std::optional<Id> srcNodeId = std::nullopt);
+
+    void setSrcNodeId(Id srcNodeId);
     /// @brief Set the street occupied by the agent
     /// @param streetId The id of the street currently occupied by the agent
     void setStreetId(std::optional<Id> streetId = std::nullopt);
diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp
index dc984616e..b2de75490 100644
--- a/src/dsm/headers/Dynamics.hpp
+++ b/src/dsm/headers/Dynamics.hpp
@@ -26,8 +26,6 @@
 #include <functional>
 
 #include "Network.hpp"
-#include "../utility/TypeTraits/is_agent.hpp"
-#include "../utility/TypeTraits/is_itinerary.hpp"
 #include "../utility/Logger.hpp"
 #include "../utility/Typedef.hpp"
 
diff --git a/src/dsm/headers/Edge.hpp b/src/dsm/headers/Edge.hpp
index dbc860440..dc69d241b 100644
--- a/src/dsm/headers/Edge.hpp
+++ b/src/dsm/headers/Edge.hpp
@@ -31,7 +31,11 @@ namespace dsm {
          int capacity = 1,
          double transportCapacity = 1.,
          std::vector<std::pair<double, double>> geometry = {});
+    Edge(Edge&&) = default;
+    Edge(const Edge&) = delete;
+    virtual ~Edge() = default;
 
+    void resetId(Id newId);
     void setCapacity(int capacity);
     void setTransportCapacity(double capacity);
     void setGeometry(std::vector<std::pair<double, double>> geometry);
diff --git a/src/dsm/headers/FirstOrderDynamics.hpp b/src/dsm/headers/FirstOrderDynamics.hpp
index 1cf321448..9ba11c9f7 100644
--- a/src/dsm/headers/FirstOrderDynamics.hpp
+++ b/src/dsm/headers/FirstOrderDynamics.hpp
@@ -23,7 +23,7 @@ namespace dsm {
     /// @brief Set the speed of an agent
     /// @param agentId The id of the agent
     /// @throw std::invalid_argument, If the agent is not found
-    void setAgentSpeed(Size agentId) override;
+    void setAgentSpeed(std::unique_ptr<Agent> const& pAgent) override;
     /// @brief Set the standard deviation of the speed fluctuation
     /// @param speedFluctuationSTD The standard deviation of the speed fluctuation
     /// @throw std::invalid_argument, If the standard deviation is negative
diff --git a/src/dsm/headers/Intersection.hpp b/src/dsm/headers/Intersection.hpp
index cd5c569c4..a530a122a 100644
--- a/src/dsm/headers/Intersection.hpp
+++ b/src/dsm/headers/Intersection.hpp
@@ -8,17 +8,19 @@
 
 #pragma once
 
-#include "Node.hpp"
+#include "Agent.hpp"
+#include "RoadJunction.hpp"
 
 #include <map>
+#include <memory>
 #include <set>
 
 namespace dsm {
   /// @brief The Intersection class represents a node in the network.
   /// @tparam Id The type of the node's id. It must be an unsigned integral type.
-  class Intersection : public Node {
+  class Intersection : public RoadJunction {
   protected:
-    std::multimap<int16_t, Id> m_agents;
+    std::multimap<int16_t, std::unique_ptr<Agent>> m_agents;
     std::set<Id>
         m_streetPriorities;  // A set containing the street ids that have priority - like main roads
     Size m_agentCounter;
@@ -26,13 +28,15 @@ namespace dsm {
   public:
     /// @brief Construct a new Intersection object
     /// @param id The node's id
-    explicit Intersection(Id id) : Node{id} {};
+    explicit Intersection(Id id) : RoadJunction{id} {};
     /// @brief Construct a new Intersection object
     /// @param id The node's id
     /// @param coords A std::pair containing the node's coordinates
-    Intersection(Id id, std::pair<double, double> coords) : Node{id, coords} {};
+    Intersection(Id id, std::pair<double, double> coords) : RoadJunction{id, coords} {};
 
-    Intersection(Node const& node) : Node{node} {};
+    Intersection(RoadJunction const& node) : RoadJunction{node} {};
+
+    Intersection(Intersection const&) = delete;
 
     virtual ~Intersection() = default;
 
@@ -47,17 +51,17 @@ namespace dsm {
     ///          The agent with the smallest angle difference is the first one to be
     ///          removed from the node.
     /// @throws std::runtime_error if the node is full
-    void addAgent(double angle, Id agentId);
+    void addAgent(double angle, std::unique_ptr<Agent> pAgent);
     /// @brief Put an agent in the node
     /// @param agentId The agent's id
     /// @details The agent's angle difference is used to order the agents in the node.
     ///          The agent with the smallest angle difference is the first one to be
     ///          removed from the node.
     /// @throws std::runtime_error if the node is full
-    void addAgent(Id agentId);
-    /// @brief Removes an agent from the node
-    /// @param agentId The agent's id
-    void removeAgent(Id agentId);
+    void addAgent(std::unique_ptr<Agent> pAgent);
+    // /// @brief Removes an agent from the node
+    // /// @param agentId The agent's id
+    // void removeAgent(Id agentId);
     /// @brief Set the node streets with priority
     /// @param streetPriorities A std::set containing the node's street priorities
     void setStreetPriorities(std::set<Id> streetPriorities) {
@@ -69,11 +73,11 @@ namespace dsm {
     /// @brief Returns the node's density
     /// @return double The node's density
     double density() const override {
-      return static_cast<double>(m_agents.size()) / m_capacity;
+      return static_cast<double>(m_agents.size()) / this->capacity();
     }
     /// @brief Returns true if the node is full
     /// @return bool True if the node is full
-    bool isFull() const override { return m_agents.size() == this->m_capacity; }
+    bool isFull() const override { return m_agents.size() == this->capacity(); }
 
     /// @brief Get the node's street priorities
     /// @details This function returns a std::set containing the node's street priorities.
@@ -83,7 +87,7 @@ namespace dsm {
     virtual const std::set<Id>& streetPriorities() const { return m_streetPriorities; };
     /// @brief Get the node's agent ids
     /// @return std::set<Id> A std::set containing the node's agent ids
-    const std::multimap<int16_t, Id>& agents() { return m_agents; };
+    std::multimap<int16_t, std::unique_ptr<Agent>>& agents() { return m_agents; };
     /// @brief Returns the number of agents that have passed through the node
     /// @return Size The number of agents that have passed through the node
     /// @details This function returns the number of agents that have passed through the node
diff --git a/src/dsm/headers/Network.hpp b/src/dsm/headers/Network.hpp
index bbeee0aff..3cf36f76c 100644
--- a/src/dsm/headers/Network.hpp
+++ b/src/dsm/headers/Network.hpp
@@ -48,6 +48,10 @@ namespace dsm {
       requires(std::is_base_of_v<node_t, TNode> &&
                std::constructible_from<TNode, Id, TArgs...>)
     void addNode(Id nodeId, TArgs&&... args);
+
+    template <typename TEdge = edge_t>
+      requires(std::is_base_of_v<edge_t, TEdge>)
+    void addEdge(TEdge&& edge);
     /// @brief Add an edge to the network
     /// @tparam TEdge The type of the edge (default is edge_t)
     /// @tparam TArgs The types of the arguments
@@ -160,6 +164,15 @@ namespace dsm {
     m_nodes.emplace(std::make_pair(
         nodeId, std::make_unique<TNode>(nodeId, std::forward<TArgs>(args)...)));
   }
+  template <typename node_t, typename edge_t>
+    requires(std::is_base_of_v<Node, node_t> && std::is_base_of_v<Edge, edge_t>)
+  template <typename TEdge>
+    requires(std::is_base_of_v<edge_t, TEdge>)
+  void Network<node_t, edge_t>::addEdge(TEdge&& edge) {
+    assert(!m_edges.contains(edge.id()));
+    m_edges.emplace(std::make_pair(edge.id(), std::make_unique<TEdge>(std::move(edge))));
+    m_addMissingNodes(edge.id());
+  }
   template <typename node_t, typename edge_t>
     requires(std::is_base_of_v<Node, node_t> && std::is_base_of_v<Edge, edge_t>)
   template <typename TEdge, typename... TArgs>
diff --git a/src/dsm/headers/Node.hpp b/src/dsm/headers/Node.hpp
index ca452db78..1d56ab9e4 100644
--- a/src/dsm/headers/Node.hpp
+++ b/src/dsm/headers/Node.hpp
@@ -29,29 +29,19 @@ namespace dsm {
     Id m_id;
     std::optional<std::pair<double, double>> m_coords;
     std::string m_name;
-    Size m_capacity;
-    int m_transportCapacity;
 
   public:
     /// @brief Construct a new Node object with capacity 1
     /// @param id The node's id
-    explicit Node(Id id) : m_id{id}, m_name{""}, m_capacity{1}, m_transportCapacity{1} {}
+    explicit Node(Id id) : m_id{id}, m_name{""} {}
     /// @brief Construct a new Node object with capacity 1
     /// @param id The node's id
     /// @param coords A std::pair containing the node's coordinates (lat, lon)
     Node(Id id, std::pair<double, double> coords)
-        : m_id{id},
-          m_coords{std::move(coords)},
-          m_name{""},
-          m_capacity{1},
-          m_transportCapacity{1} {}
+        : m_id{id}, m_coords{std::move(coords)}, m_name{""} {}
 
     Node(Node const& other)
-        : m_id{other.m_id},
-          m_coords{other.m_coords},
-          m_name{other.m_name},
-          m_capacity{other.m_capacity},
-          m_transportCapacity{other.m_transportCapacity} {};
+        : m_id{other.m_id}, m_coords{other.m_coords}, m_name{other.m_name} {};
     virtual ~Node() = default;
 
     Node& operator=(Node const& other) {
@@ -59,8 +49,6 @@ namespace dsm {
         m_id = other.m_id;
         m_coords = other.m_coords;
         m_name = other.m_name;
-        m_capacity = other.m_capacity;
-        m_transportCapacity = other.m_transportCapacity;
       }
       return *this;
     }
@@ -74,18 +62,6 @@ namespace dsm {
     /// @brief Set the node's name
     /// @param name The node's name
     void setName(const std::string& name) { m_name = name; }
-    /// @brief Set the node's capacity
-    /// @param capacity The node's capacity
-    virtual void setCapacity(Size capacity) { m_capacity = capacity; }
-    /// @brief Set the node's transport capacity
-    /// @param capacity The node's transport capacity
-    virtual void setTransportCapacity(int capacity) {
-      if (capacity < 1) {
-        Logger::error(std::format(
-            "The transport capacity of a node ({}) must be greater than 0.", capacity));
-      }
-      m_transportCapacity = capacity;
-    }
     /// @brief Get the node's id
     /// @return Id The node's id
     Id id() const { return m_id; }
@@ -95,19 +71,6 @@ namespace dsm {
     /// @brief Get the node's name
     /// @return std::string The node's name
     const std::string& name() const { return m_name; }
-    /// @brief Get the node's capacity
-    /// @return Size The node's capacity
-    Size capacity() const { return m_capacity; }
-    /// @brief Get the node's transport capacity
-    /// @return Size The node's transport capacity
-    int transportCapacity() const { return m_transportCapacity; }
-
-    virtual double density() const { return 0.; };
-    virtual bool isFull() const { return true; };
-
-    virtual bool isIntersection() const noexcept { return false; }
-    virtual bool isTrafficLight() const noexcept { return false; }
-    virtual bool isRoundabout() const noexcept { return false; }
 
     virtual bool isStation() const noexcept { return false; }
   };
diff --git a/src/dsm/headers/Road.hpp b/src/dsm/headers/Road.hpp
index 1bd1628d7..3d0219d47 100644
--- a/src/dsm/headers/Road.hpp
+++ b/src/dsm/headers/Road.hpp
@@ -2,6 +2,7 @@
 
 #include "Edge.hpp"
 
+#include <memory>
 #include <optional>
 #include <string>
 
@@ -15,12 +16,6 @@ namespace dsm {
     std::string m_name;
 
   public:
-    /// @brief Construct a new Road object starting from an existing road
-    /// @details The new road has different id but same capacity, length, speed limit, and node pair as the
-    ///          existing road.
-    /// @param Road The existing road
-    /// @param id The new road's id
-    Road(Id id, const Road&);
     /// @brief Construct a new Road object
     /// @param id The road's id
     /// @param nodePair The road's node pair
@@ -65,8 +60,6 @@ namespace dsm {
     /// @return std::string The name
     std::string name() const;
 
-    virtual void addAgent(Id agentId) = 0;
-
     virtual int nAgents() const = 0;
     virtual int nMovingAgents() const = 0;
     virtual int nExitingAgents() const = 0;
diff --git a/src/dsm/headers/RoadDynamics.hpp b/src/dsm/headers/RoadDynamics.hpp
index d3c1d19f2..8ea660549 100644
--- a/src/dsm/headers/RoadDynamics.hpp
+++ b/src/dsm/headers/RoadDynamics.hpp
@@ -32,8 +32,6 @@
 #include "Itinerary.hpp"
 #include "RoadNetwork.hpp"
 #include "SparseMatrix.hpp"
-#include "../utility/TypeTraits/is_agent.hpp"
-#include "../utility/TypeTraits/is_itinerary.hpp"
 #include "../utility/Logger.hpp"
 #include "../utility/Typedef.hpp"
 
@@ -45,7 +43,7 @@ namespace dsm {
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
   class RoadDynamics : public Dynamics<RoadNetwork> {
-    std::map<Id, std::unique_ptr<Agent>> m_agents;
+    std::vector<std::unique_ptr<Agent>> m_agents;
     std::unordered_map<Id, std::unique_ptr<Itinerary>> m_itineraries;
 
   protected:
@@ -53,7 +51,6 @@ namespace dsm {
     std::unordered_map<Id, std::array<long, 4>> m_turnMapping;
     std::unordered_map<Id, double> m_streetTails;
     std::vector<std::pair<double, double>> m_travelDTs;
-    std::vector<Id> m_agentsToRemove;
     Time m_previousOptimizationTime, m_previousSpireTime;
 
   private:
@@ -75,7 +72,7 @@ namespace dsm {
     /// @param NodeId The id of the node
     /// @param streetId The id of the incoming street
     /// @return Id The id of the randomly selected next street
-    virtual Id m_nextStreetId(Id agentId,
+    virtual Id m_nextStreetId(std::unique_ptr<Agent> const& pAgent,
                               Id NodeId,
                               std::optional<Id> streetId = std::nullopt);
     /// @brief Increase the turn counts
@@ -85,11 +82,10 @@ namespace dsm {
     /// @param reinsert_agents If true, the agents are reinserted in the simulation after they reach their destination
     /// @details If possible, removes the first agent of the street's queue, putting it in the destination node.
     /// If the agent is going into the destination node, it is removed from the simulation (and then reinserted if reinsert_agents is true)
-    void m_evolveStreet(const std::unique_ptr<Street>& pStreet, bool reinsert_agents);
+    void m_evolveStreet(std::unique_ptr<Street> const& pStreet, bool reinsert_agents);
     /// @brief If possible, removes one agent from the node, putting it on the next street.
     /// @param pNode A std::unique_ptr to the node
-    /// @return bool True if the agent has been moved, false otherwise
-    bool m_evolveNode(const std::unique_ptr<Node>& pNode);
+    void m_evolveNode(const std::unique_ptr<RoadJunction>& pNode);
     /// @brief Evolve the agents.
     /// @details Puts all new agents on a street, if possible, decrements all delays
     /// and increments all travel times.
@@ -138,7 +134,7 @@ namespace dsm {
       requires(std::is_convertible_v<typename TContainer::value_type, Id>)
     void setDestinationNodes(TContainer const& destinationNodes, bool updatePaths = true);
 
-    virtual void setAgentSpeed(Size agentId) = 0;
+    virtual void setAgentSpeed(std::unique_ptr<Agent> const& pAgent) = 0;
 
     /// @brief Update the paths of the itineraries based on the given weight function
     void updatePaths();
@@ -171,19 +167,9 @@ namespace dsm {
     void addAgent(TArgs&&... args);
 
     template <typename... TArgs>
-      requires(std::is_constructible_v<Agent, Time, Id, TArgs...>)
+      requires(std::is_constructible_v<Agent, Time, TArgs...>)
     void addAgents(Size nAgents, TArgs&&... args);
 
-    /// @brief Remove an agent from the simulation
-    /// @param agentId the id of the agent to remove
-    void removeAgent(Size agentId);
-    template <typename T1, typename... Tn>
-      requires(std::is_convertible_v<T1, Id> && (std::is_convertible_v<Tn, Size> && ...))
-    /// @brief Remove a pack of agents from the simulation
-    /// @param id the id of the first agent to remove
-    /// @param ids the pack of ides of the agents to remove
-    void removeAgents(T1 id, Tn... ids);
-
     /// @brief Add an itinerary
     /// @param ...args The arguments to construct the itinerary
     /// @details The arguments must be compatible with any constructor of the Itinerary class
@@ -223,10 +209,10 @@ namespace dsm {
     }
     /// @brief Get the agents
     /// @return const std::unordered_map<Id, Agent<Id>>&, The agents
-    const std::map<Id, std::unique_ptr<Agent>>& agents() const { return m_agents; }
+    const std::vector<std::unique_ptr<Agent>>& agents() const { return m_agents; }
     /// @brief Get the number of agents currently in the simulation
     /// @return Size The number of agents
-    Size nAgents() const { return m_agents.size(); }
+    size_t nAgents() const;
 
     /// @brief Get the mean travel time of the agents in \f$s\f$
     /// @param clearData If true, the travel times are cleared after the computation
@@ -495,10 +481,9 @@ namespace dsm {
 
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
-  Id RoadDynamics<delay_t>::m_nextStreetId(Id agentId,
+  Id RoadDynamics<delay_t>::m_nextStreetId(std::unique_ptr<Agent> const& pAgent,
                                            Id nodeId,
                                            std::optional<Id> streetId) {
-    auto const& pAgent{this->agents().at(agentId)};
     auto possibleMoves = this->graph().adjacencyMatrix().getRow(nodeId);
     if (!pAgent->isRandom()) {
       std::uniform_real_distribution<double> uniformDist{0., 1.};
@@ -512,10 +497,7 @@ namespace dsm {
         }
       }
     }
-    if (possibleMoves.empty()) {
-      Logger::error(
-          std::format("No possible moves from node {} for agent {}", nodeId, agentId));
-    }
+    assert(!possibleMoves.empty());
     std::uniform_int_distribution<Size> moveDist{
         0, static_cast<Size>(possibleMoves.size() - 1)};
     // while loop to avoid U turns in non-roundabout junctions
@@ -551,9 +533,9 @@ namespace dsm {
   void RoadDynamics<delay_t>::m_evolveStreet(const std::unique_ptr<Street>& pStreet,
                                              bool reinsert_agents) {
     auto const& transportCapacity{pStreet->transportCapacity()};
+    auto const nLanes = pStreet->nLanes();
+    std::uniform_real_distribution<double> uniformDist{0., 1.};
     for (auto i = 0; i < std::ceil(transportCapacity); ++i) {
-      auto const nLanes = pStreet->nLanes();
-      std::uniform_real_distribution<double> uniformDist{0., 1.};
       bool bCanPass{true};
       if (pStreet->isStochastic() &&
           (uniformDist(this->m_generator) >
@@ -571,14 +553,16 @@ namespace dsm {
         if (pStreet->queue(queueIndex).empty()) {
           continue;
         }
-        const auto agentId{pStreet->queue(queueIndex).front()};
-        auto const& pAgent{this->agents().at(agentId)};
-        if (pAgent->freeTime() > this->time()) {
+        Logger::debug("Taking temp agent");
+        auto const& pAgentTemp{pStreet->queue(queueIndex).front()};
+        if (pAgentTemp->freeTime() > this->time()) {
+          Logger::debug("Skipping due to time");
           continue;
         }
-        pAgent->setSpeed(0.);
+        pAgentTemp->setSpeed(0.);
         const auto& destinationNode{this->graph().node(pStreet->target())};
         if (destinationNode->isFull()) {
+          Logger::debug("Skipping due to space");
           continue;
         }
         if (destinationNode->isTrafficLight()) {
@@ -592,50 +576,44 @@ namespace dsm {
                    (uniformDist(this->m_generator) < m_passageProbability.value_or(1.1));
         bool bArrived{false};
         if (!bCanPass) {
-          if (pAgent->isRandom()) {
+          if (pAgentTemp->isRandom()) {
             bArrived = true;
           } else {
             continue;
           }
         }
-        if (!pAgent->isRandom()) {
+        if (!pAgentTemp->isRandom()) {
           if (destinationNode->id() ==
-              this->itineraries().at(pAgent->itineraryId())->destination()) {
+              this->itineraries().at(pAgentTemp->itineraryId())->destination()) {
             bArrived = true;
           }
         }
         if (bArrived) {
-          pStreet->dequeue(queueIndex);
+          auto pAgent{pStreet->dequeue(queueIndex)};
           m_travelDTs.push_back(
               {pAgent->distance(),
                static_cast<double>(this->time() - pAgent->spawnTime())});
           if (reinsert_agents) {
             // reset Agent's values
             pAgent->reset(this->time());
-          } else {
-            m_agentsToRemove.push_back(agentId);
-            // this->removeAgent(agentId);
+            this->addAgent(std::move(pAgent));
           }
           continue;
         }
-        auto const& nextStreet{
-            this->graph().edge(this->agents().at(agentId)->nextStreetId().value())};
+        auto const& nextStreet{this->graph().edge(pAgentTemp->nextStreetId().value())};
         if (nextStreet->isFull()) {
           continue;
         }
-        pStreet->dequeue(queueIndex);
-        if (destinationNode->id() != nextStreet->source()) {
-          Logger::error(std::format("Agent {} is going to the wrong street", agentId));
-        }
+        auto pAgent{pStreet->dequeue(queueIndex)};
         assert(destinationNode->id() == nextStreet->source());
         if (destinationNode->isIntersection()) {
           auto& intersection = dynamic_cast<Intersection&>(*destinationNode);
           auto const delta{nextStreet->deltaAngle(pStreet->angle())};
           // m_increaseTurnCounts(pStreet->id(), delta);
-          intersection.addAgent(delta, agentId);
+          intersection.addAgent(delta, std::move(pAgent));
         } else if (destinationNode->isRoundabout()) {
           auto& roundabout = dynamic_cast<Roundabout&>(*destinationNode);
-          roundabout.enqueue(agentId);
+          roundabout.enqueue(std::move(pAgent));
         }
       }
     }
@@ -643,156 +621,77 @@ namespace dsm {
 
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
-  bool RoadDynamics<delay_t>::m_evolveNode(const std::unique_ptr<Node>& pNode) {
-    if (pNode->isIntersection()) {
-      auto& intersection = dynamic_cast<Intersection&>(*pNode);
-      if (intersection.agents().empty()) {
-        return false;
+  void RoadDynamics<delay_t>::m_evolveNode(const std::unique_ptr<RoadJunction>& pNode) {
+    auto const transportCapacity{pNode->transportCapacity()};
+    for (auto i{0}; i < std::ceil(transportCapacity); ++i) {
+      if (i == std::ceil(transportCapacity) - 1) {
+        std::uniform_real_distribution<double> uniformDist{0., 1.};
+        double integral;
+        double fractional = std::modf(transportCapacity, &integral);
+        if (fractional != 0. && uniformDist(this->m_generator) > fractional) {
+          return;
+        }
       }
-      for (auto const [angle, agentId] : intersection.agents()) {
-        auto const& nextStreet{
-            this->graph().edge(this->agents().at(agentId)->nextStreetId().value())};
-        if (nextStreet->isFull()) {
-          if (m_forcePriorities) {
-            return false;
+      if (pNode->isIntersection()) {
+        auto& intersection = dynamic_cast<Intersection&>(*pNode);
+        if (intersection.agents().empty()) {
+          Logger::debug(std::format("No agents on node {}", pNode->id()));
+          return;
+        }
+        for (auto it{intersection.agents().begin()}; it != intersection.agents().end();) {
+          auto& pAgent{it->second};
+          auto const& nextStreet{this->graph().edge(pAgent->nextStreetId().value())};
+          if (nextStreet->isFull()) {
+            Logger::debug(std::format("Next street {} is full", nextStreet->id()));
+            if (m_forcePriorities) {
+              return;
+            }
+            ++it;
+            continue;
           }
-          continue;
+          pAgent->setStreetId();
+          this->setAgentSpeed(pAgent);
+          pAgent->setFreeTime(this->time() +
+                              std::ceil(nextStreet->length() / pAgent->speed()));
+          nextStreet->addAgent(std::move(pAgent));
+          it = intersection.agents().erase(it);
+          break;
         }
-        intersection.removeAgent(agentId);
-        this->agents().at(agentId)->setStreetId(nextStreet->id());
-        this->setAgentSpeed(agentId);
-        this->agents().at(agentId)->setFreeTime(
-            this->time() +
-            std::ceil(nextStreet->length() / this->agents().at(agentId)->speed()));
-        nextStreet->addAgent(agentId);
-        return true;
-      }
-      return false;
-    } else if (pNode->isRoundabout()) {
-      auto& roundabout = dynamic_cast<Roundabout&>(*pNode);
-      if (roundabout.agents().empty()) {
-        return false;
-      }
-      auto const agentId{roundabout.agents().front()};
-      auto const& nextStreet{
-          this->graph().edge(this->agents().at(agentId)->nextStreetId().value())};
-      if (!(nextStreet->isFull())) {
-        if (this->agents().at(agentId)->streetId().has_value()) {
-          const auto streetId = this->agents().at(agentId)->streetId().value();
-          auto delta = nextStreet->angle() - this->graph().edge(streetId)->angle();
-          if (delta > std::numbers::pi) {
-            delta -= 2 * std::numbers::pi;
-          } else if (delta < -std::numbers::pi) {
-            delta += 2 * std::numbers::pi;
+      } else if (pNode->isRoundabout()) {
+        auto& roundabout = dynamic_cast<Roundabout&>(*pNode);
+        if (roundabout.agents().empty()) {
+          return;
+        }
+        auto const& pAgentTemp{roundabout.agents().front()};
+        auto const& nextStreet{this->graph().edge(pAgentTemp->nextStreetId().value())};
+        if (!(nextStreet->isFull())) {
+          if (pAgentTemp->streetId().has_value()) {
+            const auto streetId = pAgentTemp->streetId().value();
+            auto delta = nextStreet->angle() - this->graph().edge(streetId)->angle();
+            if (delta > std::numbers::pi) {
+              delta -= 2 * std::numbers::pi;
+            } else if (delta < -std::numbers::pi) {
+              delta += 2 * std::numbers::pi;
+            }
+            m_increaseTurnCounts(streetId, delta);
           }
-          m_increaseTurnCounts(streetId, delta);
+          auto pAgent{roundabout.dequeue()};
+          pAgent->setStreetId();
+          this->setAgentSpeed(pAgent);
+          pAgent->setFreeTime(this->time() +
+                              std::ceil(nextStreet->length() / pAgent->speed()));
+          nextStreet->addAgent(std::move(pAgent));
+        } else {
+          return;
         }
-        roundabout.dequeue();
-        this->agents().at(agentId)->setStreetId(nextStreet->id());
-        this->setAgentSpeed(agentId);
-        this->agents().at(agentId)->setFreeTime(
-            this->time() +
-            std::ceil(nextStreet->length() / this->agents().at(agentId)->speed()));
-        nextStreet->addAgent(agentId);
-      } else {
-        return false;
       }
     }
-    return true;
   }
 
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
   void RoadDynamics<delay_t>::m_evolveAgent(std::unique_ptr<Agent> const& pAgent) {
-    std::uniform_int_distribution<Id> nodeDist{
-        0, static_cast<Id>(this->graph().nNodes() - 1)};
     // The "cost" of enqueuing is one time unit, so we consider it as passed
-    if (pAgent->freeTime() == this->time() + 1 && pAgent->streetId().has_value()) {
-      const auto& street{this->graph().edge(*pAgent->streetId())};
-      auto const nLanes = street->nLanes();
-      bool bArrived{false};
-      if (!pAgent->isRandom()) {
-        if (this->itineraries().at(pAgent->itineraryId())->destination() ==
-            street->target()) {
-          pAgent->updateItinerary();
-        }
-        if (this->itineraries().at(pAgent->itineraryId())->destination() ==
-            street->target()) {
-          bArrived = true;
-        }
-      }
-      pAgent->incrementDistance(street->length());
-      if (bArrived) {
-        std::uniform_int_distribution<size_t> laneDist{0,
-                                                       static_cast<size_t>(nLanes - 1)};
-        street->enqueue(pAgent->id(), laneDist(this->m_generator));
-      } else {
-        auto const nextStreetId =
-            this->m_nextStreetId(pAgent->id(), street->target(), street->id());
-        auto const& pNextStreet{this->graph().edge(nextStreetId)};
-        pAgent->setNextStreetId(nextStreetId);
-        if (nLanes == 1) {
-          street->enqueue(pAgent->id(), 0);
-        } else {
-          auto const deltaAngle{pNextStreet->deltaAngle(street->angle())};
-          if (std::abs(deltaAngle) < std::numbers::pi) {
-            // Lanes are counted as 0 is the far right lane
-            if (std::abs(deltaAngle) < std::numbers::pi / 4) {
-              std::vector<double> weights;
-              for (auto const& queue : street->exitQueues()) {
-                weights.push_back(1. / (queue.size() + 1));
-              }
-              // If all weights are the same, make the last 0
-              if (std::all_of(weights.begin(), weights.end(), [&](double w) {
-                    return std::abs(w - weights.front()) <
-                           std::numeric_limits<double>::epsilon();
-                  })) {
-                weights.back() = 0.;
-                if (nLanes > 2) {
-                  weights.front() = 0.;
-                }
-              }
-              // Normalize the weights
-              auto const sum = std::accumulate(weights.begin(), weights.end(), 0.);
-              for (auto& w : weights) {
-                w /= sum;
-              }
-              std::discrete_distribution<size_t> laneDist{weights.begin(), weights.end()};
-              street->enqueue(pAgent->id(), laneDist(this->m_generator));
-            } else if (deltaAngle < 0.) {                 // Right
-              street->enqueue(pAgent->id(), 0);           // Always the first lane
-            } else {                                      // Left (deltaAngle > 0.)
-              street->enqueue(pAgent->id(), nLanes - 1);  // Always the last lane
-            }
-          } else {                                      // U turn
-            street->enqueue(pAgent->id(), nLanes - 1);  // Always the last lane
-          }
-        }
-      }
-    } else if (!pAgent->streetId().has_value() && !pAgent->nextStreetId().has_value()) {
-      Id srcNodeId = pAgent->srcNodeId().has_value() ? pAgent->srcNodeId().value()
-                                                     : nodeDist(this->m_generator);
-      const auto& srcNode{this->graph().node(srcNodeId)};
-      if (srcNode->isFull()) {
-        return;
-      }
-      const auto& nextStreet{
-          this->graph().edge(this->m_nextStreetId(pAgent->id(), srcNode->id()))};
-      if (nextStreet->isFull()) {
-        return;
-      }
-      assert(srcNode->id() == nextStreet->nodePair().first);
-      if (srcNode->isIntersection()) {
-        auto& intersection = dynamic_cast<Intersection&>(*srcNode);
-        intersection.addAgent(0., pAgent->id());
-      } else if (srcNode->isRoundabout()) {
-        auto& roundabout = dynamic_cast<Roundabout&>(*srcNode);
-        roundabout.enqueue(pAgent->id());
-      }
-      pAgent->setNextStreetId(nextStreet->id());
-    } else if (pAgent->freeTime() == this->time()) {
-      pAgent->setSpeed(0.);
-    }
   }
 
   template <typename delay_t>
@@ -877,10 +776,6 @@ namespace dsm {
         std::advance(itineraryIt, itineraryDist(this->m_generator));
         itineraryId = itineraryIt->first;
       }
-      Id agentId{0};
-      if (!(this->agents().empty())) {
-        agentId = this->agents().rbegin()->first + 1;
-      }
       Id streetId{0};
       do {
         auto streetIt = this->graph().edges().begin();
@@ -890,13 +785,11 @@ namespace dsm {
       } while (this->graph().edge(streetId)->isFull() &&
                this->nAgents() < this->graph().maxCapacity());
       const auto& street{this->graph().edge(streetId)};
-      this->addAgent(agentId, itineraryId, street->nodePair().first);
-      auto const& pAgent{this->agents().at(agentId)};
+      auto pAgent{std::make_unique<Agent>(this->time(), itineraryId, street->source())};
       pAgent->setStreetId(streetId);
-      this->setAgentSpeed(agentId);
+      this->setAgentSpeed(pAgent);
       pAgent->setFreeTime(this->time() + std::ceil(street->length() / pAgent->speed()));
-      street->addAgent(agentId);
-      ++agentId;
+      street->addAgent(std::move(pAgent));
     }
   }
 
@@ -948,10 +841,6 @@ namespace dsm {
         })};
     std::uniform_real_distribution<double> srcUniformDist{0., srcSum};
     std::uniform_real_distribution<double> dstUniformDist{0., dstSum};
-    Id agentId{0};
-    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};
@@ -1005,9 +894,8 @@ namespace dsm {
       if (itineraryIt == this->itineraries().cend()) {
         Logger::error(std::format("Itinerary with destination {} not found.", dstId));
       }
-      this->addAgent(agentId, itineraryIt->first, srcId);
+      this->addAgent(itineraryIt->first, srcId);
       --nAgents;
-      ++agentId;
     }
   }
 
@@ -1027,21 +915,8 @@ namespace dsm {
 
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
-  void RoadDynamics<delay_t>::addAgent(std::unique_ptr<Agent> agent) {
-    if (m_agents.size() + 1 > this->graph().maxCapacity()) {
-      throw std::overflow_error(Logger::buildExceptionMessage(std::format(
-          "RoadNetwork is already holding the max possible number of agents ({})",
-          this->graph().maxCapacity())));
-    }
-    if (m_agents.contains(agent->id())) {
-      throw std::invalid_argument(Logger::buildExceptionMessage(
-          std::format("Agent with id {} already exists.", agent->id())));
-    }
-    m_agents.emplace(agent->id(), std::move(agent));
-    // Logger::info(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()));
+  void RoadDynamics<delay_t>::addAgent(std::unique_ptr<Agent> pAgent) {
+    m_agents.push_back(std::move(pAgent));
   }
 
   template <typename delay_t>
@@ -1055,34 +930,13 @@ namespace dsm {
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
   template <typename... TArgs>
-    requires(std::is_constructible_v<Agent, Time, Id, TArgs...>)
+    requires(std::is_constructible_v<Agent, Time, TArgs...>)
   void RoadDynamics<delay_t>::addAgents(Size nAgents, TArgs&&... args) {
-    Id agentId{0};
-    if (!m_agents.empty()) {
-      agentId = m_agents.rbegin()->first + 1;
-    }
-    for (size_t i{0}; i < nAgents; ++i, ++agentId) {
-      addAgent(
-          std::make_unique<Agent>(this->time(), agentId, std::forward<TArgs>(args)...));
+    for (size_t i{0}; i < nAgents; ++i) {
+      addAgent(std::make_unique<Agent>(this->time(), std::forward<TArgs>(args)...));
     }
   }
 
-  template <typename delay_t>
-    requires(is_numeric_v<delay_t>)
-  void RoadDynamics<delay_t>::removeAgent(Size agentId) {
-    m_agents.erase(agentId);
-    Logger::debug(std::format("Removed agent with id {}", agentId));
-  }
-
-  template <typename delay_t>
-    requires(is_numeric_v<delay_t>)
-  template <typename T1, typename... Tn>
-    requires(std::is_convertible_v<T1, Size> && (std::is_convertible_v<Tn, Size> && ...))
-  void RoadDynamics<delay_t>::removeAgents(T1 id, Tn... ids) {
-    removeAgent(id);
-    removeAgents(ids...);
-  }
-
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
   template <typename... TArgs>
@@ -1108,44 +962,138 @@ namespace dsm {
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
   void RoadDynamics<delay_t>::evolve(bool reinsert_agents) {
+    Logger::debug("Init evolve");
     // move the first agent of each street queue, if possible, putting it in the next node
     bool const bUpdateData =
         m_dataUpdatePeriod.has_value() && this->time() % m_dataUpdatePeriod.value() == 0;
     auto const N{this->graph().nNodes()};
-    std::for_each(this->graph().nodes().cbegin(),
-                  this->graph().nodes().cend(),
-                  [&](const auto& pair) {
-                    for (auto const& sourceId :
-                         this->graph().adjacencyMatrix().getCol(pair.first)) {
-                      auto const streetId = sourceId * N + pair.first;
-                      auto const& pStreet{this->graph().edge(streetId)};
-                      if (bUpdateData) {
-                        m_streetTails[streetId] += pStreet->nExitingAgents();
-                      }
-                      m_evolveStreet(pStreet, reinsert_agents);
+    std::for_each(
+        this->graph().nodes().cbegin(),
+        this->graph().nodes().cend(),
+        [&](const auto& pair) {
+          for (auto const& sourceId :
+               this->graph().adjacencyMatrix().getCol(pair.first)) {
+            auto const streetId = sourceId * N + pair.first;
+            auto const& pStreet{this->graph().edge(streetId)};
+            if (bUpdateData) {
+              m_streetTails[streetId] += pStreet->nExitingAgents();
+            }
+            m_evolveStreet(pStreet, reinsert_agents);
+
+            while (!pStreet->movingAgents().empty()) {
+              auto const& pAgent{pStreet->movingAgents().top()};
+              if (pAgent->freeTime() < this->time()) {
+                break;
+              }
+              pAgent->setSpeed(0.);
+              auto const nLanes = pStreet->nLanes();
+              bool bArrived{false};
+              if (!pAgent->isRandom()) {
+                if (this->itineraries().at(pAgent->itineraryId())->destination() ==
+                    pStreet->target()) {
+                  pAgent->updateItinerary();
+                }
+                if (this->itineraries().at(pAgent->itineraryId())->destination() ==
+                    pStreet->target()) {
+                  bArrived = true;
+                }
+              }
+              if (bArrived) {
+                std::uniform_int_distribution<size_t> laneDist{
+                    0, static_cast<size_t>(nLanes - 1)};
+                pStreet->enqueue(laneDist(this->m_generator));
+                continue;
+              }
+              auto const nextStreetId =
+                  this->m_nextStreetId(pAgent, pStreet->target(), pStreet->id());
+              auto const& pNextStreet{this->graph().edge(nextStreetId)};
+              pAgent->setNextStreetId(nextStreetId);
+              if (nLanes == 1) {
+                pStreet->enqueue(0);
+                continue;
+              }
+              auto const deltaAngle{pNextStreet->deltaAngle(pStreet->angle())};
+              if (std::abs(deltaAngle) < std::numbers::pi) {
+                // Lanes are counted as 0 is the far right lane
+                if (std::abs(deltaAngle) < std::numbers::pi / 4) {
+                  std::vector<double> weights;
+                  for (auto const& queue : pStreet->exitQueues()) {
+                    weights.push_back(1. / (queue.size() + 1));
+                  }
+                  // If all weights are the same, make the last 0
+                  if (std::all_of(weights.begin(), weights.end(), [&](double w) {
+                        return std::abs(w - weights.front()) <
+                               std::numeric_limits<double>::epsilon();
+                      })) {
+                    weights.back() = 0.;
+                    if (nLanes > 2) {
+                      weights.front() = 0.;
                     }
-                  });
-    std::for_each(this->m_agentsToRemove.cbegin(),
-                  this->m_agentsToRemove.cend(),
-                  [this](const auto& agentId) { this->removeAgent(agentId); });
-    m_agentsToRemove.clear();
+                  }
+                  // Normalize the weights
+                  auto const sum = std::accumulate(weights.begin(), weights.end(), 0.);
+                  for (auto& w : weights) {
+                    w /= sum;
+                  }
+                  std::discrete_distribution<size_t> laneDist{weights.begin(),
+                                                              weights.end()};
+                  pStreet->enqueue(laneDist(this->m_generator));
+                } else if (deltaAngle < 0.) {    // Right
+                  pStreet->enqueue(0);           // Always the first lane
+                } else {                         // Left (deltaAngle > 0.)
+                  pStreet->enqueue(nLanes - 1);  // Always the last lane
+                }
+              } else {                         // U turn
+                pStreet->enqueue(nLanes - 1);  // Always the last lane
+              }
+            }
+          }
+        });
+    Logger::debug("Pre-nodes");
     // Move transport capacity agents from each node
     std::for_each(this->graph().nodes().cbegin(),
                   this->graph().nodes().cend(),
                   [&](const auto& pair) {
-                    for (auto i = 0; i < pair.second->transportCapacity(); ++i) {
-                      m_evolveNode(pair.second);
-                    }
+                    m_evolveNode(pair.second);
                     if (pair.second->isTrafficLight()) {
                       auto& tl = dynamic_cast<TrafficLight&>(*pair.second);
                       ++tl;
                     }
                   });
     // cycle over agents and update their times
-    std::for_each(this->agents().cbegin(),
-                  this->agents().cend(),
-                  [this](auto const& pair) { m_evolveAgent(pair.second); });
-
+    std::uniform_int_distribution<Id> nodeDist{
+        0, static_cast<Id>(this->graph().nNodes() - 1)};
+    Logger::debug("Pre-agents");
+    for (auto itAgent{m_agents.begin()}; itAgent != m_agents.end();) {
+      auto& pAgent{*itAgent};
+      if (!pAgent->srcNodeId().has_value()) {
+        pAgent->setSrcNodeId(nodeDist(this->m_generator));
+      }
+      auto const& srcNode{this->graph().node(pAgent->srcNodeId().value())};
+      if (srcNode->isFull()) {
+        ++itAgent;
+        continue;
+      }
+      if (!pAgent->nextStreetId().has_value()) {
+        pAgent->setNextStreetId(
+            this->m_nextStreetId(pAgent, srcNode->id(), pAgent->streetId()));
+      }
+      auto const& nextStreet{
+          this->graph().edge(pAgent->nextStreetId().value())};  // next street
+      if (nextStreet->isFull()) {
+        ++itAgent;
+        continue;
+      }
+      assert(srcNode->id() == nextStreet->source());
+      if (srcNode->isIntersection()) {
+        auto& intersection = dynamic_cast<Intersection&>(*srcNode);
+        intersection.addAgent(0., std::move(pAgent));
+      } else if (srcNode->isRoundabout()) {
+        auto& roundabout = dynamic_cast<Roundabout&>(*srcNode);
+        roundabout.enqueue(std::move(pAgent));
+      }
+      itAgent = m_agents.erase(itAgent);
+    }
     Dynamics<RoadNetwork>::m_evolve();
   }
 
@@ -1253,6 +1201,28 @@ namespace dsm {
     m_previousOptimizationTime = this->time();
   }
 
+  template <typename delay_t>
+    requires(is_numeric_v<delay_t>)
+  size_t RoadDynamics<delay_t>::nAgents() const {
+    auto nAgents{m_agents.size()};
+    Logger::debug(std::format("Number of agents: {}", nAgents));
+    for (const auto& [nodeId, pNode] : this->graph().nodes()) {
+      if (pNode->isIntersection()) {
+        auto& intersection = dynamic_cast<Intersection&>(*pNode);
+        nAgents += intersection.agents().size();
+      } else if (pNode->isRoundabout()) {
+        auto& roundabout = dynamic_cast<Roundabout&>(*pNode);
+        nAgents += roundabout.agents().size();
+      }
+      Logger::debug(std::format("Number of agents: {}", nAgents));
+    }
+    for (const auto& [streetId, pStreet] : this->graph().edges()) {
+      nAgents += pStreet->nAgents();
+    }
+    Logger::debug(std::format("Number of agents: {}", nAgents));
+    return nAgents;
+  }
+
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
   Measurement<double> RoadDynamics<delay_t>::meanTravelTime(bool clearData) {
@@ -1324,12 +1294,12 @@ namespace dsm {
     requires(is_numeric_v<delay_t>)
   Measurement<double> RoadDynamics<delay_t>::agentMeanSpeed() const {
     std::vector<double> speeds;
-    if (!this->agents().empty()) {
-      speeds.reserve(this->nAgents());
-      for (const auto& [agentId, agent] : this->agents()) {
-        speeds.push_back(agent->speed());
-      }
-    }
+    // if (!this->agents().empty()) {
+    //   speeds.reserve(this->nAgents());
+    //   for (const auto& [agentId, agent] : this->agents()) {
+    //     speeds.push_back(agent->speed());
+    //   }
+    // }
     return Measurement<double>(speeds);
   }
 
@@ -1342,8 +1312,8 @@ namespace dsm {
       return 0.;
     }
     double speed{0.};
-    for (auto const& agentId : pStreet->movingAgents()) {
-      speed += this->agents().at(agentId)->speed();
+    for (auto const& pAgent : pStreet->movingAgents()) {
+      speed += pAgent->speed();
     }
     return speed / nAgents;
   }
@@ -1611,21 +1581,23 @@ namespace dsm {
       Logger::error(std::format("Error opening file \"{}\" for writing.", filename));
     }
     if (bEmptyFile) {
-      file << "time;n_agents;mean_speed;mean_speed_std;mean_density;mean_density_std;"
+      file << "time;n_ghost_agents;n_agents;mean_speed;mean_speed_std;mean_density;mean_"
+              "density_std;"
               "mean_flow;mean_flow_std;mean_flow_spires;mean_flow_spires_std;mean_"
               "traveltime;mean_traveltime_std;mean_traveldistance;mean_traveldistance_"
               "err;mean_travelspeed;mean_travelspeed_std\n";
     }
     file << this->time() << separator;
     file << m_agents.size() << separator;
-    file << std::scientific << std::setprecision(2);
     {
       std::vector<double> speeds, densities, flows, spireFlows;
+      size_t nAgents{0};
       speeds.reserve(this->graph().nEdges());
       densities.reserve(this->graph().nEdges());
       flows.reserve(this->graph().nEdges());
       spireFlows.reserve(this->graph().nEdges());
       for (auto const& [streetId, street] : this->graph().edges()) {
+        nAgents += street->nAgents();
         speeds.push_back(this->streetMeanSpeed(streetId));
         densities.push_back(street->density(true));
         flows.push_back(street->density(true) * speeds.back());
@@ -1635,10 +1607,21 @@ namespace dsm {
                                (this->time() - m_previousSpireTime));
         }
       }
+      for (auto const& [nodeId, pNode] : this->graph().nodes()) {
+        if (pNode->isIntersection()) {
+          auto& intersection = dynamic_cast<Intersection&>(*pNode);
+          nAgents += intersection.agents().size();
+        } else if (pNode->isRoundabout()) {
+          auto& roundabout = dynamic_cast<Roundabout&>(*pNode);
+          nAgents += roundabout.agents().size();
+        }
+      }
       auto speed{Measurement<double>(speeds)};
       auto density{Measurement<double>(densities)};
       auto flow{Measurement<double>(flows)};
       auto spireFlow{Measurement<double>(spireFlows)};
+      file << nAgents << separator;
+      file << std::scientific << std::setprecision(2);
       file << speed.mean << separator << speed.std << separator;
       file << density.mean << separator << density.std << separator;
       file << flow.mean << separator << flow.std << separator;
diff --git a/src/dsm/headers/RoadJunction.hpp b/src/dsm/headers/RoadJunction.hpp
new file mode 100644
index 000000000..ab8328ccc
--- /dev/null
+++ b/src/dsm/headers/RoadJunction.hpp
@@ -0,0 +1,47 @@
+#pragma once
+
+#include "Node.hpp"
+
+#include "../utility/Typedef.hpp"
+
+namespace dsm {
+  class RoadJunction : public Node {
+    Size m_capacity;
+    double m_transportCapacity;
+
+  public:
+    explicit RoadJunction(Id id);
+    RoadJunction(Id id, std::pair<double, double> coords);
+    RoadJunction(RoadJunction const& other);
+
+    RoadJunction& operator=(RoadJunction const& other) {
+      if (this != &other) {
+        Node::operator=(other);
+        m_capacity = other.m_capacity;
+        m_transportCapacity = other.m_transportCapacity;
+      }
+      return *this;
+    }
+
+    /// @brief Set the junction's capacity
+    /// @param capacity The junction's capacity
+    virtual void setCapacity(Size capacity);
+    /// @brief Set the junction's transport capacity
+    /// @param capacity The junction's transport capacity
+    void setTransportCapacity(double capacity);
+
+    /// @brief Get the junction's capacity
+    /// @return Size The junction's capacity
+    Size capacity() const;
+    /// @brief Get the junction's transport capacity
+    /// @return Size The junction's transport capacity
+    double transportCapacity() const;
+
+    virtual double density() const;
+    virtual bool isFull() const;
+
+    virtual bool isIntersection() const noexcept;
+    virtual bool isTrafficLight() const noexcept;
+    virtual bool isRoundabout() const noexcept;
+  };
+}  // namespace dsm
\ No newline at end of file
diff --git a/src/dsm/headers/RoadNetwork.hpp b/src/dsm/headers/RoadNetwork.hpp
index ccce2af72..5bcee31fb 100644
--- a/src/dsm/headers/RoadNetwork.hpp
+++ b/src/dsm/headers/RoadNetwork.hpp
@@ -29,6 +29,7 @@
 #include "AdjacencyMatrix.hpp"
 #include "DijkstraWeights.hpp"
 #include "Network.hpp"
+#include "RoadJunction.hpp"
 #include "Intersection.hpp"
 #include "TrafficLight.hpp"
 #include "Roundabout.hpp"
@@ -45,7 +46,7 @@ namespace dsm {
   /// @brief The RoadNetwork class represents a graph in the network.
   /// @tparam Id, The type of the graph's id. It must be an unsigned integral type.
   /// @tparam Size, The type of the graph's capacity. It must be an unsigned integral type.
-  class RoadNetwork : public Network<Node, Street> {
+  class RoadNetwork : public Network<RoadJunction, Street> {
   private:
     std::unordered_map<std::string, Id> m_nodeMapping;
     std::vector<Id> m_inputNodes;
@@ -71,39 +72,6 @@ namespace dsm {
     /// @param streetSet A map of streets representing the graph's streets
     RoadNetwork(const std::unordered_map<Id, std::unique_ptr<Street>>& streetSet);
 
-    RoadNetwork(const RoadNetwork& other) : Network{AdjacencyMatrix()} {
-      std::for_each(other.m_nodes.begin(), other.m_nodes.end(), [this](const auto& pair) {
-        this->m_nodes.emplace(pair.first, pair.second.get());
-      });
-      std::for_each(other.m_edges.begin(), other.m_edges.end(), [this](const auto& pair) {
-        this->m_edges.emplace(pair.first, pair.second.get());
-      });
-      m_nodeMapping = other.m_nodeMapping;
-      m_adjacencyMatrix = other.m_adjacencyMatrix;
-      m_inputNodes = other.m_inputNodes;
-      m_outputNodes = other.m_outputNodes;
-    }
-
-    RoadNetwork& operator=(const RoadNetwork& other) {
-      std::for_each(other.m_nodes.begin(), other.m_nodes.end(), [this](const auto& pair) {
-        this->m_nodes.insert_or_assign(pair.first,
-                                       std::unique_ptr<Node>(pair.second.get()));
-      });
-      std::for_each(other.m_edges.begin(), other.m_edges.end(), [this](const auto& pair) {
-        this->m_edges.insert_or_assign(pair.first,
-                                       std::make_unique<Street>(*pair.second));
-      });
-      m_nodeMapping = other.m_nodeMapping;
-      m_adjacencyMatrix = other.m_adjacencyMatrix;
-      m_inputNodes = other.m_inputNodes;
-      m_outputNodes = other.m_outputNodes;
-
-      return *this;
-    }
-
-    RoadNetwork(RoadNetwork&&) = default;
-    RoadNetwork& operator=(RoadNetwork&&) = default;
-
     /// @brief Build the graph's adjacency matrix and computes max capacity
     /// @details The adjacency matrix is built using the graph's streets and nodes. N.B.: The street ids
     /// are reassigned using the max node id, i.e. newStreetId = srcId * n + dstId, where n is the max node id.
@@ -174,7 +142,7 @@ namespace dsm {
     /// @throws std::invalid_argument if the node does not exist
     Roundabout& makeRoundabout(Id nodeId);
 
-    StochasticStreet& makeStochasticStreet(Id streetId, double const flowRate);
+    void makeStochasticStreet(Id streetId, double const flowRate);
     /// @brief Convert an existing street into a spire street
     /// @param streetId The id of the street to convert to a spire street
     /// @throws std::invalid_argument if the street does not exist
@@ -188,7 +156,7 @@ namespace dsm {
 
     /// @brief Add a street to the graph
     /// @param street A reference to the street to add
-    void addStreet(const Street& street);
+    void addStreet(Street&& street);
 
     template <typename T1>
       requires is_street_v<std::remove_reference_t<T1>>
@@ -265,14 +233,14 @@ namespace dsm {
       throw std::invalid_argument(Logger::buildExceptionMessage(
           std::format("Street with id {} already exists.", street.id())));
     }
-    addEdge<Street>(street.id(), street);
+    addEdge(std::move(street));
   }
 
   template <typename T1, typename... Tn>
     requires is_street_v<std::remove_reference_t<T1>> &&
              (is_street_v<std::remove_reference_t<Tn>> && ...)
   void RoadNetwork::addStreets(T1&& street, Tn&&... streets) {
-    addStreet(std::forward<T1>(street));
+    addStreet(std::move(street));
     addStreets(std::forward<Tn>(streets)...);
   }
 
diff --git a/src/dsm/headers/Roundabout.hpp b/src/dsm/headers/Roundabout.hpp
index b59862cbe..7634c4fec 100644
--- a/src/dsm/headers/Roundabout.hpp
+++ b/src/dsm/headers/Roundabout.hpp
@@ -7,49 +7,52 @@
 
 #pragma once
 
-#include "Node.hpp"
+#include "Agent.hpp"
+#include "RoadJunction.hpp"
 #include "../utility/queue.hpp"
 
+#include <memory>
+
 namespace dsm {
   /// @brief The Roundabout class represents a roundabout node in the network.
   /// @tparam Id The type of the node's id
   /// @tparam Size The type of the node's capacity
-  class Roundabout : public Node {
+  class Roundabout : public RoadJunction {
   protected:
-    dsm::queue<Id> m_agents;
+    dsm::queue<std::unique_ptr<Agent>> m_agents;
 
   public:
     /// @brief Construct a new Roundabout object
     /// @param id The node's id
-    explicit Roundabout(Id id) : Node{id} {};
+    explicit Roundabout(Id id) : RoadJunction{id} {};
     /// @brief Construct a new Roundabout object
     /// @param id The node's id
     /// @param coords A std::pair containing the node's coordinates
-    Roundabout(Id id, std::pair<double, double> coords) : Node{id, coords} {};
+    Roundabout(Id id, std::pair<double, double> coords) : RoadJunction{id, coords} {};
     /// @brief Construct a new Roundabout object
     /// @param node An Intersection object
-    Roundabout(const Node& node);
+    Roundabout(const RoadJunction& node);
 
     ~Roundabout() = default;
 
     /// @brief Put an agent in the node
     /// @param agentId The agent's id
     /// @throws std::runtime_error if the node is full
-    void enqueue(Id agentId);
+    void enqueue(std::unique_ptr<Agent> agentId);
     /// @brief Removes the first agent from the node
     /// @return Id The agent's id
-    Id dequeue();
+    std::unique_ptr<Agent> dequeue();
     /// @brief Get the node's queue
     /// @return dsm::queue<Id> The node's queue
-    const dsm::queue<Id>& agents() const { return m_agents; }
+    dsm::queue<std::unique_ptr<Agent>> const& agents() const { return m_agents; }
     /// @brief Returns the node's density
     /// @return double The node's density
     double density() const override {
-      return static_cast<double>(m_agents.size()) / m_capacity;
+      return static_cast<double>(m_agents.size()) / this->capacity();
     }
     /// @brief Returns true if the node is full
     /// @return bool True if the node is full
-    bool isFull() const override { return m_agents.size() == this->m_capacity; }
+    bool isFull() const override { return m_agents.size() == this->capacity(); }
     /// @brief Returns true if the node is a roundabout
     /// @return bool True if the node is a roundabout
     bool isRoundabout() const noexcept override { return true; }
diff --git a/src/dsm/headers/Station.hpp b/src/dsm/headers/Station.hpp
index dbb6e21f6..c9797eee6 100644
--- a/src/dsm/headers/Station.hpp
+++ b/src/dsm/headers/Station.hpp
@@ -6,12 +6,12 @@
 
 #pragma once
 
-#include "Node.hpp"
+#include "RoadJunction.hpp"
 
 #include <map>
 
 namespace dsm {
-  class Station : public Node {
+  class Station : public RoadJunction {
   private:
     Delay m_managementTime;
     std::multimap<train_t, Id, std::greater<train_t>> m_trains;
@@ -29,7 +29,7 @@ namespace dsm {
     /// @brief Construct a new Station object
     /// @param node A Node object representing the station
     /// @param managementTime The time it takes between two train departures/arrivals
-    Station(Node const& node, Delay managementTime);
+    Station(RoadJunction const& node, Delay managementTime);
     /// @brief Construct a new Station object by copying another Station object
     /// @param other The Station object to copy
     Station(Station const& other);
@@ -45,10 +45,10 @@ namespace dsm {
     Delay managementTime() const;
     /// @brief Get the train density of the station
     /// @return The train density of the station
-    double density() const final;
+    double density() const;
     /// @brief Check if the station is full
     /// @return True if the station is full, false otherwise
-    bool isFull() const final;
+    bool isFull() const;
     /// @brief Check if the node is a station
     /// @return True
     bool isStation() const noexcept final;
diff --git a/src/dsm/headers/Street.hpp b/src/dsm/headers/Street.hpp
index 742aac957..2cbfc75ca 100644
--- a/src/dsm/headers/Street.hpp
+++ b/src/dsm/headers/Street.hpp
@@ -15,14 +15,13 @@
 #include <stdexcept>
 #include <cmath>
 #include <numbers>
-#include <set>
 #include <format>
 #include <cassert>
 #include <string>
 #include <vector>
 
-#include "Road.hpp"
 #include "Agent.hpp"
+#include "Road.hpp"
 #include "Sensors.hpp"
 #include "../utility/TypeTraits/is_numeric.hpp"
 #include "../utility/queue.hpp"
@@ -30,20 +29,28 @@
 #include "../utility/Typedef.hpp"
 
 namespace dsm {
+
+  class AgentComparator {
+  public:
+    template <typename T>
+    bool operator()(T const& lhs, T const& rhs) const {
+      return lhs->freeTime() > rhs->freeTime();
+    }
+  };
+
+  class Agent;
+
   /// @brief The Street class represents a street in the network.
   class Street : public Road {
   private:
-    std::vector<dsm::queue<Size>> m_exitQueues;
+    std::vector<dsm::queue<std::unique_ptr<Agent>>> m_exitQueues;
+    dsm::priority_queue<std::unique_ptr<Agent>,
+                        std::vector<std::unique_ptr<Agent>>,
+                        AgentComparator>
+        m_movingAgents;
     std::vector<Direction> m_laneMapping;
-    std::vector<Id> m_movingAgents;
 
   public:
-    /// @brief Construct a new Street object starting from an existing street
-    /// @details The new street has different id but same capacity, length, speed limit, and node pair as the
-    ///          existing street.
-    /// @param Street The existing street
-    /// @param id The new street's id
-    Street(Id id, const Street&);
     /// @brief Construct a new Street object
     /// @param id The street's id
     /// @param nodePair The street's node pair
@@ -62,11 +69,12 @@ namespace dsm {
            std::vector<std::pair<double, double>> geometry = {},
            std::optional<int> capacity = std::nullopt,
            double transportCapacity = 1.);
-    virtual ~Street() = default;
+    Street(Street&&) = default;
+    Street(Street const&) = delete;
 
     /// @brief Set the street's queue
     /// @param queue The street's queue
-    inline void setQueue(dsm::queue<Size> queue, size_t index) {
+    inline void setQueue(dsm::queue<std::unique_ptr<Agent>> queue, size_t index) {
       m_exitQueues[index] = std::move(queue);
     }
     /// @brief Set the mean vehicle length
@@ -74,15 +82,16 @@ namespace dsm {
     /// @throw std::invalid_argument If the mean vehicle length is negative
     static void setMeanVehicleLength(double meanVehicleLength);
 
-    /// @brief Get the street's waiting agents
-    /// @return std::set<Id>, The street's waiting agents
-    std::vector<Id> const& movingAgents() const;
     /// @brief Get the street's queue
     /// @return dsm::queue<Size>, The street's queue
-    const dsm::queue<Size>& queue(size_t index) const { return m_exitQueues[index]; }
+    const dsm::queue<std::unique_ptr<Agent>>& queue(size_t const& index) const {
+      return m_exitQueues[index];
+    }
     /// @brief Get the street's queues
     /// @return std::vector<dsm::queue<Size>> The street's queues
-    const std::vector<dsm::queue<Size>>& exitQueues() const { return m_exitQueues; }
+    std::vector<dsm::queue<std::unique_ptr<Agent>>> const& exitQueues() const {
+      return m_exitQueues;
+    }
     /// @brief  Get the number of agents on the street
     /// @return Size, The number of agents on the street
     int nAgents() const final;
@@ -93,6 +102,13 @@ namespace dsm {
     /// @brief Check if the street is full
     /// @return bool, True if the street is full, false otherwise
     bool isFull() const final { return nAgents() == m_capacity; }
+
+    dsm::priority_queue<std::unique_ptr<Agent>,
+                        std::vector<std::unique_ptr<Agent>>,
+                        AgentComparator>&
+    movingAgents() {
+      return m_movingAgents;
+    }
     int nMovingAgents() const override { return m_movingAgents.size(); }
     /// @brief Get the number of agents on all queues
     /// @return Size The number of agents on all queues
@@ -100,14 +116,14 @@ namespace dsm {
 
     inline std::vector<Direction> const& laneMapping() const { return m_laneMapping; }
 
-    void addAgent(Id agentId) override;
+    virtual void addAgent(std::unique_ptr<Agent> pAgent);
     /// @brief Add an agent to the street's queue
     /// @param agentId The id of the agent to add to the street's queue
     /// @throw std::runtime_error If the street's queue is full
-    void enqueue(Id agentId, size_t index);
+    void enqueue(size_t const& queueId);
     /// @brief Remove an agent from the street's queue
     /// @return Id The id of the agent removed from the street's queue
-    virtual Id dequeue(size_t index);
+    virtual std::unique_ptr<Agent> 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; };
@@ -125,7 +141,7 @@ namespace dsm {
     double m_flowRate;
 
   public:
-    StochasticStreet(Id id, const Street& street, double flowRate);
+    StochasticStreet(Street&&, double flowRate);
     StochasticStreet(Id id,
                      std::pair<Id, Id> nodePair,
                      double length = Road::meanVehicleLength(),
@@ -150,12 +166,15 @@ namespace dsm {
   private:
   public:
     using Street::Street;
+    SpireStreet(Street&& street) : Street(std::move(street)) {}
+    SpireStreet(SpireStreet&&) = default;
+    SpireStreet(SpireStreet const&) = delete;
     ~SpireStreet() = default;
 
     /// @brief Add an agent to the street's queue
     /// @param agentId The id of the agent to add to the street's queue
     /// @throw std::runtime_error If the street's queue is full
-    void addAgent(Id agentId) final;
+    void addAgent(std::unique_ptr<Agent> pAgent) final;
 
     /// @brief Get the mean flow of the street
     /// @return int The flow of the street, i.e. the difference between input and output flows
@@ -164,7 +183,7 @@ namespace dsm {
     int meanFlow();
     /// @brief Remove an agent from the street's queue
     /// @return Id The id of the agent removed from the street's queue
-    Id dequeue(size_t index) final;
+    std::unique_ptr<Agent> 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; };
@@ -176,7 +195,7 @@ namespace dsm {
     /// @brief Add an agent to the street's queue
     /// @param agentId The id of the agent to add to the street's queue
     /// @throw std::runtime_error If the street's queue is full
-    void addAgent(Id agentId) final;
+    void addAgent(std::unique_ptr<Agent> pAgent) final;
 
     /// @brief Get the mean flow of the street
     /// @return int The flow of the street, i.e. the difference between input and output flows
@@ -185,7 +204,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
-    Id dequeue(size_t index) final;
+    std::unique_ptr<Agent> 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; };
diff --git a/src/dsm/headers/TrafficLight.hpp b/src/dsm/headers/TrafficLight.hpp
index 33190e339..fb81736ac 100644
--- a/src/dsm/headers/TrafficLight.hpp
+++ b/src/dsm/headers/TrafficLight.hpp
@@ -65,7 +65,7 @@ namespace dsm {
     TrafficLight(Id id, Delay cycleTime, std::pair<double, double> coords)
         : Intersection{id, std::move(coords)}, m_cycleTime{cycleTime}, m_counter{0} {}
 
-    TrafficLight(Node const& node, Delay const cycleTime, Delay const counter = 0)
+    TrafficLight(RoadJunction const& node, Delay const cycleTime, Delay const counter = 0)
         : Intersection{node}, m_cycleTime{cycleTime}, m_counter{counter} {}
 
     ~TrafficLight() = default;
diff --git a/src/dsm/sources/Agent.cpp b/src/dsm/sources/Agent.cpp
index 7624a224d..47b1b4df8 100644
--- a/src/dsm/sources/Agent.cpp
+++ b/src/dsm/sources/Agent.cpp
@@ -2,12 +2,11 @@
 
 namespace dsm {
   Agent::Agent(Time const& spawnTime,
-               Id id,
                std::optional<Id> itineraryId,
                std::optional<Id> srcNodeId)
       : m_spawnTime{spawnTime},
         m_freeTime{0},
-        m_id{id},
+        m_id{0},
         m_trip{itineraryId.has_value() ? std::vector<Id>{*itineraryId}
                                        : std::vector<Id>{}},
         m_srcNodeId{srcNodeId},
@@ -16,12 +15,11 @@ namespace dsm {
         m_speed{0.},
         m_distance{0.} {}
   Agent::Agent(Time const& spawnTime,
-               Id id,
                std::vector<Id> const& trip,
                std::optional<Id> srcNodeId)
       : m_spawnTime{spawnTime},
         m_freeTime{spawnTime},
-        m_id{id},
+        m_id{0},
         m_trip{trip},
         m_srcNodeId{srcNodeId},
         m_nextStreetId{std::nullopt},
@@ -29,6 +27,7 @@ namespace dsm {
         m_speed{0.},
         m_distance{0.} {}
 
+  void Agent::setSrcNodeId(Id srcNodeId) { m_srcNodeId = srcNodeId; }
   void Agent::setStreetId(std::optional<Id> streetId) {
     if (!streetId.has_value()) {
       assert(m_nextStreetId.has_value());
diff --git a/src/dsm/sources/Edge.cpp b/src/dsm/sources/Edge.cpp
index bf668deba..8e5409da2 100644
--- a/src/dsm/sources/Edge.cpp
+++ b/src/dsm/sources/Edge.cpp
@@ -41,6 +41,7 @@ namespace dsm {
     assert(!(std::abs(m_angle) > 2 * std::numbers::pi));
   }
 
+  void Edge::resetId(Id newId) { m_id = newId; }
   void Edge::setCapacity(int capacity) {
     if (capacity < 1) {
       Logger::error(std::format("Edge capacity ({}) must be greater than 0.", capacity));
diff --git a/src/dsm/sources/FirstOrderDynamics.cpp b/src/dsm/sources/FirstOrderDynamics.cpp
index 3ad234fc4..df5674954 100644
--- a/src/dsm/sources/FirstOrderDynamics.cpp
+++ b/src/dsm/sources/FirstOrderDynamics.cpp
@@ -29,16 +29,15 @@ namespace dsm {
     }
   }
 
-  void FirstOrderDynamics::setAgentSpeed(Size agentId) {
-    const auto& agent{this->agents().at(agentId)};
-    const auto& street{this->graph().edge(agent->streetId().value())};
+  void FirstOrderDynamics::setAgentSpeed(std::unique_ptr<Agent> const& pAgent) {
+    const auto& street{this->graph().edge(pAgent->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};
       speed = speedDist(this->m_generator);
     }
-    speed < 0. ? agent->setSpeed(street->maxSpeed() * (1. - m_alpha))
-               : agent->setSpeed(speed);
+    speed < 0. ? pAgent->setSpeed(street->maxSpeed() * (1. - m_alpha))
+               : pAgent->setSpeed(speed);
   }
 
   void FirstOrderDynamics::setSpeedFluctuationSTD(double speedFluctuationSTD) {
@@ -62,13 +61,13 @@ namespace dsm {
       double alpha{m_alpha / street->capacity()};
       meanSpeed = street->maxSpeed() * n * (1. - 0.5 * alpha * (n - 1.));
     } else {
-      for (const auto& agentId : street->movingAgents()) {
-        meanSpeed += this->agents().at(agentId)->speed();
+      for (auto const& pAgent : street->movingAgents()) {
+        meanSpeed += pAgent->speed();
         ++n;
       }
       for (auto const& queue : street->exitQueues()) {
-        for (const auto& agentId : queue) {
-          meanSpeed += this->agents().at(agentId)->speed();
+        for (auto const& pAgent : queue) {
+          meanSpeed += pAgent->speed();
           ++n;
         }
       }
@@ -76,19 +75,17 @@ namespace dsm {
     const auto& node = this->graph().node(street->nodePair().second);
     if (node->isIntersection()) {
       auto& intersection = dynamic_cast<Intersection&>(*node);
-      for (const auto& [angle, agentId] : intersection.agents()) {
-        const auto& agent{this->agents().at(agentId)};
-        if (agent->streetId().has_value() && agent->streetId().value() == streetId) {
-          meanSpeed += agent->speed();
+      for (auto const& [angle, pAgent] : intersection.agents()) {
+        if (pAgent->streetId().has_value() && pAgent->streetId().value() == streetId) {
+          meanSpeed += pAgent->speed();
           ++n;
         }
       }
     } else if (node->isRoundabout()) {
       auto& roundabout = dynamic_cast<Roundabout&>(*node);
-      for (const auto& agentId : roundabout.agents()) {
-        const auto& agent{this->agents().at(agentId)};
-        if (agent->streetId().has_value() && agent->streetId().value() == streetId) {
-          meanSpeed += agent->speed();
+      for (auto const& pAgent : roundabout.agents()) {
+        if (pAgent->streetId().has_value() && pAgent->streetId().value() == streetId) {
+          meanSpeed += pAgent->speed();
           ++n;
         }
       }
@@ -109,9 +106,6 @@ namespace dsm {
   }
   Measurement<double> FirstOrderDynamics::streetMeanSpeed(double threshold,
                                                           bool above) const {
-    if (this->agents().empty()) {
-      return Measurement(0., 0.);
-    }
     std::vector<double> speeds;
     speeds.reserve(this->graph().edges().size());
     for (const auto& [streetId, street] : this->graph().edges()) {
diff --git a/src/dsm/sources/Intersection.cpp b/src/dsm/sources/Intersection.cpp
index 00ac82d25..00aa32f10 100644
--- a/src/dsm/sources/Intersection.cpp
+++ b/src/dsm/sources/Intersection.cpp
@@ -12,35 +12,23 @@ namespace dsm {
           capacity,
           m_agents.size())));
     }
-    Node::setCapacity(capacity);
+    RoadJunction::setCapacity(capacity);
   }
 
-  void Intersection::addAgent(double angle, Id agentId) {
-    if (isFull()) {
-      throw std::runtime_error(Logger::buildExceptionMessage("Intersection is full."));
-    }
-    for (auto const [angle, id] : m_agents) {
-      if (id == agentId) {
-        throw std::runtime_error(Logger::buildExceptionMessage(
-            std::format("Agent with id {} is already on the node.", agentId)));
-      }
-    }
+  void Intersection::addAgent(double angle, std::unique_ptr<Agent> pAgent) {
+    assert(!isFull());
+    Logger::debug(std::format("Agente nell'intersezione {}", this->id()));
     auto iAngle{static_cast<int16_t>(angle * 100)};
-    m_agents.emplace(iAngle, agentId);
+    m_agents.emplace(iAngle, std::move(pAgent));
     ++m_agentCounter;
   }
 
-  void Intersection::addAgent(Id agentId) {
+  void Intersection::addAgent(std::unique_ptr<Agent> pAgent) {
     int lastKey{0};
     if (!m_agents.empty()) {
       lastKey = m_agents.rbegin()->first + 1;
     }
-    addAgent(static_cast<double>(lastKey), agentId);
-  }
-
-  void Intersection::removeAgent(Id agentId) {
-    std::erase_if(m_agents,
-                  [agentId](const auto& pair) { return pair.second == agentId; });
+    addAgent(static_cast<double>(lastKey), std::move(pAgent));
   }
 
   Size Intersection::agentCounter() {
diff --git a/src/dsm/sources/Road.cpp b/src/dsm/sources/Road.cpp
index 721b998f8..66d884006 100644
--- a/src/dsm/sources/Road.cpp
+++ b/src/dsm/sources/Road.cpp
@@ -8,17 +8,6 @@
 namespace dsm {
   double Road::m_meanVehicleLength = 5.;
 
-  Road::Road(Id id, const Road& road)
-      : Edge(id,
-             road.nodePair(),
-             road.capacity(),
-             road.transportCapacity(),
-             road.geometry()),
-        m_length{road.length()},
-        m_maxSpeed{road.maxSpeed()},
-        m_nLanes{road.nLanes()},
-        m_name{road.name()} {}
-
   Road::Road(Id id,
              std::pair<Id, Id> nodePair,
              double length,
diff --git a/src/dsm/sources/RoadJunction.cpp b/src/dsm/sources/RoadJunction.cpp
new file mode 100644
index 000000000..bc62b09d6
--- /dev/null
+++ b/src/dsm/sources/RoadJunction.cpp
@@ -0,0 +1,26 @@
+#include "../headers/RoadJunction.hpp"
+
+namespace dsm {
+  RoadJunction::RoadJunction(Id id) : Node(id), m_capacity{1}, m_transportCapacity{1.} {}
+  RoadJunction::RoadJunction(Id id, std::pair<double, double> coords)
+      : Node(id, coords), m_capacity{1}, m_transportCapacity{1.} {}
+  RoadJunction::RoadJunction(RoadJunction const& other)
+      : Node(other),
+        m_capacity{other.m_capacity},
+        m_transportCapacity{other.m_transportCapacity} {}
+
+  void RoadJunction::setCapacity(Size capacity) { m_capacity = capacity; }
+  void RoadJunction::setTransportCapacity(double capacity) {
+    assert(capacity > 0.);
+    m_transportCapacity = capacity;
+  }
+
+  Size RoadJunction::capacity() const { return m_capacity; }
+  double RoadJunction::transportCapacity() const { return m_transportCapacity; }
+  double RoadJunction::density() const { return 0.; }
+  bool RoadJunction::isFull() const { return true; }
+
+  bool RoadJunction::isIntersection() const noexcept { return false; }
+  bool RoadJunction::isTrafficLight() const noexcept { return false; }
+  bool RoadJunction::isRoundabout() const noexcept { return false; }
+}  // namespace dsm
\ No newline at end of file
diff --git a/src/dsm/sources/RoadNetwork.cpp b/src/dsm/sources/RoadNetwork.cpp
index 5c001a02a..1b02e0ed6 100644
--- a/src/dsm/sources/RoadNetwork.cpp
+++ b/src/dsm/sources/RoadNetwork.cpp
@@ -49,31 +49,20 @@ namespace dsm {
     auto const oldStreetSet{std::move(m_edges)};
     auto const N{nNodes()};
     std::unordered_map<Id, Id> newStreetIds;
-    for (const auto& [streetId, street] : oldStreetSet) {
+    for (auto& [streetId, street] : oldStreetSet) {
       const auto srcId{street->source()};
       const auto dstId{street->target()};
       const auto newStreetId{static_cast<Id>(srcId * N + dstId)};
-      if (m_edges.contains(newStreetId)) {
-        throw std::invalid_argument(Logger::buildExceptionMessage(
-            std::format("Street with same id ({}) from {} to {} already exists.",
-                        newStreetId,
-                        srcId,
-                        dstId)));
-      }
+      assert(!m_edges.contains(newStreetId));
+      street->resetId(newStreetId);
       if (street->isSpire() && street->isStochastic()) {
-        addEdge<StochasticSpireStreet>(
-            newStreetId,
-            *street,
-            dynamic_cast<StochasticSpireStreet&>(*street).flowRate());
+        addEdge(std::move(dynamic_cast<StochasticSpireStreet&>(*street)));
       } else if (street->isStochastic()) {
-        addEdge<StochasticStreet>(
-            newStreetId, *street, dynamic_cast<StochasticStreet&>(*street).flowRate());
+        addEdge(std::move(dynamic_cast<StochasticStreet&>(*street)));
       } else if (street->isSpire()) {
-        addEdge<SpireStreet>(newStreetId, *street);
-        dynamic_cast<SpireStreet&>(*m_edges.at(newStreetId))
-            .setCode(dynamic_cast<SpireStreet&>(*street).code());
+        addEdge(std::move(dynamic_cast<SpireStreet&>(*street)));
       } else {
-        addEdge<Street>(newStreetId, *street);
+        addEdge(std::move(*street));
       }
       newStreetIds.emplace(streetId, newStreetId);
     }
@@ -121,9 +110,9 @@ namespace dsm {
   }
 
   void RoadNetwork::adjustNodeCapacities() {
-    int16_t value;
+    double value;
     for (Id nodeId = 0; nodeId < m_nodes.size(); ++nodeId) {
-      value = 0;
+      value = 0.;
       auto const N{nNodes()};
       for (const auto& sourceId : m_adjacencyMatrix.getCol(nodeId)) {
         auto const streetId{sourceId * N + nodeId};
@@ -132,13 +121,13 @@ namespace dsm {
       }
       auto const& pNode{m_nodes.at(nodeId)};
       pNode->setCapacity(value);
-      value = 0;
+      value = 0.;
       for (const auto& targetId : m_adjacencyMatrix.getRow(nodeId)) {
         auto const streetId{nodeId * N + targetId};
         auto const& pStreet{m_edges.at(streetId)};
         value += pStreet->nLanes() * pStreet->transportCapacity();
       }
-      pNode->setTransportCapacity(value == 0 ? 1 : value);
+      pNode->setTransportCapacity(value == 0. ? 1. : value);
       if (pNode->capacity() == 0) {
         pNode->setCapacity(value);
       }
@@ -577,43 +566,22 @@ namespace dsm {
     pNode = std::make_unique<Station>(*pNode, managementTime);
     return node<Station>(nodeId);
   }
-  StochasticStreet& RoadNetwork::makeStochasticStreet(Id streetId,
-                                                      double const flowRate) {
+  void RoadNetwork::makeStochasticStreet(Id streetId, double const flowRate) {
     auto& pStreet = m_edges.at(streetId);
-    pStreet = std::make_unique<StochasticStreet>(pStreet->id(), *pStreet, flowRate);
-    return edge<StochasticStreet>(streetId);
+    pStreet = std::unique_ptr<StochasticStreet>(
+        new StochasticStreet(std::move(*pStreet), flowRate));
   }
   void RoadNetwork::makeSpireStreet(Id streetId) {
     auto& pStreet = m_edges.at(streetId);
     if (pStreet->isStochastic()) {
-      pStreet = std::make_unique<StochasticSpireStreet>(
-          pStreet->id(), *pStreet, edge<StochasticStreet>(streetId).flowRate());
+      pStreet = std::unique_ptr<StochasticSpireStreet>(new StochasticSpireStreet(
+          std::move(*pStreet), dynamic_cast<StochasticStreet&>(*pStreet).flowRate()));
       return;
     }
-    pStreet = std::make_unique<SpireStreet>(pStreet->id(), *pStreet);
+    pStreet = std::unique_ptr<SpireStreet>(new SpireStreet(std::move(*pStreet)));
   }
 
-  void RoadNetwork::addStreet(const Street& street) {
-    if (m_edges.contains(street.id())) {
-      throw std::invalid_argument(Logger::buildExceptionMessage(
-          std::format("Street with id {} from {} to {} already exists.",
-                      street.id(),
-                      street.source(),
-                      street.target())));
-    }
-    // emplace nodes
-    const auto srcId{street.source()};
-    const auto dstId{street.target()};
-    if (!m_nodes.contains(srcId)) {
-      m_nodes.emplace(srcId, std::make_unique<Intersection>(srcId));
-    }
-    if (!m_nodes.contains(dstId)) {
-      m_nodes.emplace(dstId, std::make_unique<Intersection>(dstId));
-    }
-    // emplace street
-    m_edges.emplace(std::make_pair(street.id(), std::make_unique<Street>(street)));
-    m_adjacencyMatrix.insert(srcId, dstId);
-  }
+  void RoadNetwork::addStreet(Street&& street) { addEdge<Street>(std::move(street)); }
 
   const std::unique_ptr<Street>* RoadNetwork::street(Id source, Id destination) const {
     auto streetIt = std::find_if(m_edges.begin(),
diff --git a/src/dsm/sources/Roundabout.cpp b/src/dsm/sources/Roundabout.cpp
index d574109ac..cdfe3a040 100644
--- a/src/dsm/sources/Roundabout.cpp
+++ b/src/dsm/sources/Roundabout.cpp
@@ -4,32 +4,22 @@
 #include <stdexcept>
 
 namespace dsm {
-  Roundabout::Roundabout(const Node& node) : Node{node.id()} {
+  Roundabout::Roundabout(const RoadJunction& node) : RoadJunction{node.id()} {
     if (node.coords().has_value()) {
       this->setCoords(node.coords().value());
     }
     this->setCapacity(node.capacity());
   }
 
-  void Roundabout::enqueue(Id agentId) {
-    if (isFull()) {
-      throw std::runtime_error(Logger::buildExceptionMessage("Roundabout is full."));
-    }
-    for (const auto id : m_agents) {
-      if (id == agentId) {
-        throw std::runtime_error(Logger::buildExceptionMessage(
-            std::format("Agent with id {} is already on the roundabout.", agentId)));
-      }
-    }
-    m_agents.push(agentId);
+  void Roundabout::enqueue(std::unique_ptr<Agent> pAgent) {
+    assert(!isFull());
+    m_agents.push(std::move(pAgent));
   }
 
-  Id Roundabout::dequeue() {
-    if (m_agents.empty()) {
-      throw std::runtime_error(Logger::buildExceptionMessage("Roundabout is empty."));
-    }
-    Id agentId{m_agents.front()};
+  std::unique_ptr<Agent> Roundabout::dequeue() {
+    assert(!m_agents.empty());
+    std::unique_ptr<Agent> pAgent{std::move(m_agents.front())};
     m_agents.pop();
-    return agentId;
+    return pAgent;
   }
 }  // namespace dsm
\ No newline at end of file
diff --git a/src/dsm/sources/Station.cpp b/src/dsm/sources/Station.cpp
index 033738c7b..9d70dad06 100644
--- a/src/dsm/sources/Station.cpp
+++ b/src/dsm/sources/Station.cpp
@@ -2,16 +2,18 @@
 
 namespace dsm {
   Station::Station(Id id, Delay managementTime)
-      : Node(id), m_managementTime{managementTime} {}
+      : RoadJunction(id), m_managementTime{managementTime} {}
 
   Station::Station(Id id, std::pair<double, double> coords, Delay managementTime)
-      : Node(id, coords), m_managementTime{managementTime} {}
+      : RoadJunction(id, coords), m_managementTime{managementTime} {}
 
-  Station::Station(Node const& node, Delay managementTime)
-      : Node(node), m_managementTime{managementTime} {}
+  Station::Station(RoadJunction const& node, Delay managementTime)
+      : RoadJunction(node), m_managementTime{managementTime} {}
 
   Station::Station(Station const& other)
-      : Node(other), m_managementTime{other.m_managementTime}, m_trains{other.m_trains} {}
+      : RoadJunction(other),
+        m_managementTime{other.m_managementTime},
+        m_trains{other.m_trains} {}
 
   void Station::enqueue(Id trainId, train_t trainType) {
     m_trains.emplace(trainType, trainId);
@@ -27,10 +29,10 @@ namespace dsm {
   Delay Station::managementTime() const { return m_managementTime; }
 
   double Station::density() const {
-    return static_cast<double>(m_trains.size()) / m_capacity;
+    return static_cast<double>(m_trains.size()) / this->capacity();
   }
 
-  bool Station::isFull() const { return m_trains.size() >= m_capacity; }
+  bool Station::isFull() const { return m_trains.size() >= this->capacity(); }
 
   bool Station::isStation() const noexcept { return true; }
 }  // namespace dsm
\ No newline at end of file
diff --git a/src/dsm/sources/Street.cpp b/src/dsm/sources/Street.cpp
index 6b9372a56..a3aee6944 100644
--- a/src/dsm/sources/Street.cpp
+++ b/src/dsm/sources/Street.cpp
@@ -5,22 +5,6 @@
 #include <cassert>
 
 namespace dsm {
-  Street::Street(Id id, const Street& street)
-      : Road(id,
-             street.nodePair(),
-             street.length(),
-             street.maxSpeed(),
-             street.nLanes(),
-             street.name(),
-             street.geometry(),
-             street.capacity(),
-             street.transportCapacity()) {
-    for (auto i{0}; i < street.nLanes(); ++i) {
-      m_exitQueues.push_back(dsm::queue<Size>());
-    }
-    m_laneMapping = street.laneMapping();
-  }
-
   Street::Street(Id id,
                  std::pair<Id, Id> nodePair,
                  double length,
@@ -38,11 +22,11 @@ namespace dsm {
              std::move(name),
              std::move(geometry),
              capacity,
-             transportCapacity) {
-    m_exitQueues.resize(nLanes);
-    for (auto i{0}; i < nLanes; ++i) {
-      m_exitQueues.push_back(dsm::queue<Size>());
-    }
+             transportCapacity),
+        m_exitQueues{std::vector<dsm::queue<std::unique_ptr<Agent>>>(nLanes)},
+        m_movingAgents{dsm::priority_queue<std::unique_ptr<Agent>,
+                                           std::vector<std::unique_ptr<Agent>>,
+                                           AgentComparator>()} {
     switch (nLanes) {
       case 1:
         m_laneMapping.emplace_back(Direction::ANY);
@@ -66,51 +50,34 @@ namespace dsm {
     }
   }
 
-  std::vector<Id> const& Street::movingAgents() const { return m_movingAgents; }
-
-  void Street::addAgent(Id agentId) {
-    assert((void("Agent is already on the street."),
-            std::find(m_movingAgents.cbegin(), m_movingAgents.cend(), agentId) ==
-                m_movingAgents.cend()));
-    for (auto const& queue : m_exitQueues) {
-      for (auto const& id : queue) {
-        assert((void("Agent is already in queue."), id != agentId));
-      }
-    }
-    m_movingAgents.push_back(agentId);
-    ;
+  void Street::addAgent(std::unique_ptr<Agent> pAgent) {
+    assert(!isFull());
+    m_movingAgents.push(std::move(pAgent));
   }
-  void Street::enqueue(Id agentId, size_t index) {
-    assert((void("Agent is not on the street."),
-            std::find(m_movingAgents.cbegin(), m_movingAgents.cend(), agentId) !=
-                m_movingAgents.cend()));
-    for (auto const& queue : m_exitQueues) {
-      for (auto const& id : queue) {
-        assert((void("Agent is already in queue."), id != agentId));
-      }
-    }
-    m_movingAgents.erase(
-        std::remove(m_movingAgents.begin(), m_movingAgents.end(), agentId),
-        m_movingAgents.end());
-    m_exitQueues[index].push(agentId);
+  void Street::enqueue(size_t const& queueId) {
+    assert(!m_movingAgents.empty());
+    m_movingAgents.top()->incrementDistance(m_length);
+    Logger::debug("Pusing into queue");
+    m_exitQueues[queueId].push(
+        std::move(const_cast<std::unique_ptr<Agent>&>(m_movingAgents.top())));
+    m_movingAgents.pop();
+    Logger::debug("Popped from moving queue");
   }
-  Id Street::dequeue(size_t index) {
-    if (m_exitQueues[index].empty()) {
-      Logger::error(std::format(
-          "Trying to remove an agent from queue {} of street {} which is empty.",
-          index,
-          this->id()));
-    }
-    Id id = m_exitQueues[index].front();
+  std::unique_ptr<Agent> Street::dequeue(size_t index) {
+    assert(!m_exitQueues[index].empty());
+    Logger::debug("Dequeueing from queue");
+    auto pAgent{std::move(m_exitQueues[index].front())};
     m_exitQueues[index].pop();
-    return id;
+    return pAgent;
   }
 
   int Street::nAgents() const {
     auto nAgents{static_cast<int>(m_movingAgents.size())};
+    Logger::debug(std::format("Number of moving agents street {}: {}", id(), nAgents));
     for (const auto& queue : m_exitQueues) {
       nAgents += queue.size();
     }
+    Logger::debug(std::format("Number of agents street {}: {}", id(), nAgents));
     return nAgents;
   }
 
@@ -127,8 +94,8 @@ namespace dsm {
     return nAgents;
   }
 
-  StochasticStreet::StochasticStreet(Id id, const Street& street, double flowRate)
-      : Street(id, street) {
+  StochasticStreet::StochasticStreet(Street&& street, double flowRate)
+      : Street(std::move(street)) {
     setFlowRate(flowRate);
   }
   StochasticStreet::StochasticStreet(Id id,
@@ -161,26 +128,25 @@ namespace dsm {
   double StochasticStreet::flowRate() const { return m_flowRate; }
   bool StochasticStreet::isStochastic() const { return true; }
 
-  void SpireStreet::addAgent(Id agentId) {
-    Street::addAgent(agentId);
+  void SpireStreet::addAgent(std::unique_ptr<Agent> pAgent) {
+    Street::addAgent(std::move(pAgent));
     increaseInputCounter();
   }
 
   int SpireStreet::meanFlow() { return inputCounts() - outputCounts(); }
 
-  Id SpireStreet::dequeue(size_t index) {
+  std::unique_ptr<Agent> SpireStreet::dequeue(size_t index) {
     increaseOutputCounter();
     return Street::dequeue(index);
   }
-  void StochasticSpireStreet::addAgent(Id agentId) {
-    Street::addAgent(agentId);
-    Logger::info(std::format("Adding agent {} to spire street {}", agentId, this->id()));
+  void StochasticSpireStreet::addAgent(std::unique_ptr<Agent> pAgent) {
+    Street::addAgent(std::move(pAgent));
     increaseInputCounter();
   }
 
   int StochasticSpireStreet::meanFlow() { return inputCounts() - outputCounts(); }
 
-  Id StochasticSpireStreet::dequeue(size_t index) {
+  std::unique_ptr<Agent> StochasticSpireStreet::dequeue(size_t index) {
     increaseOutputCounter();
     return Street::dequeue(index);
   }
diff --git a/src/dsm/utility/TypeTraits/is_agent.hpp b/src/dsm/utility/TypeTraits/is_agent.hpp
deleted file mode 100644
index ca4b6ee35..000000000
--- a/src/dsm/utility/TypeTraits/is_agent.hpp
+++ /dev/null
@@ -1,23 +0,0 @@
-
-// #pragma once
-
-// #include <concepts>
-// #include <memory>
-// #include <type_traits>
-// #include "is_numeric.hpp"
-
-// namespace dsm {
-//   class Agent;
-
-//   // define is_node type trait
-//   template <typename T>
-//   struct is_agent : std::false_type {};
-
-//   struct is_agent<Agent> : std::true_type {};
-
-//   struct is_agent<std::unique_ptr<Agent>> : std::true_type {};
-
-//   template <typename T>
-//   inline constexpr bool is_agent_v = is_agent<T>::value;
-
-// };  // namespace dsm
diff --git a/src/dsm/utility/TypeTraits/is_itinerary.hpp b/src/dsm/utility/TypeTraits/is_itinerary.hpp
deleted file mode 100644
index 3ae531787..000000000
--- a/src/dsm/utility/TypeTraits/is_itinerary.hpp
+++ /dev/null
@@ -1,22 +0,0 @@
-
-// #pragma once
-
-// #include <concepts>
-// #include <memory>
-// #include <type_traits>
-
-// namespace dsm {
-//   class Itinerary;
-
-//   // define is_node type trait
-//   template <typename T>
-//   struct is_itinerary : std::false_type {};
-
-//   struct is_itinerary<Agent> : std::true_type {};
-
-//   struct is_itinerary<std::unique_ptr<Agent>> : std::true_type {};
-
-//   template <typename T>
-//   inline constexpr bool is_itinerary_v = is_itinerary<T>::value;
-
-// };  // namespace dsm
diff --git a/test/Test_agent.cpp b/test/Test_agent.cpp
index caa9635d2..ac7e11fc6 100644
--- a/test/Test_agent.cpp
+++ b/test/Test_agent.cpp
@@ -11,12 +11,10 @@ using Agent = dsm::Agent;
 TEST_CASE("Agent") {
   SUBCASE("Constructors") {
     GIVEN("An agent and its itinerary ids") {
-      uint16_t agentId{1};
       uint16_t itineraryId{0};
       WHEN("The Agent is constructed") {
-        Agent agent{0, agentId, itineraryId};
+        Agent agent{0, itineraryId};
         THEN("The agent and itinerary ids are set correctly") {
-          CHECK_EQ(agent.id(), agentId);
           CHECK_EQ(agent.itineraryId(), itineraryId);
           CHECK_FALSE(agent.streetId().has_value());
           CHECK_FALSE(agent.srcNodeId().has_value());
@@ -27,13 +25,11 @@ TEST_CASE("Agent") {
       }
     }
     GIVEN("An agent, its itinerary ids, and a node id") {
-      uint16_t agentId{1};
       uint16_t itineraryId{0};
       uint16_t srcNodeId{0};
       WHEN("The Agent is constructed") {
-        Agent agent{0, agentId, itineraryId, srcNodeId};
+        Agent agent{0, itineraryId, srcNodeId};
         THEN("The agent and itinerary ids are set correctly") {
-          CHECK_EQ(agent.id(), agentId);
           CHECK_EQ(agent.itineraryId(), itineraryId);
           CHECK_FALSE(agent.streetId().has_value());
           CHECK(agent.srcNodeId().has_value());
@@ -45,9 +41,8 @@ TEST_CASE("Agent") {
       }
     }
     GIVEN("An agent it") {
-      uint16_t agentId{1};
       WHEN("The agent is constructed") {
-        auto randomAgent = Agent{0, agentId};
+        auto randomAgent = Agent{0};
         THEN("The agent is a random agent") { CHECK(randomAgent.isRandom()); }
       }
     }
diff --git a/test/Test_dynamics.cpp b/test/Test_dynamics.cpp
index b9b4631b7..e7200abaf 100644
--- a/test/Test_dynamics.cpp
+++ b/test/Test_dynamics.cpp
@@ -2,30 +2,19 @@
 
 #include "FirstOrderDynamics.hpp"
 #include "RoadNetwork.hpp"
-#include "SparseMatrix.hpp"
+#include "Itinerary.hpp"
 #include "Street.hpp"
 
 #include "doctest.h"
 
-using Dynamics = dsm::FirstOrderDynamics;
-using RoadNetwork = dsm::RoadNetwork;
-using SparseMatrix = dsm::SparseMatrix<bool>;
-using Road = dsm::Road;
-using Street = dsm::Street;
-using SpireStreet = dsm::SpireStreet;
-using Agent = dsm::Agent;
-using Itinerary = dsm::Itinerary;
-using Intersection = dsm::Intersection;
-using TrafficLight = dsm::TrafficLight;
-using Roundabout = dsm::Roundabout;
-using Measurement = dsm::Measurement<float>;
+using namespace dsm;
 
 TEST_CASE("Measurement") {
   SUBCASE("STL vector") {
     std::vector<float> data(100);
     std::iota(data.begin(), data.end(), 0.f);
 
-    Measurement m(data);
+    Measurement<float> m(data);
     CHECK_EQ(m.mean, 49.5f);
     CHECK_EQ(m.std, doctest::Approx(28.8661f));
   }
@@ -33,7 +22,7 @@ TEST_CASE("Measurement") {
     std::array<float, 100> data;
     std::iota(data.begin(), data.end(), 0.f);
 
-    Measurement m(data);
+    Measurement<float> m(data);
     CHECK_EQ(m.mean, 49.5f);
     CHECK_EQ(m.std, doctest::Approx(28.8661f));
   }
@@ -42,27 +31,29 @@ TEST_CASE("Measurement") {
     std::span<float> data(p.get(), 100);
     std::iota(data.begin(), data.end(), 0.f);
 
-    Measurement m(data);
+    Measurement<float> m(data);
     CHECK_EQ(m.mean, 49.5f);
     CHECK_EQ(m.std, doctest::Approx(28.8661f));
   }
 }
 
-TEST_CASE("Dynamics") {
+TEST_CASE("FirstOrderDynamics") {
+  // dsm::Logger::setLogLevel(dsm::log_level_t::DEBUG);
   SUBCASE("Constructor") {
     GIVEN("A graph object") {
       auto graph = RoadNetwork{};
       graph.importMatrix("./data/matrix.dsm");
       graph.buildAdj();
       WHEN("A dynamics object is created") {
-        Dynamics dynamics{graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+        FirstOrderDynamics dynamics{
+            graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
         THEN("The node and the street sets are the same") {
           CHECK_EQ(dynamics.graph().nNodes(), 3);
           CHECK_EQ(dynamics.graph().nEdges(), 4);
         }
         THEN("The mean speed, density, flow and travel time are 0") {
-          CHECK_EQ(dynamics.agentMeanSpeed().mean, 0.);
-          CHECK_EQ(dynamics.agentMeanSpeed().std, 0.);
+          // CHECK_EQ(dynamics.agentMeanSpeed().mean, 0.);
+          // CHECK_EQ(dynamics.agentMeanSpeed().std, 0.);
           CHECK_EQ(dynamics.streetMeanDensity().mean, 0.);
           CHECK_EQ(dynamics.streetMeanDensity().std, 0.);
           CHECK_EQ(dynamics.streetMeanFlow().mean, 0.);
@@ -73,7 +64,7 @@ TEST_CASE("Dynamics") {
       }
       WHEN("We transform a node into a traffic light and create the dynamics") {
         auto& tl = graph.makeTrafficLight(0, 2);
-        Dynamics dynamics{graph, false, 69};
+        FirstOrderDynamics dynamics{graph, false, 69};
         THEN("The node is a traffic light") {
           CHECK(dynamics.graph().node(0)->isTrafficLight());
           CHECK_EQ(tl.cycleTime(), 2);
@@ -81,14 +72,14 @@ TEST_CASE("Dynamics") {
       }
       WHEN("We transform a node into a roundabout and create the dynamics") {
         graph.makeRoundabout(0);
-        Dynamics dynamics{graph, false, 69};
+        FirstOrderDynamics dynamics{graph, false, 69};
         THEN("The node is a roundabout") {
           CHECK(dynamics.graph().node(0)->isRoundabout());
         }
       }
       WHEN("We transorm a street into a spire and create the dynamcis") {
         graph.makeSpireStreet(8);
-        Dynamics dynamics{graph, false, 69};
+        FirstOrderDynamics dynamics{graph, false, 69};
         THEN("The street is a spire") { CHECK(dynamics.graph().edge(8)->isSpire()); }
       }
     }
@@ -97,7 +88,8 @@ TEST_CASE("Dynamics") {
     GIVEN("A dynamics object and a destination node") {
       auto graph = RoadNetwork{};
       graph.importMatrix("./data/matrix.dat");
-      Dynamics dynamics{graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       WHEN("We add a span of destination nodes") {
         std::array<uint32_t, 3> nodes{0, 1, 2};
         dynamics.setDestinationNodes(nodes);
@@ -115,14 +107,15 @@ TEST_CASE("Dynamics") {
     GIVEN("A dynamics object, a source node and a destination node") {
       auto graph = RoadNetwork{};
       graph.importMatrix("./data/matrix.dsm");
-      Dynamics dynamics{graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
-      dynamics.addItinerary(std::unique_ptr<Itinerary>(new Itinerary(2, 2)));
+      FirstOrderDynamics dynamics{
+          graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      dynamics.addItinerary(2, 2);
       WHEN("We add the agent") {
-        dynamics.addAgent(0, 2, 0);
+        dynamics.addAgent(2, 0);
         THEN("The agent is added") {
           CHECK_EQ(dynamics.nAgents(), 1);
           const auto& agent = dynamics.agents().at(0);
-          CHECK_EQ(agent->id(), 0);
+          // CHECK_EQ(agent->id(), 0);
           CHECK_EQ(agent->srcNodeId().value(), 0);
           CHECK_EQ(agent->itineraryId(), 2);
         }
@@ -133,27 +126,25 @@ TEST_CASE("Dynamics") {
     GIVEN("A dynamics object and an itinerary") {
       auto graph = RoadNetwork{};
       graph.importMatrix("./data/matrix.dsm");
-      Dynamics dynamics{graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       WHEN("We add a random agent") {
-        dynamics.addItinerary(std::unique_ptr<Itinerary>(new Itinerary(0, 2)));
+        dynamics.addItinerary(2, 2);
         dynamics.addAgentsUniformly(1);
         THEN(
             "The number of agents is 1 and the destination is the same as the "
             "itinerary") {
           CHECK_EQ(dynamics.nAgents(), 1);
-          CHECK_EQ(dynamics.itineraries()
-                       .at(dynamics.agents().at(0)->itineraryId())
-                       ->destination(),
-                   2);
         }
       }
     }
     GIVEN("A dynamics object and many itineraries") {
       auto graph = RoadNetwork{};
       graph.importMatrix("./data/matrix.dsm");
-      Dynamics dynamics{graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
-      dynamics.addItinerary(std::unique_ptr<Itinerary>(new Itinerary(0, 2)));
-      dynamics.addItinerary(std::unique_ptr<Itinerary>(new Itinerary(1, 1)));
+      FirstOrderDynamics dynamics{
+          graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      dynamics.addItinerary(2, 2);
+      dynamics.addItinerary(1, 1);
       WHEN("We add many agents") {
         dynamics.addAgentsUniformly(3);
         THEN(
@@ -161,41 +152,20 @@ TEST_CASE("Dynamics") {
             "same as "
             "the itinerary") {
           CHECK_EQ(dynamics.nAgents(), 3);
-          CHECK(dynamics.agents().at(0)->streetId().has_value());
-          CHECK(dynamics.agents().at(1)->streetId().has_value());
-          CHECK(dynamics.agents().at(2)->streetId().has_value());
 #ifdef __APPLE__
-          CHECK_EQ(dynamics.itineraries()
-                       .at(dynamics.agents().at(0)->itineraryId())
-                       ->destination(),
-                   1);
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 6);
-          CHECK_EQ(dynamics.itineraries()
-                       .at(dynamics.agents().at(1)->itineraryId())
-                       ->destination(),
-                   2);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 1);
-          CHECK_EQ(dynamics.itineraries()
-                       .at(dynamics.agents().at(2)->itineraryId())
-                       ->destination(),
-                   2);
-          CHECK_EQ(dynamics.agents().at(2)->streetId().value(), 8);
+          CHECK_EQ(dynamics.graph().edge(1)->nAgents(), 1);
+          CHECK_EQ(dynamics.graph().edge(1)->movingAgents().top()->itineraryId(), 2);
+          CHECK_EQ(dynamics.graph().edge(6)->nAgents(), 1);
+          CHECK_EQ(dynamics.graph().edge(6)->movingAgents().top()->itineraryId(), 1);
+          CHECK_EQ(dynamics.graph().edge(8)->nAgents(), 1);
+          CHECK_EQ(dynamics.graph().edge(8)->movingAgents().top()->itineraryId(), 2);
 #else
-          CHECK_EQ(dynamics.itineraries()
-                       .at(dynamics.agents().at(0)->itineraryId())
-                       ->destination(),
-                   2);
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 3);
-          CHECK_EQ(dynamics.itineraries()
-                       .at(dynamics.agents().at(1)->itineraryId())
-                       ->destination(),
-                   2);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 8);
-          CHECK_EQ(dynamics.itineraries()
-                       .at(dynamics.agents().at(2)->itineraryId())
-                       ->destination(),
-                   1);
-          CHECK_EQ(dynamics.agents().at(2)->streetId().value(), 1);
+          CHECK_EQ(dynamics.graph().edge(1)->nAgents(), 1);
+          CHECK_EQ(dynamics.graph().edge(1)->movingAgents().top()->itineraryId(), 1);
+          CHECK_EQ(dynamics.graph().edge(3)->nAgents(), 1);
+          CHECK_EQ(dynamics.graph().edge(3)->movingAgents().top()->itineraryId(), 2);
+          CHECK_EQ(dynamics.graph().edge(8)->nAgents(), 1);
+          CHECK_EQ(dynamics.graph().edge(8)->movingAgents().top()->itineraryId(), 2);
 #endif
         }
       }
@@ -205,11 +175,12 @@ TEST_CASE("Dynamics") {
     GIVEN("A graph object") {
       auto graph = RoadNetwork{};
       graph.importMatrix("./data/matrix.dat");
-      Dynamics dynamics{graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       WHEN("We add one agent for existing itinerary") {
         std::unordered_map<uint32_t, double> src{{0, 1.}};
         std::unordered_map<uint32_t, double> dst{{2, 1.}};
-        dynamics.addItinerary(std::unique_ptr<Itinerary>(new Itinerary(0, 2)));
+        dynamics.addItinerary(2, 2);
         dynamics.addAgentsRandomly(1, src, dst);
         THEN("The agents are correctly set") {
           CHECK_EQ(dynamics.nAgents(), 1);
@@ -254,7 +225,8 @@ TEST_CASE("Dynamics") {
       auto graph = RoadNetwork{};
       graph.importMatrix("./data/matrix.dat", false);
       graph.buildAdj();
-      Dynamics dynamics{graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.setPassageProbability(p);
       WHEN("We add some agent") {
         dynamics.addAgents(n);
@@ -272,10 +244,11 @@ TEST_CASE("Dynamics") {
     GIVEN("A dynamics object and one itinerary") {
       auto graph = RoadNetwork{};
       graph.importMatrix("./data/matrix.dsm");
-      Dynamics dynamics{graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
-      dynamics.addItinerary(std::unique_ptr<Itinerary>(new Itinerary(0, 2)));
-      WHEN("We add an agent with itinerary 0") {
-        dynamics.addAgent(0, 0);
+      FirstOrderDynamics dynamics{
+          graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      dynamics.addItinerary(2, 2);
+      WHEN("We add an agent with itinerary 2") {
+        dynamics.addAgent(2, 0);
         THEN(
             "The number of agents is 1 and the destination is the same as the "
             "itinerary") {
@@ -292,26 +265,6 @@ TEST_CASE("Dynamics") {
       }
     }
   }
-  SUBCASE("Add too many agents") {
-    GIVEN("A simple graph with two nodes and only one street") {
-      Street s{0, std::make_pair(0, 1), 2.};  // Capacity of 1 agent
-      RoadNetwork graph2;
-      graph2.addStreets(s);
-      graph2.buildAdj();
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
-      dynamics.addItinerary(std::unique_ptr<Itinerary>(new Itinerary(0, 1)));
-      dynamics.updatePaths();
-      dynamics.addAgentsUniformly(1);
-      WHEN("We add more than one agent") {
-        THEN("It throws") {
-          CHECK_THROWS_AS(dynamics.addAgentsUniformly(1), std::overflow_error);
-          CHECK_THROWS_AS(dynamics.addAgent(1, 0, 0), std::overflow_error);
-          CHECK_THROWS_AS(dynamics.addAgent(std::make_unique<Agent>(Agent(1, 0))),
-                          std::overflow_error);
-        }
-      }
-    }
-  }
   SUBCASE("Update paths") {
     GIVEN("A dynamics object, many streets and an itinerary") {
       Street s1{0, std::make_pair(0, 1), 2.};
@@ -320,7 +273,8 @@ TEST_CASE("Dynamics") {
       RoadNetwork graph2;
       graph2.addStreets(s1, s2, s3);
       graph2.buildAdj();
-      Dynamics dynamics{graph2, false, 69, 0.0, dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0.0, dsm::weight_functions::streetLength, 1.};
       WHEN("We add an itinerary and update the paths") {
         dynamics.addItinerary(std::unique_ptr<Itinerary>(new Itinerary(0, 2)));
         dynamics.updatePaths();
@@ -355,7 +309,7 @@ TEST_CASE("Dynamics") {
       graph2.addEdge(5, std::make_pair(4, 5), 10.);
       graph2.addEdge(6, std::make_pair(5, 4), 10.);
       graph2.buildAdj();
-      Dynamics dynamics{graph2, false, 69, 0.};
+      FirstOrderDynamics dynamics{graph2, false, 69, 0.};
       WHEN("We add an iitinerary to node 3 and update paths") {
         dynamics.addItinerary(3, 3);
         dynamics.updatePaths();
@@ -378,7 +332,8 @@ TEST_CASE("Dynamics") {
         "destination") {
       RoadNetwork graph2{};
       graph2.importMatrix("./data/matrix.dat");
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.addItinerary(0, 118);
       dynamics.addItinerary(1, 118);
       dynamics.addItinerary(2, 118);
@@ -403,7 +358,8 @@ TEST_CASE("Dynamics") {
       RoadNetwork graph;
       graph.addStreets(s1, s2, s3, s4);
       graph.buildAdj();
-      Dynamics dynamics{graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.addItinerary(0, 2);
       WHEN("We update the paths") {
         dynamics.updatePaths();
@@ -438,7 +394,8 @@ TEST_CASE("Dynamics") {
       RoadNetwork graph;
       graph.addStreets(s1, s2, s3, s4);
       graph.buildAdj();
-      Dynamics dynamics{graph, false, 69, 0., dsm::weight_functions::streetTime, 1.};
+      FirstOrderDynamics dynamics{
+          graph, false, 69, 0., dsm::weight_functions::streetTime, 1.};
       // dynamics.setWeightFunction(dsm::weight_functions::streetTime);
       dynamics.addItinerary(0, 2);
       WHEN("We update the paths") {
@@ -473,29 +430,43 @@ TEST_CASE("Dynamics") {
       RoadNetwork graph;
       graph.addStreets(s1, s2, s3);
       graph.buildAdj();
-      Dynamics dynamics{graph, false, 69, 0.0, dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph, false, 69, 0.0, dsm::weight_functions::streetLength, 1.};
       dynamics.addItinerary(2, 2);
       dynamics.updatePaths();
       WHEN("We add an agent randomly and evolve the dynamics") {
-        dynamics.addAgent(0, 2, 0);
+        dynamics.addAgent(2, 0);
+        auto const& streets{dynamics.graph().edges()};
         dynamics.evolve(false);  // Agent goes into node 0
         dynamics.evolve(false);  // Agent goes from node 0 to street 0->1
         THEN("The agent evolves") {
-          CHECK_EQ(dynamics.time() - dynamics.agents().at(0)->spawnTime(), 2);
-          CHECK_EQ(dynamics.agents().at(0)->freeTime(), dynamics.time());
-          CHECK(dynamics.agents().at(0)->streetId().has_value());
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
-          CHECK_EQ(dynamics.agents().at(0)->speed(), 13.8888888889);
+          CHECK_EQ(streets.at(1)->movingAgents().size(), 1);
+          auto const& pAgent{streets.at(1)->movingAgents().top()};
+          CHECK(pAgent);
+          CHECK_EQ(dynamics.time() - pAgent->spawnTime(), dynamics.time());
+          CHECK_EQ(pAgent->freeTime(), dynamics.time());
+          CHECK_EQ(pAgent->streetId().value(), 1);
+          CHECK_EQ(pAgent->speed(), 13.8888888889);
         }
         dynamics.evolve(false);  // Agent enqueues on street 0->1
         THEN("The agent evolves again, changing street") {
-          CHECK_EQ(dynamics.time() - dynamics.agents().at(0)->spawnTime(), 3);
-          CHECK_EQ(dynamics.agents().at(0)->freeTime(), dynamics.time());
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 5);
-          CHECK_EQ(dynamics.agents().at(0)->speed(), 13.8888888889);
+          auto const& pAgent{streets.at(1)->queue(0).front()};
+          CHECK_EQ(dynamics.time() - pAgent->spawnTime(), dynamics.time());
+          CHECK_EQ(pAgent->freeTime() + 1, dynamics.time());
+          CHECK_EQ(pAgent->streetId().value(), 1);
+          CHECK_EQ(pAgent->speed(), 0.);
         }
-        dynamics.evolve(false);
-        THEN("And again, reaching the destination") { CHECK(dynamics.agents().empty()); }
+        dynamics.evolve(false);  // Agent changes street
+        THEN("The agent evolves again, changing street") {
+          auto const& pAgent{streets.at(5)->movingAgents().top()};
+          CHECK_EQ(dynamics.time() - pAgent->spawnTime(), dynamics.time());
+          CHECK_EQ(pAgent->freeTime(), dynamics.time());
+          CHECK_EQ(pAgent->streetId().value(), 5);
+          CHECK_EQ(pAgent->speed(), 13.8888888889);
+        }
+        dynamics.evolve(false);  // Enqueues on street 5
+        dynamics.evolve(false);  // Goes into destination nodes and gets killed
+        THEN("And again, reaching the destination") { CHECK_EQ(dynamics.nAgents(), 0); }
       }
     }
     GIVEN("A dynamics object, an itinerary and an agent") {
@@ -504,22 +475,25 @@ TEST_CASE("Dynamics") {
       RoadNetwork graph2;
       graph2.addStreets(s1, s2);
       graph2.buildAdj();
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
-      dynamics.addItinerary(std::unique_ptr<Itinerary>(new Itinerary(0, 1)));
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      dynamics.addItinerary(1, 1);
       dynamics.updatePaths();
-      dynamics.addAgent(0, 0, 0);
+      dynamics.addAgent(1, 0);
       WHEN("We evolve the dynamics") {
         dynamics.evolve(false);
         dynamics.evolve(false);
         THEN("The agent evolves") {
-          CHECK_EQ(dynamics.time() - dynamics.agents().at(0)->spawnTime(), 2);
-          CHECK_EQ(dynamics.agents().at(0)->freeTime(), dynamics.time());
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
-          CHECK_EQ(dynamics.agents().at(0)->speed(), 13.8888888889);
-          CHECK_EQ(dynamics.agents().at(0)->distance(), 13.8888888889);
+          auto const& pAgent{dynamics.graph().edge(1)->movingAgents().top()};
+          CHECK_EQ(dynamics.time() - pAgent->spawnTime(), 2);
+          CHECK_EQ(pAgent->freeTime(), dynamics.time());
+          CHECK_EQ(pAgent->streetId().value(), 1);
+          CHECK_EQ(pAgent->speed(), 13.8888888889);
+          CHECK_EQ(pAgent->distance(), 0.);  // Not updated yet
         }
         dynamics.evolve(false);
-        THEN("The agent reaches the destination") { CHECK(dynamics.agents().empty()); }
+        dynamics.evolve(false);
+        THEN("The agent reaches the destination") { CHECK_EQ(dynamics.nAgents(), 0); }
       }
     }
     GIVEN("A dynamics object, an itinerary and an agent") {
@@ -529,19 +503,21 @@ TEST_CASE("Dynamics") {
       RoadNetwork graph2;
       graph2.addStreets(s1, s2);
       graph2.buildAdj();
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.addItinerary(std::unique_ptr<Itinerary>(new Itinerary(0, 1)));
       dynamics.updatePaths();
-      dynamics.addAgent(0, 0, 0);
+      dynamics.addAgent(0, 0);
       WHEN("We evolve the dynamics") {
         dynamics.evolve(false);
         dynamics.evolve(false);
         THEN("The agent evolves") {
-          CHECK_EQ(dynamics.time() - dynamics.agents().at(0)->spawnTime(), 2);
-          CHECK_EQ(dynamics.agents().at(0)->freeTime(), dynamics.time());
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
-          CHECK_EQ(dynamics.agents().at(0)->speed(), 13.8888888889);
-          CHECK_EQ(dynamics.agents().at(0)->distance(), 13.8888888889);
+          auto const& pAgent{dynamics.graph().edge(1)->movingAgents().top()};
+          CHECK_EQ(dynamics.time() - pAgent->spawnTime(), 2);
+          CHECK_EQ(pAgent->freeTime(), dynamics.time());
+          CHECK_EQ(pAgent->streetId().value(), 1);
+          CHECK_EQ(pAgent->speed(), 13.8888888889);
+          CHECK_EQ(pAgent->distance(), 0.);  // Not updated yet
         }
         auto i{0};
         while (dynamics.nAgents() > 0) {
@@ -557,27 +533,27 @@ TEST_CASE("Dynamics") {
       RoadNetwork graph2;
       graph2.addStreets(s1, s2);
       graph2.buildAdj();
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.addItinerary(1, 1);
       dynamics.updatePaths();
-      dynamics.addAgent(0, 1, 0);
+      dynamics.addAgent(1, 0);
       WHEN("We evolve the dynamics with reinsertion") {
         dynamics.evolve(true);
         dynamics.evolve(true);
         THEN("The agent has correct values") {
-          CHECK_EQ(dynamics.time() - dynamics.agents().at(0)->spawnTime(), 2);
-          CHECK_EQ(dynamics.agents().at(0)->freeTime(), dynamics.time());
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
-          CHECK_EQ(dynamics.agents().at(0)->speed(), 13.8888888889);
-          CHECK_EQ(dynamics.agents().at(0)->distance(), 13.8888888889);
+          auto const& pAgent{dynamics.graph().edge(1)->movingAgents().top()};
+          CHECK_EQ(dynamics.time() - pAgent->spawnTime(), 2);
+          CHECK_EQ(pAgent->freeTime(), dynamics.time());
+          CHECK_EQ(pAgent->streetId().value(), 1);
+          CHECK_EQ(pAgent->speed(), 13.8888888889);
+          CHECK_EQ(pAgent->distance(), 0.);  // Not updated yet
         }
         dynamics.evolve(true);
+        dynamics.evolve(true);
         THEN("The agent is reinserted") {
+          CHECK(dynamics.graph().node(0)->density() > 0.);
           CHECK_EQ(dynamics.nAgents(), 1);
-          CHECK_EQ(dynamics.time() - dynamics.agents().at(0)->spawnTime(), 1);
-          CHECK(dynamics.agents().at(0)->freeTime() < dynamics.time());
-          CHECK_FALSE(dynamics.agents().at(0)->streetId().has_value());
-          CHECK_EQ(dynamics.agents().at(0)->speed(), 0.);
         }
       }
     }
@@ -589,11 +565,11 @@ TEST_CASE("Dynamics") {
       RoadNetwork graph2;
       graph2.addStreets(s0_1, s1_0, s1_2, s2_1);
       graph2.buildAdj();
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.setDestinationNodes({1, 2});
       std::vector<dsm::Id> trip{2, 1};
-      dynamics.addAgent(0, trip, 0);
-      auto const& pAgent{dynamics.agents().at(0)};
+      dynamics.addAgent(trip, 0);
       WHEN("We evolve the dynamics") {
         dynamics.evolve(false);
         dynamics.evolve(false);
@@ -601,12 +577,14 @@ TEST_CASE("Dynamics") {
         dynamics.evolve(false);
         dynamics.evolve(false);
         THEN("The agent goes first into node 2") {
+          auto const& pAgent{dynamics.graph().edge(5)->queue(0).front()};
           CHECK_EQ(pAgent->streetId().value(), 5);
           CHECK_EQ(pAgent->distance(), 60.);
         }
         dynamics.evolve(false);
         dynamics.evolve(false);
         THEN("The agent goes then to node 1") {
+          auto const& pAgent{dynamics.graph().edge(7)->queue(0).front()};
           CHECK_EQ(pAgent->streetId().value(), 7);
           CHECK_EQ(pAgent->distance(), 90.);
         }
@@ -632,12 +610,14 @@ TEST_CASE("Dynamics") {
       tl.setCycle(7, dsm::Direction::RIGHT, {2, 0});
       tl.setCycle(16, dsm::Direction::RIGHT, {2, 2});
       tl.setCycle(9, dsm::Direction::RIGHT, {2, 2});
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.addItinerary(2, 2);
       dynamics.updatePaths();
-      dynamics.addAgent(0, 2, 0);
+      dynamics.addAgent(2, 0);
       WHEN("We evolve the dynamics") {
         dynamics.evolve(false);
+        auto const& streets{dynamics.graph().edges()};
         THEN(
             "The agent is ready to go through the traffic light at time 3 but "
             "the "
@@ -646,15 +626,15 @@ TEST_CASE("Dynamics") {
           for (uint8_t i{0}; i < 5; ++i) {
             dynamics.evolve(false);
             if (i < 3) {
-              CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
+              CHECK_EQ(streets.at(1)->nAgents(), 1);
             } else {
-              CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 7);
+              CHECK_EQ(streets.at(7)->nAgents(), 1);
             }
             if (i == 2) {
-              CHECK_EQ(dynamics.agents().at(0)->distance(), 30.);
+              CHECK_EQ(streets.at(1)->queue(0).front()->distance(), 30.);
             }
           }
-          CHECK_EQ(dynamics.agents().at(0)->distance(), 60.);
+          CHECK_EQ(streets.at(7)->queue(0).front()->distance(), 60.);
         }
       }
     }
@@ -691,32 +671,32 @@ TEST_CASE("Dynamics") {
       graph2.buildAdj();
       graph2.adjustNodeCapacities();
 
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.setDestinationNodes({0, 2, 3, 4});
 
       WHEN("We add agents and make the system evolve") {
-        dynamics.addAgent(0, 2, 0);
-        dynamics.addAgent(1, 4, 0);
-        dynamics.evolve(false);  // Counter 0
+        dynamics.addAgent(2, 0);
+        dynamics.addAgent(4, 0);
         THEN("The agents are not yet on the streets") {
           CHECK_FALSE(dynamics.agents().at(0)->streetId().has_value());
           CHECK_FALSE(dynamics.agents().at(1)->streetId().has_value());
         }
+        dynamics.evolve(false);  // Counter 0
         dynamics.evolve(false);  // Counter 1
         THEN("The agents are correctly placed") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 1);
+          CHECK_EQ(dynamics.graph().edge(1)->nAgents(), 2);
         }
         dynamics.evolve(false);  // Counter 2
         dynamics.evolve(false);  // Counter 3
         THEN("The agent 0 passes and agent 1 waits") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 7);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 1);
+          CHECK_EQ(dynamics.graph().edge(1)->nAgents(), 1);
+          CHECK_EQ(dynamics.graph().edge(7)->nAgents(), 1);
         }
         dynamics.evolve(false);  // Counter 4
         THEN("The agent 1 passes") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 7);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 9);
+          CHECK_EQ(dynamics.graph().edge(7)->nAgents(), 1);
+          CHECK_EQ(dynamics.graph().edge(9)->nAgents(), 1);
         }
       }
     }
@@ -754,36 +734,33 @@ TEST_CASE("Dynamics") {
       graph2.buildAdj();
       graph2.adjustNodeCapacities();
 
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.setDestinationNodes({0, 2, 3, 4});
 
       WHEN("We add agents and make the system evolve") {
-        dynamics.addAgent(0, 2, 0);
-        dynamics.addAgent(1, 4, 0);
+        dynamics.addAgent(2, 0);
+        dynamics.addAgent(4, 0);
         dynamics.evolve(false);  // Counter 0
         dynamics.evolve(false);  // Counter 1
         THEN("The agents are correctly placed") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 1);
+          CHECK_EQ(dynamics.graph().edge(1)->nAgents(), 2);
         }
         dynamics.evolve(false);  // Counter 2
         dynamics.evolve(false);  // Counter 3
         THEN("The agents are still") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 1);
+          CHECK_EQ(dynamics.graph().edge(1)->nExitingAgents(), 2);
         }
         dynamics.evolve(false);  // Counter 4
         dynamics.evolve(false);  // Counter 5
         dynamics.evolve(false);  // Counter 0
         THEN("The agent 0 passes and agent 1 waits") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 7);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 1);
+          CHECK_EQ(dynamics.graph().edge(1)->nAgents(), 1);
+          CHECK_EQ(dynamics.graph().edge(7)->nAgents(), 1);
         }
         dynamics.evolve(false);  // Counter 1
         dynamics.evolve(false);  // Counter 2
-        THEN("The agent 1 passes") {
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 9);
-        }
+        THEN("The agent 1 passes") { CHECK_EQ(dynamics.graph().edge(9)->nAgents(), 1); }
       }
     }
     SUBCASE("Traffic Lights optimization algorithm") {
@@ -807,7 +784,8 @@ TEST_CASE("Dynamics") {
         tl.setCycle(11, dsm::Direction::ANY, {4, 0});
         tl.setComplementaryCycle(16, 11);
         tl.setComplementaryCycle(21, 11);
-        Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+        FirstOrderDynamics dynamics{
+            graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
         dynamics.setDestinationNodes({0, 2, 3, 4});
         WHEN("We evolve the dynamics and optimize traffic lights") {
           dynamics.addAgents(7, 0, 2);
@@ -856,30 +834,40 @@ TEST_CASE("Dynamics") {
       graph2.buildAdj();
       auto& rb = graph2.makeRoundabout(1);
       graph2.adjustNodeCapacities();
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.addItinerary(0, 0);
       dynamics.addItinerary(2, 2);
       dynamics.updatePaths();
-      dynamics.addAgent(0, 2, 0);
-      dynamics.addAgent(1, 0, 2);
+      dynamics.addAgent(2, 0);
+      dynamics.addAgent(0, 2);
       WHEN(
           "We evolve the dynamics adding an agent on the path of the agent "
           "with "
           "priority") {
-        dynamics.evolve(false);
-        dynamics.addAgent(2, 2, 1);
-        dynamics.evolve(false);
-        dynamics.evolve(false);
+        dynamics.evolve(false);  // Agents into sources
+        dynamics.evolve(false);  // Agents from sources to streets
+        dynamics.addAgent(2, 1);
+        dynamics.evolve(false);  // Agents into queues, other agent into roundabout
+        dynamics.evolve(false);  // Agents from queues to roundabout
+        auto const& streets{dynamics.graph().edges()};
         THEN("The agents are trapped into the roundabout") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 7);
-          CHECK_EQ(dynamics.agents().at(2)->streetId().value(), 5);
+          CHECK_EQ(streets.at(1)->nAgents(), 0);
+          CHECK_EQ(streets.at(5)->nAgents(), 1);
+          CHECK_EQ(streets.at(7)->nAgents(), 1);
           CHECK_EQ(rb.agents().size(), 1);
         }
         dynamics.evolve(false);
         THEN("The agent with priority leaves the roundabout") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 5);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 3);
+          CHECK_EQ(streets.at(3)->nAgents(), 0);
+          CHECK_EQ(streets.at(5)->nAgents(), 1);
+          CHECK_EQ(streets.at(7)->nAgents(), 0);
+          CHECK_EQ(rb.agents().size(), 2);
+        }
+        dynamics.evolve(false);
+        THEN("The agent with priority leaves the roundabout") {
+          CHECK_EQ(streets.at(3)->nAgents(), 1);
+          CHECK_EQ(streets.at(5)->nAgents(), 1);
           CHECK(rb.agents().empty());
         }
       }
@@ -892,19 +880,22 @@ TEST_CASE("Dynamics") {
       RoadNetwork graph2;
       graph2.addStreets(s1, s2);
       graph2.buildAdj();
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.addItinerary(2, 2);
       dynamics.updatePaths();
-      dynamics.addAgent(0, 2, 0);
+      dynamics.addAgent(2, 0);
       WHEN("We evolve the dynamics") {
+        dynamics.evolve(false);
         dynamics.evolve(false);
         dynamics.evolve(false);
         THEN("The agent has travelled the correct distance") {
-          CHECK_EQ(dynamics.time() - dynamics.agents().at(0)->spawnTime(), 2);
-          CHECK_EQ(dynamics.agents().at(0)->freeTime(), dynamics.time());
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
-          CHECK_EQ(dynamics.agents().at(0)->speed(), 13.8888888889);
-          CHECK_EQ(dynamics.agents().at(0)->distance(), 3.);
+          auto const& pAgent{dynamics.graph().edge(1)->queue(0).front()};
+          CHECK_EQ(dynamics.time() - pAgent->spawnTime(), 3);
+          CHECK_EQ(pAgent->freeTime() + 1, dynamics.time());
+          CHECK_EQ(pAgent->streetId().value(), 1);
+          CHECK_EQ(pAgent->speed(), 0.);
+          CHECK_EQ(pAgent->distance(), 3.);
         }
       }
     }
@@ -917,10 +908,11 @@ TEST_CASE("Dynamics") {
       graph2.addStreets(s1, s2);
       graph2.buildAdj();
       graph2.makeStochasticStreet(1, 0.3);
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.addItinerary(2, 2);
       dynamics.updatePaths();
-      dynamics.addAgent(0, 2, 0);
+      dynamics.addAgent(2, 0);
       WHEN("We evolve the dynamics") {
         dynamics.evolve(false);
         dynamics.evolve(false);
@@ -928,23 +920,26 @@ TEST_CASE("Dynamics") {
         dynamics.evolve(false);
         dynamics.evolve(false);
         dynamics.evolve(false);
+        dynamics.evolve(false);
 #ifndef __APPLE__
         THEN("The agent has travelled the correct distance") {
-          CHECK_EQ(dynamics.time() - dynamics.agents().at(0)->spawnTime(), 6);
-          CHECK_EQ(dynamics.agents().at(0)->freeTime(), dynamics.time());
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 5);
-          CHECK_EQ(dynamics.agents().at(0)->speed(), 13.8888888889);
-          CHECK_EQ(dynamics.agents().at(0)->distance(), 4.);
+          auto const& pAgent{dynamics.graph().edge(5)->queue(0).front()};
+          CHECK_EQ(dynamics.time() - pAgent->spawnTime(), 7);
+          CHECK_EQ(pAgent->freeTime() + 1, dynamics.time());
+          CHECK_EQ(pAgent->streetId().value(), 5);
+          CHECK_EQ(pAgent->speed(), 0.);
+          CHECK_EQ(pAgent->distance(), 4.);
         }
 #else
         dynamics.evolve(false);
         dynamics.evolve(false);
+        auto const& pAgent{dynamics.graph().edge(5)->queue(0).front()};
         THEN("The agent has travelled the correct distance") {
-          CHECK_EQ(dynamics.time() - dynamics.agents().at(0)->spawnTime(), 8);
-          CHECK_EQ(dynamics.agents().at(0)->freeTime(), dynamics.time());
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 5);
-          CHECK_EQ(dynamics.agents().at(0)->speed(), 13.8888888889);
-          CHECK_EQ(dynamics.agents().at(0)->distance(), 4.);
+          CHECK_EQ(dynamics.time() - pAgent->spawnTime(), 9);
+          CHECK_EQ(pAgent->freeTime() + 1, dynamics.time());
+          CHECK_EQ(pAgent->streetId().value(), 5);
+          CHECK_EQ(pAgent->speed(), 0.);
+          CHECK_EQ(pAgent->distance(), 4.);
         }
 #endif
       }
@@ -966,39 +961,26 @@ TEST_CASE("Dynamics") {
       node->setCapacity(4);
       node->setTransportCapacity(4);
     }
-    Dynamics dynamics{graph2, false, 69, 0.5};
+    FirstOrderDynamics dynamics{graph2, false, 69, 0.5};
     dynamics.addItinerary(2, 2);
     dynamics.updatePaths();
     dynamics.addAgents(4, 2, 0);
+    auto const& pStreet{dynamics.graph().edge(1)};
     dynamics.evolve(false);
     dynamics.evolve(false);
-    double meanSpeed{0.};
-    for (const auto& [agentId, agent] : dynamics.agents()) {
-      meanSpeed += agent->speed();
-    }
-    auto const& pStreet{dynamics.graph().edge(1)};
-    meanSpeed /= (pStreet->nExitingAgents() + pStreet->movingAgents().size());
-    CHECK_EQ(dynamics.streetMeanSpeed(1), meanSpeed);
+    CHECK_EQ(dynamics.streetMeanSpeed(1), 18.5);
     // I don't think the mean speed of agents should be equal to the street's
     // one... CHECK_EQ(dynamics.streetMeanSpeed().mean,
     // dynamics.agentMeanSpeed().mean); CHECK_EQ(dynamics.streetMeanSpeed().std,
     // 0.); street 1 density should be 0.4 so...
-    CHECK_EQ(dynamics.streetMeanSpeed(0.2, true).mean, meanSpeed);
+    CHECK_EQ(dynamics.streetMeanSpeed(0.2, true).mean, 18.5);
     CHECK_EQ(dynamics.streetMeanSpeed(0.2, true).std, 0.);
-    CHECK_EQ(dynamics.streetMeanSpeed(0.2, false).mean, 15.);
-    CHECK_EQ(dynamics.streetMeanSpeed(0.2, false).std, 0.);
+    CHECK_EQ(dynamics.streetMeanSpeed(0.8, false).mean, 15.875);
+    CHECK_EQ(dynamics.streetMeanSpeed(0.8, false).std, doctest::Approx(1.51554));
+    dynamics.evolve(false);
     dynamics.evolve(false);
-    meanSpeed = 0.;
-    for (const auto& [agentId, agent] : dynamics.agents()) {
-      if (!agent->streetId().has_value())
-        continue;
-      if (agent->streetId().value() == 1) {
-        meanSpeed += agent->speed();
-      }
-    }
-    meanSpeed /= pStreet->queue(0).size();
     CHECK_EQ(pStreet->queue(0).size(), 3);
-    CHECK_EQ(dynamics.streetMeanSpeed(1), meanSpeed);
+    CHECK_EQ(dynamics.streetMeanSpeed(1), 0.);
   }
   SUBCASE("Intersection priorities") {
     GIVEN("A dynamics object with five nodes and eight streets") {
@@ -1008,8 +990,8 @@ TEST_CASE("Dynamics") {
       graph2.addNode<Intersection>(2, std::make_pair(1, 1));    // B
       graph2.addNode<Intersection>(3, std::make_pair(1, -1));   // C
       graph2.addNode<Intersection>(4, std::make_pair(-1, -1));  // D
-      graph2.addEdge<Street>(0, std::make_pair(0, 1), 10., 10.);
-      graph2.addEdge<Street>(1, std::make_pair(0, 2), 10., 10.);
+      graph2.addEdge<Street>(0, std::make_pair(0, 1), 30., 10.);
+      graph2.addEdge<Street>(1, std::make_pair(0, 2), 30., 10.);
       graph2.addEdge<Street>(2, std::make_pair(0, 3), 10., 10.);
       graph2.addEdge<Street>(3, std::make_pair(0, 4), 10., 10.);
       graph2.addEdge<Street>(4, std::make_pair(1, 0), 10., 10.);
@@ -1017,54 +999,63 @@ TEST_CASE("Dynamics") {
       graph2.addEdge<Street>(6, std::make_pair(3, 0), 10., 10.);
       graph2.addEdge<Street>(7, std::make_pair(4, 0), 10., 10.);
       graph2.buildAdj();
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.graph().node(0)->setCapacity(3);
+      dynamics.graph().node(0)->setTransportCapacity(1);
+      auto& nodeO{dynamic_cast<Intersection&>(*dynamics.graph().node(0))};
       dynamics.addItinerary(1, 1);
       dynamics.addItinerary(2, 2);
       dynamics.updatePaths();
       WHEN("We add agents and evolve the dynamics") {
         // add an agent in C, D, A
-        dynamics.addAgent(0, 2, 4);
-        dynamics.addAgent(1, 2, 3);
-        dynamics.addAgent(2, 2, 1);
+        dynamics.addAgent(2, 4);  // Second
+        dynamics.addAgent(2, 3);  // Third
+        dynamics.addAgent(2, 1);  // First
+        dynamics.evolve(false);
         dynamics.evolve(false);
         dynamics.evolve(false);
         dynamics.evolve(false);
         THEN("The agent in A passes first") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 20);  // second
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 15);  // third
-          CHECK_EQ(dynamics.agents().at(2)->streetId().value(), 2);   // first
+          CHECK_EQ(dynamics.graph().edge(2)->nAgents(), 1);
+          CHECK_EQ(nodeO.agents().size(), 2);
+          CHECK_EQ(nodeO.agents().begin()->second->streetId().value(), 20);
         }
         dynamics.evolve(false);
         THEN("The agent in D passes second") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 2);   // first
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 15);  // second
+          CHECK_EQ(dynamics.graph().edge(2)->nAgents(), 2);
+          CHECK_EQ(nodeO.agents().size(), 1);
+          CHECK_EQ(nodeO.agents().begin()->second->streetId().value(), 15);
         }
         dynamics.evolve(false);
         THEN("The agent in C passes last") {
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 2);
+          CHECK_EQ(dynamics.graph().edge(2)->nAgents(), 3);
+          CHECK(nodeO.agents().empty());
         }
       }
       WHEN("We add agents of another itinerary and update the dynamics") {
-        dynamics.addAgent(0, 1, 2);
-        dynamics.addAgent(1, 1, 3);
-        dynamics.addAgent(2, 1, 4);
+        dynamics.addAgent(1, 2);
+        dynamics.addAgent(1, 3);
+        dynamics.addAgent(1, 4);
+        dynamics.evolve(false);
         dynamics.evolve(false);
         dynamics.evolve(false);
         dynamics.evolve(false);
         THEN("The agent in D passes first") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 10);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 15);
-          CHECK_EQ(dynamics.agents().at(2)->streetId().value(), 1);
+          CHECK_EQ(dynamics.graph().edge(1)->nAgents(), 1);
+          CHECK_EQ(nodeO.agents().size(), 2);
+          CHECK_EQ(nodeO.agents().begin()->second->streetId().value(), 15);
         }
         dynamics.evolve(false);
         THEN("The agent in C passes second") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 10);
-          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 1);
+          CHECK_EQ(dynamics.graph().edge(1)->nAgents(), 2);
+          CHECK_EQ(nodeO.agents().size(), 1);
+          CHECK_EQ(nodeO.agents().begin()->second->streetId().value(), 10);
         }
         dynamics.evolve(false);
         THEN("The agent in C passes last") {
-          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
+          CHECK_EQ(dynamics.graph().edge(1)->nAgents(), 3);
+          CHECK(nodeO.agents().empty());
         }
       }
     }
@@ -1075,10 +1066,11 @@ TEST_CASE("Dynamics") {
       graph2.addEdge<SpireStreet>(0, std::make_pair(0, 1), 10., 5.);
       graph2.addEdge<Street>(1, std::make_pair(1, 2), 10., 10.);
       graph2.buildAdj();
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.addItinerary(std::unique_ptr<Itinerary>(new Itinerary(2, 2)));
       dynamics.updatePaths();
-      dynamics.addAgent(0, 2, 0);
+      dynamics.addAgent(2, 0);
       WHEN("We evolve the dynamics") {
         dynamics.evolve(false);
         dynamics.evolve(false);
@@ -1103,10 +1095,11 @@ TEST_CASE("Dynamics") {
       graph2.addEdge<SpireStreet>(0, std::make_pair(0, 1), 10., 5.);
       graph2.addEdge(1, std::make_pair(1, 2), 10., 10.);
       graph2.buildAdj();
-      Dynamics dynamics{graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
+      FirstOrderDynamics dynamics{
+          graph2, false, 69, 0., dsm::weight_functions::streetLength, 1.};
       dynamics.addItinerary(2, 2);
       dynamics.updatePaths();
-      dynamics.addAgent(0, 2, 0);
+      dynamics.addAgent(2, 0);
       WHEN("We evolve the dynamics") {
         dynamics.evolve(false);
         dynamics.evolve(false);
diff --git a/test/Test_graph.cpp b/test/Test_graph.cpp
index 096b79dd2..9b6887a16 100644
--- a/test/Test_graph.cpp
+++ b/test/Test_graph.cpp
@@ -60,17 +60,12 @@ TEST_CASE("RoadNetwork") {
   }
 
   SUBCASE("Construction with addStreet") {
-    Street s1(1, std::make_pair(0, 1));
-    Street s2(2, std::make_pair(1, 2));
-    Street s3(3, std::make_pair(0, 2));
-    Street s4(4, std::make_pair(0, 3));
-    Street s5(5, std::make_pair(2, 3));
     RoadNetwork graph;
-    graph.addStreet(s1);
-    graph.addStreet(s2);
-    graph.addStreet(s3);
-    graph.addStreet(s4);
-    graph.addStreet(s5);
+    graph.addEdge<Street>(1, std::make_pair(0, 1));
+    graph.addEdge<Street>(2, std::make_pair(1, 2));
+    graph.addEdge<Street>(3, std::make_pair(0, 2));
+    graph.addEdge<Street>(4, std::make_pair(0, 3));
+    graph.addEdge<Street>(5, std::make_pair(2, 3));
     graph.buildAdj();
 
     CHECK_EQ(graph.nEdges(), 5);
@@ -263,8 +258,7 @@ TEST_CASE("RoadNetwork") {
     /// WHEN: we add a street
     /// THEN: the street is added
     RoadNetwork graph{};
-    Street street{1, std::make_pair(0, 1), 1.};
-    graph.addStreet(street);
+    graph.addEdge<Street>(1, std::make_pair(0, 1), 1.);
     auto result = graph.street(0, 1);
     CHECK(result);
     const auto& street2 = *result;
diff --git a/test/Test_node.cpp b/test/Test_node.cpp
index 99e5bf10a..60485dcee 100644
--- a/test/Test_node.cpp
+++ b/test/Test_node.cpp
@@ -19,8 +19,6 @@ TEST_CASE("Intersection") {
       constexpr dsm::Id id = 1;
       constexpr double lat = 2.5;
       constexpr double lon = 3.5;
-      constexpr dsm::Size capacity = 2;
-      constexpr dsm::Size transportCapacity = 3;
       const std::string name = "MyName";
       WHEN("An Intersection is created using only an Id") {
         Intersection intersection{id};
@@ -45,20 +43,20 @@ TEST_CASE("Intersection") {
         }
       }
       WHEN("An intersection is created using copy constructor") {
-        Intersection base{id, std::make_pair(lat, lon)};
-        base.setCapacity(capacity);
-        base.setTransportCapacity(transportCapacity);
-        base.setName(name);
-        auto copy = base;
-        THEN("Parameters are set correctly") {
-          CHECK_EQ(copy.id(), id);
-          CHECK(copy.coords().has_value());
-          CHECK_EQ(copy.coords().value().first, lat);
-          CHECK_EQ(copy.coords().value().second, lon);
-          CHECK_EQ(copy.capacity(), capacity);
-          CHECK_EQ(copy.transportCapacity(), transportCapacity);
-          CHECK_EQ(copy.name(), name);
-        }
+        // Intersection base{id, std::make_pair(lat, lon)};
+        // base.setCapacity(capacity);
+        // base.setTransportCapacity(transportCapacity);
+        // base.setName(name);
+        // auto copy = base;
+        // THEN("Parameters are set correctly") {
+        //   CHECK_EQ(copy.id(), id);
+        //   CHECK(copy.coords().has_value());
+        //   CHECK_EQ(copy.coords().value().first, lat);
+        //   CHECK_EQ(copy.coords().value().second, lon);
+        //   CHECK_EQ(copy.capacity(), capacity);
+        //   CHECK_EQ(copy.transportCapacity(), transportCapacity);
+        //   CHECK_EQ(copy.name(), name);
+        // }
       }
     }
   }
diff --git a/test/Test_street.cpp b/test/Test_street.cpp
index 01dee26b9..f5b305635 100644
--- a/test/Test_street.cpp
+++ b/test/Test_street.cpp
@@ -71,29 +71,44 @@ TEST_CASE("Street") {
     CHECK_EQ(street.maxSpeed(), 40.);
     CHECK_EQ(street.nLanes(), 1);
   }
+  SUBCASE("addAgent") {
+    Agent a1{0, 1, 0};
+    a1.setFreeTime(5);
+    Agent a2{0, 1, 0};
+    a2.setFreeTime(3);
+    Agent a3{0, 1, 0};
+    a3.setFreeTime(7);
 
+    Street street{1, std::make_pair(0, 1), 3.5};
+    street.addAgent(std::make_unique<Agent>(a1));
+    CHECK_EQ(street.movingAgents().top()->freeTime(), 5);
+    street.addAgent(std::make_unique<Agent>(a2));
+    CHECK_EQ(street.movingAgents().top()->freeTime(), 3);
+    street.addAgent(std::make_unique<Agent>(a3));
+    CHECK_EQ(street.movingAgents().top()->freeTime(), 3);
+  }
   SUBCASE("Enqueue") {
     /*This tests the insertion of an agent in a street's queue*/
 
     // define some agents
-    Agent a1{0, 1, 1, 0};  // they are all in street 1
-    Agent a2{0, 2, 1, 0};
-    Agent a3{0, 3, 1, 0};
-    Agent a4{0, 4, 1, 0};
+    Agent a1{0, 1, 0};  // they are all in street 1
+    Agent a2{0, 1, 0};
+    Agent a3{0, 1, 0};
+    Agent a4{0, 1, 0};
 
     Street street{1, std::make_pair(0, 1), 3.5};
     // fill the queue
-    street.addAgent(a1.id());
-    street.enqueue(a1.id(), 0);
-    street.addAgent(a2.id());
-    street.enqueue(a2.id(), 0);
+    street.addAgent(std::make_unique<Agent>(a1));
+    street.enqueue(0);
+    street.addAgent(std::make_unique<Agent>(a2));
+    street.enqueue(0);
     CHECK_EQ(doctest::Approx(street.density()), 0.571429);
-    street.addAgent(a3.id());
-    street.enqueue(a3.id(), 0);
-    street.addAgent(a4.id());
-    street.enqueue(a4.id(), 0);
-    CHECK_EQ(street.queue(0).front(), 1);
-    CHECK_EQ(street.queue(0).back(), 4);
+    street.addAgent(std::make_unique<Agent>(a3));
+    street.enqueue(0);
+    street.addAgent(std::make_unique<Agent>(a4));
+    street.enqueue(0);
+    CHECK(street.queue(0).front());
+    CHECK(street.queue(0).back());
     CHECK_EQ(street.queue(0).size(), street.capacity());
     CHECK_EQ(street.queue(0).size(), street.capacity());
     CHECK_EQ(doctest::Approx(street.density()), 1.14286);
@@ -104,31 +119,30 @@ TEST_CASE("Street") {
     /*This tests the exit of an agent from a street's queue*/
 
     // define some agents
-    Agent a1{0, 1, 1, 0};  // they are all in street 1
-    Agent a2{0, 2, 1, 0};
-    Agent a3{0, 3, 1, 0};
-    Agent a4{0, 4, 1, 0};
+    Agent a1{0, 1, 0};  // they are all in street 1
+    Agent a2{0, 1, 0};
+    Agent a3{0, 1, 0};
+    Agent a4{0, 1, 0};
 
     Street street{1, std::make_pair(0, 1), 3.5};
     // fill the queue
-    street.addAgent(a1.id());
-    street.enqueue(a1.id(), 0);
-    street.addAgent(a2.id());
-    street.enqueue(a2.id(), 0);
-    street.addAgent(a3.id());
-    street.enqueue(a3.id(), 0);
-    street.addAgent(a4.id());
-    street.enqueue(a4.id(), 0);
-    CHECK_EQ(street.queue(0).front(),
-             1);  // check that agent 1 is at the front of the queue
+    street.addAgent(std::make_unique<Agent>(a1));
+    street.enqueue(0);
+    street.addAgent(std::make_unique<Agent>(a2));
+    street.enqueue(0);
+    street.addAgent(std::make_unique<Agent>(a3));
+    street.enqueue(0);
+    street.addAgent(std::make_unique<Agent>(a4));
+    street.enqueue(0);
+    CHECK(street.queue(0).front());
     // dequeue
     street.dequeue(0);
-    CHECK_EQ(street.queue(0).front(), 2);  // check that agent 2 is now at front
+    CHECK(street.queue(0).front());  // check that agent 2 is now at front
     // check that the length of the queue has decreased
     CHECK_EQ(street.queue(0).size(), 3);
     CHECK_EQ(street.queue(0).size(), 3);
     // check that the next agent dequeued is agent 2
-    CHECK_EQ(street.dequeue(0), 2);
+    CHECK(street.dequeue(0));
     CHECK_EQ(street.queue(0).size(), 2);
     street.dequeue(0);
     street.dequeue(0);  // the queue is now empty
@@ -150,9 +164,9 @@ TEST_CASE("SpireStreet") {
     GIVEN("A spire street") {
       SpireStreet street{1, std::make_pair(0, 1), 3.5};
       WHEN("An agent is enqueued") {
-        street.addAgent(1);
+        street.addAgent(std::make_unique<Agent>(0, 1));
         THEN("The input flow is one") { CHECK_EQ(street.inputCounts(), 1); }
-        street.enqueue(1, 0);
+        street.enqueue(0);
         THEN("The density is updated") {
           CHECK_EQ(doctest::Approx(street.density()), 0.285714);
         }
@@ -160,12 +174,12 @@ TEST_CASE("SpireStreet") {
         THEN("Mean flow is one") { CHECK_EQ(street.meanFlow(), 1); }
       }
       WHEN("Three agents are enqueued") {
-        street.addAgent(1);
-        street.enqueue(1, 0);
-        street.addAgent(2);
-        street.enqueue(2, 0);
-        street.addAgent(3);
-        street.enqueue(3, 0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
         THEN("The density is updated") {
           CHECK_EQ(doctest::Approx(street.density()), 0.857143);
         }
@@ -174,8 +188,8 @@ TEST_CASE("SpireStreet") {
         THEN("Mean flow is three") { CHECK_EQ(street.meanFlow(), 3); }
       }
       WHEN("An agent is dequeued") {
-        street.addAgent(1);
-        street.enqueue(1, 0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
         street.dequeue(0);
         THEN("The density is updated") { CHECK_EQ(doctest::Approx(street.density()), 0); }
         THEN("Input flow is one") { CHECK_EQ(street.inputCounts(), 1); }
@@ -183,12 +197,12 @@ TEST_CASE("SpireStreet") {
         THEN("Mean flow is zero") { CHECK_EQ(street.meanFlow(), 0); }
       }
       WHEN("Three agents are dequeued") {
-        street.addAgent(1);
-        street.enqueue(1, 0);
-        street.addAgent(2);
-        street.enqueue(2, 0);
-        street.addAgent(3);
-        street.enqueue(3, 0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
         street.dequeue(0);
         street.dequeue(0);
         street.dequeue(0);
@@ -198,27 +212,27 @@ TEST_CASE("SpireStreet") {
         THEN("Mean flow is zero") { CHECK_EQ(street.meanFlow(), 0); }
       }
       WHEN("Input is greater than output") {
-        street.addAgent(1);
-        street.enqueue(1, 0);
-        street.addAgent(2);
-        street.enqueue(2, 0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
         street.dequeue(0);
         street.dequeue(0);
-        street.addAgent(3);
-        street.enqueue(3, 0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
         THEN("The density is updated") {
           CHECK_EQ(doctest::Approx(street.density()), 0.285714);
         }
         THEN("Mean flow is one") { CHECK_EQ(street.meanFlow(), 1); }
       }
       WHEN("Output is greater than input") {
-        street.addAgent(1);
-        street.enqueue(1, 0);
-        street.addAgent(2);
-        street.enqueue(2, 0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
         street.meanFlow();
-        street.addAgent(3);
-        street.enqueue(3, 0);
+        street.addAgent(std::make_unique<Agent>(0, 1));
+        street.enqueue(0);
         street.dequeue(0);
         street.dequeue(0);
         THEN("The density is updated") {