From 5041c197e605bdb71f510cbffd8049f5ba89f462 Mon Sep 17 00:00:00 2001
From: vero-dav-vero <100243585+vero-dav-vero@users.noreply.github.com>
Date: Thu, 3 Oct 2024 14:13:04 +0200
Subject: [PATCH 01/19] Update Dynamics.hpp with new algorithm

New algorithm from my thesis
---
 src/dsm/headers/Dynamics.hpp | 274 +++++++++++++++++++++--------------
 1 file changed, 163 insertions(+), 111 deletions(-)

diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp
index f0eaea739..99bdc0c95 100644
--- a/src/dsm/headers/Dynamics.hpp
+++ b/src/dsm/headers/Dynamics.hpp
@@ -6,7 +6,8 @@
 ///             of the graph's id and the type of the graph's capacity.
 ///             The graph's id and capacity must be unsigned integral types.
 
-#pragma once
+#ifndef dynamics_hpp
+#define dynamics_hpp
 
 #include <algorithm>
 #include <concepts>
@@ -17,8 +18,6 @@
 #include <unordered_map>
 #include <cmath>
 #include <cassert>
-#include <format>
-#include <thread>
 
 #include "Agent.hpp"
 #include "Itinerary.hpp"
@@ -86,6 +85,7 @@ namespace dsm {
     bool m_forcePriorities;
     std::unordered_map<Id, std::array<unsigned long long, 4>> m_turnCounts;
     std::unordered_map<Id, std::array<long, 4>> m_turnMapping;
+    std::unordered_map<Id,unsigned long long> m_streetTails;
 
     /// @brief Get the next street id
     /// @param agentId The id of the agent
@@ -110,9 +110,6 @@ namespace dsm {
     /// @details Puts all new agents on a street, if possible, decrements all delays
     /// and increments all travel times.
     virtual void m_evolveAgents();
-    /// @brief Update the path of a single itinerary
-    /// @param pItinerary An std::unique_prt to the itinerary
-    void m_updatePath(const std::unique_ptr<Itinerary<Id>>& pItinerary);
 
   public:
     Dynamics() = delete;
@@ -325,6 +322,7 @@ namespace dsm {
         m_maxFlowPercentage{1.},
         m_forcePriorities{false} {
     for (const auto& [streetId, street] : m_graph.streetSet()) {
+      m_streetTails.emplace(streetId, 0);
       m_turnCounts.emplace(streetId, std::array<unsigned long long, 4>{0, 0, 0, 0});
       // fill turn mapping as [streetId, [left street Id, straight street Id, right street Id, U self street Id]]
       m_turnMapping.emplace(streetId, std::array<long, 4>{-1, -1, -1, -1});
@@ -414,6 +412,10 @@ namespace dsm {
       if (m_agents[agentId]->delay() > 0) {
         continue;
       }
+      if(m_time % 30 == 0) {
+        //m_streetTails[streetId] += street->queue().size();
+        m_streetTails[streetId] += street->waitingAgents().size();
+      }
       m_agents[agentId]->setSpeed(0.);
       const auto& destinationNode{this->m_graph.nodeSet()[street->nodePair().second]};
       if (destinationNode->isFull()) {
@@ -582,53 +584,6 @@ namespace dsm {
     }
   }
 
-  template <typename Id, typename Size, typename Delay>
-    requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
-             is_numeric_v<Delay>)
-  void Dynamics<Id, Size, Delay>::m_updatePath(
-      const std::unique_ptr<Itinerary<Id>>& pItinerary) {
-    const Size dimension = m_graph.adjMatrix().getRowDim();
-    const auto destinationID = pItinerary->destination();
-    SparseMatrix<Id, bool> path{dimension, dimension};
-    // cycle over the nodes
-    for (const auto& [nodeId, node] : m_graph.nodeSet()) {
-      if (nodeId == destinationID) {
-        continue;
-      }
-      auto result{m_graph.shortestPath(nodeId, destinationID)};
-      if (!result.has_value()) {
-        continue;
-      }
-      // save the minimum distance between i and the destination
-      const auto minDistance{result.value().distance()};
-      for (const auto [nextNodeId, _] : m_graph.adjMatrix().getRow(nodeId)) {
-        // init distance from a neighbor node to the destination to zero
-        double distance{0.};
-
-        // TimePoint expectedTravelTime{
-        //     streetLength};  // / street->maxSpeed()};  // TODO: change into input speed
-        result = m_graph.shortestPath(nextNodeId, destinationID);
-
-        if (result.has_value()) {
-          // if the shortest path exists, save the distance
-          distance = result.value().distance();
-        } else if (nextNodeId != destinationID) {
-          std::cerr << std::format(
-                           "WARNING: No path found between from node {} to node {}",
-                           nodeId,
-                           destinationID)
-                    << std::endl;
-        }
-        if (minDistance ==
-            distance +
-                m_graph.streetSet().at(nodeId * dimension + nextNodeId)->length()) {
-          path.insert(nodeId, nextNodeId, true);
-        }
-      }
-      pItinerary->setPath(path);
-    }
-  }
-
   template <typename Id, typename Size, typename Delay>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
@@ -643,8 +598,8 @@ namespace dsm {
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::setMinSpeedRateo(double minSpeedRateo) {
     if (minSpeedRateo < 0. || minSpeedRateo > 1.) {
-      throw std::invalid_argument(buildLog(std::format(
-          "The minimum speed rateo ({}) must be between 0 and 1", minSpeedRateo)));
+      throw std::invalid_argument(
+          buildLog("The minim speed rateo must be between 0 and 1"));
     }
     m_minSpeedRateo = minSpeedRateo;
   }
@@ -654,8 +609,8 @@ namespace dsm {
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::setErrorProbability(double errorProbability) {
     if (errorProbability < 0. || errorProbability > 1.) {
-      throw std::invalid_argument(buildLog(std::format(
-          "The error probability ({}) must be between 0 and 1", errorProbability)));
+      throw std::invalid_argument(
+          buildLog("The error probability must be between 0 and 1"));
     }
     m_errorProbability = errorProbability;
   }
@@ -666,8 +621,7 @@ namespace dsm {
   void Dynamics<Id, Size, Delay>::setMaxFlowPercentage(double maxFlowPercentage) {
     if (maxFlowPercentage < 0. || maxFlowPercentage > 1.) {
       throw std::invalid_argument(
-          buildLog(std::format("The maximum flow percentage ({}) must be between 0 and 1",
-                               maxFlowPercentage)));
+          buildLog("The maximum flow percentage must be between 0 and 1"));
     }
     m_maxFlowPercentage = maxFlowPercentage;
   }
@@ -676,14 +630,52 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::updatePaths() {
-    std::vector<std::thread> threads;
-    threads.reserve(m_itineraries.size());
+    const Size dimension = m_graph.adjMatrix().getRowDim();
     for (const auto& [itineraryId, itinerary] : m_itineraries) {
-      threads.emplace_back(
-          std::thread([this, &itinerary] { this->m_updatePath(itinerary); }));
-    }
-    for (auto& thread : threads) {
-      thread.join();
+      SparseMatrix<Id, bool> path{dimension, dimension};
+      // cycle over the nodes
+      for (const auto& [nodeId, node] : m_graph.nodeSet()) {
+        if (nodeId == itinerary->destination()) {
+          continue;
+        }
+        auto result{m_graph.shortestPath(nodeId, itinerary->destination())};
+        if (!result.has_value()) {
+          continue;
+        }
+        // save the minimum distance between i and the destination
+        const auto minDistance{result.value().distance()};
+        for (const auto [nextNodeId, _] : m_graph.adjMatrix().getRow(nodeId)) {
+          // init distance from a neighbor node to the destination to zero
+          double distance{0.};
+
+          // TimePoint expectedTravelTime{
+          //     streetLength};  // / street->maxSpeed()};  // TODO: change into input velocity
+          result = m_graph.shortestPath(nextNodeId, itinerary->destination());
+
+          if (result.has_value()) {
+            // if the shortest path exists, save the distance
+            distance = result.value().distance();
+          } else if (nextNodeId != itinerary->destination()) {
+            std::cerr << "WARNING: No path found between " << nodeId << " and "
+                      << itinerary->destination() << '\n';
+          }
+
+          // if (!(distance > minDistance + expectedTravelTime)) {
+          if (minDistance ==
+              distance +
+                  m_graph.streetSet().at(nodeId * dimension + nextNodeId)->length()) {
+            // std::cout << "minDistance: " << minDistance << " distance: " << distance
+            //           << " streetLength: " << streetLength << '\n';
+            // std::cout << "Inserting " << i << ';' << node.first << '\n';
+            path.insert(nodeId, nextNodeId, true);
+          }
+        }
+        itinerary->setPath(path);
+        // for (auto i{0}; i < dimension; ++i) {
+        //   std::cout << path.getRow(i).size() << ' ';
+        // }
+        // std::cout << '\n';
+      }
     }
   }
 
@@ -708,64 +700,142 @@ namespace dsm {
                                                         double threshold) {
     for (const auto& [nodeId, node] : m_graph.nodeSet()) {
       if (!node->isTrafficLight()) {
+        //std::cout << '\t' << " -> Non è un semaforo" << '\n';
         continue;
       }
       auto& tl = dynamic_cast<TrafficLight<Id, Size, Delay>&>(*node);
       if (!tl.delay().has_value()) {
+        //std::cout << '\t' << " -> Il delay non ha valore" << '\n';
         continue;
       }
+      //std::cout << '\t' << '\t' << '\t' << "NODO " << nodeId << '\n';
       auto [greenTime, redTime] = tl.delay().value();
+      //std::cout << '\t' << " -> greenTime: " << static_cast<unsigned int>(greenTime) << '\n';
+      //std::cout << '\t' << " -> redTime: " << static_cast<unsigned int>(redTime) << '\n';
       const auto cycleTime = greenTime + redTime;
-      // const Delay delta = cycleTime * percentage;
       const auto& streetPriorities = tl.streetPriorities();
       Size greenSum{0}, greenQueue{0};
       Size redSum{0}, redQueue{0};
       for (const auto& [streetId, _] : m_graph.adjMatrix().getCol(nodeId, true)) {
         if (streetPriorities.contains(streetId)) {
-          greenSum += m_graph.streetSet()[streetId]->nAgents();
-          greenQueue += m_graph.streetSet()[streetId]->queue().size();
+          greenSum += m_streetTails[streetId];
+          greenQueue += m_graph.streetSet()[streetId]->queue().size(); 
         } else {
-          redSum += m_graph.streetSet()[streetId]->nAgents();
+          redSum += m_streetTails[streetId];
           redQueue += m_graph.streetSet()[streetId]->queue().size();
         }
       }
+      //std::cout << '\t' << " -> greenSum: " << static_cast<unsigned int>(greenSum) << '\n';
+      //std::cout << '\t' << " -> greenQueue: " << static_cast<unsigned int>(greenQueue) << '\n';
+      //std::cout << '\t' << " -> redSum: " << static_cast<unsigned int>(redSum) << '\n';
+      //std::cout << '\t' << " -> redQueue: " << static_cast<unsigned int>(redQueue) << '\n';
       const Delay delta =
           std::floor(std::abs(static_cast<int>(greenQueue - redQueue)) / percentage);
+          //std::cout << '\t' << " -> delta: " << static_cast<unsigned int>(delta) << '\n';
       if (delta == 0) {
+        //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
         continue;
       }
       const Size smallest = std::min(greenSum, redSum);
       if (std::abs(static_cast<int>(greenSum - redSum)) < threshold * smallest) {
+        //std::cout << '\t' << " -> MINORE della threshold" << '\n';
         tl.setDelay(std::floor(cycleTime / 2));
+        //auto [greenTime1, redTime1] = tl.delay().value();
+        //std::cout << '\t' << " -> greenTime: " << static_cast<unsigned int>(greenTime1) << '\n';
+        //std::cout << '\t' << " -> redTime: " << static_cast<unsigned int>(redTime1) << '\n';
+        //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
         continue;
       }
-      if ((greenSum > redSum) && !(greenTime > redTime) && (greenQueue > redQueue)) {
-        if (redTime > delta) {
-          greenTime += delta;
-          redTime -= delta;
-          tl.setDelay(std::make_pair(greenTime, redTime));
+      //std::cout << '\t' << " -> MAGGIORE della threshold" << '\n';
+      // se non vale la condizione sopra (vale quella col >)
+      // cosa devo fare:
+      //    - controllo che le le strade che danno sul nodo abbiano una densità minore della densità media (o minore
+      //      della densità media + percentuale): voglio evitare di essere dentro al cluster. Voglio essere fuori o (al 
+      //      più) sul bordo
+      //    - se è minore: faccio fare all'algoritmo quello che vuole fare
+      //    - se NON lo è: non gli faccio fare nulla (non modifico nulla)
+      auto meanDensity_whole = streetMeanDensity().mean;  // Measurement<double>
+      double densitySum{};
+      int i{};
+      // cerco su tutto il set di strade quelle che sono USCENTI rispetto a quel nodo e mi salvo il loro id
+      for (const auto& [streetId, street] : m_graph.streetSet()) {
+        if (street->nodePair().first == nodeId) {
+        i++;
+        auto density = street->density();
+        densitySum += density;
         }
-      } else if (!(redTime > greenTime) && (greenTime > delta) &&
-                 (redQueue > greenQueue)) {
-        greenTime -= delta;
-        redTime += delta;
-        tl.setDelay(std::make_pair(greenTime, redTime));
       }
+      // fai la media delle quattro e confronti la media
+      auto meanDensity_streets = densitySum / i;
+      //std::cout << '\t' << " -> Densità media del network: " << std::setprecision(7) << meanDensity_whole << '\n';
+      //std::cout << '\t' << " -> Densità media delle 4 strade uscenti: " << std::setprecision(7) << meanDensity_streets << '\n';
+      auto ratio = meanDensity_whole/meanDensity_streets;
+      auto dyn_thresh = std::tanh(ratio) * 3/10; // il 3/10 è lì perché è il massimo bordo che voglio includere
+      //std::cout << '\t' << " -> Parametro ratio: " << std::setprecision(7) << ratio << '\n';
+      //std::cout << '\t' << " -> Parametro dyn_thresh: " << std::setprecision(7) << dyn_thresh << '\n';
+      if (meanDensity_whole + (dyn_thresh) * meanDensity_whole > meanDensity_streets) {
+        //std::cout << '\t' << " -> Sono sul BORDO del cluster" << '\n';
+        if(meanDensity_whole > meanDensity_streets) {
+          //std::cout << '\t' << " -> MINORE della densità massima" << '\n';
+          if(!(redTime > greenTime) && (redSum > greenSum) && (greenTime > delta)) {
+              greenTime -= delta; 
+              redTime += delta;
+              tl.setDelay(std::make_pair(greenTime, redTime));
+            } else if (!(greenTime > redTime) && (greenSum > redSum) && (redTime > delta)) {
+              greenTime += delta;
+              redTime -= delta; 
+              tl.setDelay(std::make_pair(greenTime, redTime));
+            } else { 
+              //std::cout << '\t' << " -> NON sono entrato negli if precedenti" << '\n';
+              tl.setDelay(std::make_pair(greenTime, redTime)); 
+            }
+            //std::cout << '\t' << " -> greenTime: " << static_cast<unsigned int>(greenTime) << '\n';
+            //std::cout << '\t' << " -> redTime: " << static_cast<unsigned int>(redTime) << '\n';
+            //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
+        } else {
+          //std::cout << '\t' << " -> MAGGIORE della densità massima" << '\n';
+          if(!(redTime > greenTime) && (redSum > greenSum) && (greenTime > ratio * delta)) {
+            greenTime -= dyn_thresh * delta;  //
+            redTime += delta;
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else if (!(greenTime > redTime) && (greenSum > redSum) && (redTime > ratio * delta)) {
+            greenTime += delta;
+            redTime -= dyn_thresh * delta;      //
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else if (!(redTime > greenTime) && (redSum < greenSum) && (redTime > delta)) {
+            greenTime += dyn_thresh * delta;    //
+            redTime -= delta;
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else if (!(redTime < greenTime) && (redSum > greenSum) && (greenTime > delta)) {
+            greenTime -= delta;
+            redTime += dyn_thresh * delta;      //
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else {
+            //std::cout << '\t' << " -> NON sono entrato negli if precedenti" << '\n';
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          }
+          //std::cout << '\t' << " -> greenTime: " << static_cast<unsigned int>(greenTime) << '\n';
+          //std::cout << '\t' << " -> redTime: " << static_cast<unsigned int>(redTime) << '\n';
+          //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
+        }
+      } else {
+        //std::cout << '\t' << " -> Sono DENTRO il cluster" << '\n';
+        //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
+      } 
+    }
+    for (auto& [id, element] : m_streetTails) {
+      element = 0;
     }
   }
 
+
   template <typename Id, typename Size, typename Delay>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::addAgent(const Agent<Id, Size, Delay>& agent) {
-    if (this->m_agents.size() + 1 > this->m_graph.maxCapacity()) {
-      throw std::overflow_error(buildLog(
-          std::format("Graph its already holding the max possible number of agents ({})",
-                      this->m_graph.maxCapacity())));
-    }
     if (this->m_agents.contains(agent.id())) {
       throw std::invalid_argument(
-          buildLog(std::format("Agent with id {} already exists.", agent.id())));
+          buildLog("Agent " + std::to_string(agent.id()) + " already exists."));
     }
     this->m_agents.emplace(agent.id(), std::make_unique<Agent<Id, Size, Delay>>(agent));
   }
@@ -773,14 +843,9 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::addAgent(std::unique_ptr<Agent<Id, Size, Delay>> agent) {
-    if (this->m_agents.size() + 1 > this->m_graph.maxCapacity()) {
-      throw std::overflow_error(buildLog(
-          std::format("Graph its already holding the max possible number of agents ({})",
-                      this->m_graph.maxCapacity())));
-    }
     if (this->m_agents.contains(agent->id())) {
       throw std::invalid_argument(
-          buildLog(std::format("Agent with id {} already exists.", agent->id())));
+          buildLog("Agent " + std::to_string(agent->id()) + " already exists."));
     }
     this->m_agents.emplace(agent->id(), std::move(agent));
   }
@@ -790,21 +855,16 @@ namespace dsm {
   void Dynamics<Id, Size, Delay>::addAgents(Id itineraryId,
                                             Size nAgents,
                                             std::optional<Id> srcNodeId) {
-    if (this->m_agents.size() + nAgents > this->m_graph.maxCapacity()) {
-      throw std::overflow_error(buildLog(
-          std::format("Graph its already holding the max possible number of agents ({})",
-                      this->m_graph.maxCapacity())));
-    }
     auto itineraryIt{m_itineraries.find(itineraryId)};
     if (itineraryIt == m_itineraries.end()) {
       throw std::invalid_argument(
-          buildLog(std::format("Itinerary with id {} not found", itineraryId)));
+          buildLog("Itinerary " + std::to_string(itineraryId) + " not found"));
     }
     Size agentId{0};
     if (!this->m_agents.empty()) {
       agentId = this->m_agents.rbegin()->first + 1;
     }
-    for (Size i{0}; i < nAgents; ++i, ++agentId) {
+    for (auto i{0}; i < nAgents; ++i, ++agentId) {
       this->addAgent(Agent<Id, Size, Delay>{agentId, itineraryId});
       if (srcNodeId.has_value()) {
         this->m_agents[agentId]->setSourceNodeId(srcNodeId.value());
@@ -833,11 +893,6 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::addAgents(std::span<Agent<Id, Size, Delay>> agents) {
-    if (this->m_agents.size() + agents.size() > this->m_graph.maxCapacity()) {
-      throw std::overflow_error(buildLog(
-          std::format("Graph its already holding the max possible number of agents ({})",
-                      this->m_graph.maxCapacity())));
-    }
     std::ranges::for_each(agents, [this](const auto& agent) -> void {
       this->m_agents.push_back(std::make_unique(agent));
     });
@@ -848,14 +903,9 @@ namespace dsm {
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::addAgentsUniformly(Size nAgents,
                                                      std::optional<Id> itineraryId) {
-    if (this->m_agents.size() + nAgents > this->m_graph.maxCapacity()) {
-      throw std::overflow_error(buildLog(
-          std::format("Graph its already holding the max possible number of agents ({})",
-                      this->m_graph.maxCapacity())));
-    }
     if (this->m_itineraries.empty()) {
       // TODO: make this possible for random agents
-      throw std::invalid_argument(
+      throw std::runtime_error(
           buildLog("It is not possible to add random agents without itineraries."));
     }
     const bool randomItinerary{!itineraryId.has_value()};
@@ -901,7 +951,7 @@ namespace dsm {
     auto agentIt{m_agents.find(agentId)};
     if (agentIt == m_agents.end()) {
       throw std::invalid_argument(
-          buildLog(std::format("Agent with id {} not found.", agentId)));
+          buildLog("Agent " + std::to_string(agentId) + " not found."));
     }
     m_agents.erase(agentId);
   }
@@ -1294,3 +1344,5 @@ namespace dsm {
   void SecondOrderDynamics<Id, Size>::setSpeed() {}
 
 };  // namespace dsm
+
+#endif

From 0811de41d70914ed7e5833ec8593460a8b4507d0 Mon Sep 17 00:00:00 2001
From: vero-dav-vero <100243585+vero-dav-vero@users.noreply.github.com>
Date: Thu, 3 Oct 2024 14:35:13 +0200
Subject: [PATCH 02/19] Update Node.hpp with modTime

modTime to keep track of how the algorithm changes the values of redTime and greenTime
---
 src/dsm/headers/Node.hpp | 54 ++++++++++++++++++++++++++--------------
 1 file changed, 36 insertions(+), 18 deletions(-)

diff --git a/src/dsm/headers/Node.hpp b/src/dsm/headers/Node.hpp
index 3cf8e913c..43e78da80 100644
--- a/src/dsm/headers/Node.hpp
+++ b/src/dsm/headers/Node.hpp
@@ -9,7 +9,8 @@
 ///               - TrafficLight: represents a traffic light intersection node
 ///             - Roundabout: represents a roundabout node with a queue of agents
 
-#pragma once
+#ifndef Node_hpp
+#define Node_hpp
 
 #include <functional>
 #include <utility>
@@ -17,7 +18,6 @@
 #include <optional>
 #include <set>
 #include <map>
-#include <format>
 
 #include "../utility/Logger.hpp"
 #include "../utility/queue.hpp"
@@ -151,10 +151,8 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   void Node<Id, Size>::setCapacity(Size capacity) {
     if (capacity < m_agents.size()) {
-      throw std::runtime_error(buildLog(
-          std::format("Node capacity ({}) is smaller than the current queue size ({}).",
-                      capacity,
-                      m_agents.size())));
+      throw std::runtime_error(
+          buildLog("Node capacity is smaller than the current queue size"));
     }
     NodeConcept<Id, Size>::setCapacity(capacity);
   }
@@ -163,12 +161,11 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   void Node<Id, Size>::addAgent(double angle, Id agentId) {
     if (m_agents.size() == this->m_capacity) {
-      throw std::runtime_error(buildLog("Node is full."));
+      throw std::runtime_error(buildLog("Node is full"));
     }
     for (auto const [angle, id] : m_agents) {
       if (id == agentId) {
-        throw std::runtime_error(
-            buildLog(std::format("Agent with id {} is already on the node.", agentId)));
+        throw std::runtime_error(buildLog("Agent is already on the node."));
       }
     }
     auto iAngle{static_cast<int16_t>(angle * 100)};
@@ -180,12 +177,11 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   void Node<Id, Size>::addAgent(Id agentId) {
     if (m_agents.size() == this->m_capacity) {
-      throw std::runtime_error(buildLog("Node is full."));
+      throw std::runtime_error(buildLog("Node is full"));
     }
     for (auto const [angle, id] : m_agents) {
       if (id == agentId) {
-        throw std::runtime_error(
-            buildLog(std::format("Agent with id {} is already on the node.", id)));
+        throw std::runtime_error(buildLog("Agent is already on the node."));
       }
     }
     int lastKey{0};
@@ -205,8 +201,7 @@ namespace dsm {
         return;
       }
     }
-    throw std::runtime_error(
-        buildLog(std::format("Agent with id {} is not on the node", agentId)));
+    throw std::runtime_error(buildLog("Agent is not on the node"));
   }
 
   template <typename Id, typename Size>
@@ -225,6 +220,12 @@ namespace dsm {
     std::optional<std::pair<Delay, Delay>> m_delay;
     Delay m_counter;
     Delay m_phase;
+    /// @brief Variabile che mi dice come sono in rapporto tra di loro il tempo di verde e il tempo di rosso
+    /// @details Valori possibili: 0, 1, 2 (fino a che non verrà modificata)
+    ///           - se 0: |redTime - greenTime| < 10 && redTime > 5 && greenTime > 5
+    ///           - se 1: |redtime - greenTime| \geq 10 && |redTime - greenTime| < 40 && redTime > 5 && greenTime > 5
+    ///           - se 2: |redTime - greenTime| \geq 40 || redTime < 5 || greenTime < 5
+    int m_modTime{7};
 
   public:
     TrafficLight() = delete;
@@ -272,6 +273,9 @@ namespace dsm {
     /// @return std::optional<Delay> The node's delay
     std::optional<std::pair<Delay, Delay>> delay() const { return m_delay; }
     Delay counter() const { return m_counter; }
+    /// @brief Get the node's modTime
+    /// @return int. The node's modTime
+    int modTime() const { return m_modTime; }
     /// @brief Returns true if the traffic light is green
     /// @return bool True if the traffic light is green
     bool isGreen() const;
@@ -304,6 +308,9 @@ namespace dsm {
       }
     }
     m_delay = std::make_pair(delay, delay);
+    if (delay < 5) { 
+      m_modTime = 2; 
+    } else { m_modTime = 0; }
   }
   template <typename Id, typename Size, typename Delay>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
@@ -319,6 +326,16 @@ namespace dsm {
       }
     }
     m_delay = std::move(delay);
+    if (delay.first < 5 || delay.second < 5) {
+      m_modTime = 2;
+    } else if (std::abs(delay.first - delay.second) < 10) {
+      m_modTime = 0;
+    } else if ((std::abs(delay.first - delay.second) > 10 || std::abs(delay.first - delay.second) == 10) 
+          && (std::abs(delay.first - delay.second) < 40 || std::abs(delay.first - delay.second) == 40)) {
+      m_modTime = 1;
+    } else if (std::abs(delay.first - delay.second) > 40) {
+      m_modTime = 2;
+    }
   }
   template <typename Id, typename Size, typename Delay>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
@@ -440,12 +457,11 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   void Roundabout<Id, Size>::enqueue(Id agentId) {
     if (m_agents.size() == this->m_capacity) {
-      throw std::runtime_error(buildLog("Roundabout is full."));
+      throw std::runtime_error(buildLog("Roundabout is full"));
     }
     for (const auto id : m_agents) {
       if (id == agentId) {
-        throw std::runtime_error(buildLog(
-            std::format("Agent with id {} is already on the roundabout.", agentId)));
+        throw std::runtime_error(buildLog("Agent is already on the roundabout."));
       }
     }
     m_agents.push(agentId);
@@ -454,10 +470,12 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   Id Roundabout<Id, Size>::dequeue() {
     if (m_agents.empty()) {
-      throw std::runtime_error(buildLog("Roundabout is empty."));
+      throw std::runtime_error(buildLog("Roundabout is empty"));
     }
     Id agentId{m_agents.front()};
     m_agents.pop();
     return agentId;
   }
 };  // namespace dsm
+
+#endif

From 4aa4b74c6733a28f6a4def5e926895efb3cc6529 Mon Sep 17 00:00:00 2001
From: Gregorio Berselli <gregorio_2000@hotmail.it>
Date: Thu, 3 Oct 2024 22:19:55 +0200
Subject: [PATCH 03/19] Merge Dynamics.hpp with main branch

---
 src/dsm/headers/Dynamics.hpp | 270 +++++++++++++++++++----------------
 1 file changed, 145 insertions(+), 125 deletions(-)

diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp
index 99bdc0c95..c952ae71a 100644
--- a/src/dsm/headers/Dynamics.hpp
+++ b/src/dsm/headers/Dynamics.hpp
@@ -6,8 +6,7 @@
 ///             of the graph's id and the type of the graph's capacity.
 ///             The graph's id and capacity must be unsigned integral types.
 
-#ifndef dynamics_hpp
-#define dynamics_hpp
+#pragma once
 
 #include <algorithm>
 #include <concepts>
@@ -18,6 +17,8 @@
 #include <unordered_map>
 #include <cmath>
 #include <cassert>
+#include <format>
+#include <thread>
 
 #include "Agent.hpp"
 #include "Itinerary.hpp"
@@ -85,7 +86,7 @@ namespace dsm {
     bool m_forcePriorities;
     std::unordered_map<Id, std::array<unsigned long long, 4>> m_turnCounts;
     std::unordered_map<Id, std::array<long, 4>> m_turnMapping;
-    std::unordered_map<Id,unsigned long long> m_streetTails;
+    std::unordered_map<Id, unsigned long long> m_streetTails;
 
     /// @brief Get the next street id
     /// @param agentId The id of the agent
@@ -110,6 +111,9 @@ namespace dsm {
     /// @details Puts all new agents on a street, if possible, decrements all delays
     /// and increments all travel times.
     virtual void m_evolveAgents();
+    /// @brief Update the path of a single itinerary
+    /// @param pItinerary An std::unique_prt to the itinerary
+    void m_updatePath(const std::unique_ptr<Itinerary<Id>>& pItinerary);
 
   public:
     Dynamics() = delete;
@@ -412,7 +416,7 @@ namespace dsm {
       if (m_agents[agentId]->delay() > 0) {
         continue;
       }
-      if(m_time % 30 == 0) {
+      if (m_time % 30 == 0) {
         //m_streetTails[streetId] += street->queue().size();
         m_streetTails[streetId] += street->waitingAgents().size();
       }
@@ -584,6 +588,53 @@ namespace dsm {
     }
   }
 
+  template <typename Id, typename Size, typename Delay>
+    requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
+             is_numeric_v<Delay>)
+  void Dynamics<Id, Size, Delay>::m_updatePath(
+      const std::unique_ptr<Itinerary<Id>>& pItinerary) {
+    const Size dimension = m_graph.adjMatrix().getRowDim();
+    const auto destinationID = pItinerary->destination();
+    SparseMatrix<Id, bool> path{dimension, dimension};
+    // cycle over the nodes
+    for (const auto& [nodeId, node] : m_graph.nodeSet()) {
+      if (nodeId == destinationID) {
+        continue;
+      }
+      auto result{m_graph.shortestPath(nodeId, destinationID)};
+      if (!result.has_value()) {
+        continue;
+      }
+      // save the minimum distance between i and the destination
+      const auto minDistance{result.value().distance()};
+      for (const auto [nextNodeId, _] : m_graph.adjMatrix().getRow(nodeId)) {
+        // init distance from a neighbor node to the destination to zero
+        double distance{0.};
+
+        // TimePoint expectedTravelTime{
+        //     streetLength};  // / street->maxSpeed()};  // TODO: change into input speed
+        result = m_graph.shortestPath(nextNodeId, destinationID);
+
+        if (result.has_value()) {
+          // if the shortest path exists, save the distance
+          distance = result.value().distance();
+        } else if (nextNodeId != destinationID) {
+          std::cerr << std::format(
+                           "WARNING: No path found between from node {} to node {}",
+                           nodeId,
+                           destinationID)
+                    << std::endl;
+        }
+        if (minDistance ==
+            distance +
+                m_graph.streetSet().at(nodeId * dimension + nextNodeId)->length()) {
+          path.insert(nodeId, nextNodeId, true);
+        }
+      }
+      pItinerary->setPath(path);
+    }
+  }
+
   template <typename Id, typename Size, typename Delay>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
@@ -598,8 +649,8 @@ namespace dsm {
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::setMinSpeedRateo(double minSpeedRateo) {
     if (minSpeedRateo < 0. || minSpeedRateo > 1.) {
-      throw std::invalid_argument(
-          buildLog("The minim speed rateo must be between 0 and 1"));
+      throw std::invalid_argument(buildLog(std::format(
+          "The minimum speed rateo ({}) must be between 0 and 1", minSpeedRateo)));
     }
     m_minSpeedRateo = minSpeedRateo;
   }
@@ -609,8 +660,8 @@ namespace dsm {
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::setErrorProbability(double errorProbability) {
     if (errorProbability < 0. || errorProbability > 1.) {
-      throw std::invalid_argument(
-          buildLog("The error probability must be between 0 and 1"));
+      throw std::invalid_argument(buildLog(std::format(
+          "The error probability ({}) must be between 0 and 1", errorProbability)));
     }
     m_errorProbability = errorProbability;
   }
@@ -621,7 +672,8 @@ namespace dsm {
   void Dynamics<Id, Size, Delay>::setMaxFlowPercentage(double maxFlowPercentage) {
     if (maxFlowPercentage < 0. || maxFlowPercentage > 1.) {
       throw std::invalid_argument(
-          buildLog("The maximum flow percentage must be between 0 and 1"));
+          buildLog(std::format("The maximum flow percentage ({}) must be between 0 and 1",
+                               maxFlowPercentage)));
     }
     m_maxFlowPercentage = maxFlowPercentage;
   }
@@ -630,52 +682,14 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::updatePaths() {
-    const Size dimension = m_graph.adjMatrix().getRowDim();
+    std::vector<std::thread> threads;
+    threads.reserve(m_itineraries.size());
     for (const auto& [itineraryId, itinerary] : m_itineraries) {
-      SparseMatrix<Id, bool> path{dimension, dimension};
-      // cycle over the nodes
-      for (const auto& [nodeId, node] : m_graph.nodeSet()) {
-        if (nodeId == itinerary->destination()) {
-          continue;
-        }
-        auto result{m_graph.shortestPath(nodeId, itinerary->destination())};
-        if (!result.has_value()) {
-          continue;
-        }
-        // save the minimum distance between i and the destination
-        const auto minDistance{result.value().distance()};
-        for (const auto [nextNodeId, _] : m_graph.adjMatrix().getRow(nodeId)) {
-          // init distance from a neighbor node to the destination to zero
-          double distance{0.};
-
-          // TimePoint expectedTravelTime{
-          //     streetLength};  // / street->maxSpeed()};  // TODO: change into input velocity
-          result = m_graph.shortestPath(nextNodeId, itinerary->destination());
-
-          if (result.has_value()) {
-            // if the shortest path exists, save the distance
-            distance = result.value().distance();
-          } else if (nextNodeId != itinerary->destination()) {
-            std::cerr << "WARNING: No path found between " << nodeId << " and "
-                      << itinerary->destination() << '\n';
-          }
-
-          // if (!(distance > minDistance + expectedTravelTime)) {
-          if (minDistance ==
-              distance +
-                  m_graph.streetSet().at(nodeId * dimension + nextNodeId)->length()) {
-            // std::cout << "minDistance: " << minDistance << " distance: " << distance
-            //           << " streetLength: " << streetLength << '\n';
-            // std::cout << "Inserting " << i << ';' << node.first << '\n';
-            path.insert(nodeId, nextNodeId, true);
-          }
-        }
-        itinerary->setPath(path);
-        // for (auto i{0}; i < dimension; ++i) {
-        //   std::cout << path.getRow(i).size() << ' ';
-        // }
-        // std::cout << '\n';
-      }
+      threads.emplace_back(
+          std::thread([this, &itinerary] { this->m_updatePath(itinerary); }));
+    }
+    for (auto& thread : threads) {
+      thread.join();
     }
   }
 
@@ -700,18 +714,13 @@ namespace dsm {
                                                         double threshold) {
     for (const auto& [nodeId, node] : m_graph.nodeSet()) {
       if (!node->isTrafficLight()) {
-        //std::cout << '\t' << " -> Non è un semaforo" << '\n';
         continue;
       }
       auto& tl = dynamic_cast<TrafficLight<Id, Size, Delay>&>(*node);
       if (!tl.delay().has_value()) {
-        //std::cout << '\t' << " -> Il delay non ha valore" << '\n';
         continue;
       }
-      //std::cout << '\t' << '\t' << '\t' << "NODO " << nodeId << '\n';
       auto [greenTime, redTime] = tl.delay().value();
-      //std::cout << '\t' << " -> greenTime: " << static_cast<unsigned int>(greenTime) << '\n';
-      //std::cout << '\t' << " -> redTime: " << static_cast<unsigned int>(redTime) << '\n';
       const auto cycleTime = greenTime + redTime;
       const auto& streetPriorities = tl.streetPriorities();
       Size greenSum{0}, greenQueue{0};
@@ -719,96 +728,85 @@ namespace dsm {
       for (const auto& [streetId, _] : m_graph.adjMatrix().getCol(nodeId, true)) {
         if (streetPriorities.contains(streetId)) {
           greenSum += m_streetTails[streetId];
-          greenQueue += m_graph.streetSet()[streetId]->queue().size(); 
+          greenQueue += m_graph.streetSet()[streetId]->queue().size();
         } else {
           redSum += m_streetTails[streetId];
           redQueue += m_graph.streetSet()[streetId]->queue().size();
         }
       }
-      //std::cout << '\t' << " -> greenSum: " << static_cast<unsigned int>(greenSum) << '\n';
-      //std::cout << '\t' << " -> greenQueue: " << static_cast<unsigned int>(greenQueue) << '\n';
-      //std::cout << '\t' << " -> redSum: " << static_cast<unsigned int>(redSum) << '\n';
-      //std::cout << '\t' << " -> redQueue: " << static_cast<unsigned int>(redQueue) << '\n';
       const Delay delta =
           std::floor(std::abs(static_cast<int>(greenQueue - redQueue)) / percentage);
-          //std::cout << '\t' << " -> delta: " << static_cast<unsigned int>(delta) << '\n';
       if (delta == 0) {
-        //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
         continue;
       }
       const Size smallest = std::min(greenSum, redSum);
       if (std::abs(static_cast<int>(greenSum - redSum)) < threshold * smallest) {
-        //std::cout << '\t' << " -> MINORE della threshold" << '\n';
         tl.setDelay(std::floor(cycleTime / 2));
-        //auto [greenTime1, redTime1] = tl.delay().value();
-        //std::cout << '\t' << " -> greenTime: " << static_cast<unsigned int>(greenTime1) << '\n';
-        //std::cout << '\t' << " -> redTime: " << static_cast<unsigned int>(redTime1) << '\n';
-        //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
         continue;
       }
-      //std::cout << '\t' << " -> MAGGIORE della threshold" << '\n';
-      // se non vale la condizione sopra (vale quella col >)
-      // cosa devo fare:
-      //    - controllo che le le strade che danno sul nodo abbiano una densità minore della densità media (o minore
-      //      della densità media + percentuale): voglio evitare di essere dentro al cluster. Voglio essere fuori o (al 
-      //      più) sul bordo
-      //    - se è minore: faccio fare all'algoritmo quello che vuole fare
-      //    - se NON lo è: non gli faccio fare nulla (non modifico nulla)
+      // If the difference is not less than the threshold
+      //    - Check that the incoming streets have a density less than the mean one (eventually + tolerance): I want to avoid being into the cluster, better to be out or on the border
+      //    - If the previous check fails, do nothing
       auto meanDensity_whole = streetMeanDensity().mean;  // Measurement<double>
       double densitySum{};
       int i{};
-      // cerco su tutto il set di strade quelle che sono USCENTI rispetto a quel nodo e mi salvo il loro id
+      // Store the ids of outgoing streets
       for (const auto& [streetId, street] : m_graph.streetSet()) {
         if (street->nodePair().first == nodeId) {
-        i++;
-        auto density = street->density();
-        densitySum += density;
+          i++;
+          auto density = street->density();
+          densitySum += density;
         }
       }
-      // fai la media delle quattro e confronti la media
+      // Take the mean density of the outgoing streets
       auto meanDensity_streets = densitySum / i;
-      //std::cout << '\t' << " -> Densità media del network: " << std::setprecision(7) << meanDensity_whole << '\n';
-      //std::cout << '\t' << " -> Densità media delle 4 strade uscenti: " << std::setprecision(7) << meanDensity_streets << '\n';
-      auto ratio = meanDensity_whole/meanDensity_streets;
-      auto dyn_thresh = std::tanh(ratio) * 3/10; // il 3/10 è lì perché è il massimo bordo che voglio includere
+      //std::cout << '\t' << " -> Mean network density: " << std::setprecision(7) << meanDensity_whole << '\n';
+      //std::cout << '\t' << " -> Mean density of 4 outgoing streets: " << std::setprecision(7) << meanDensity_streets << '\n';
+      auto ratio = meanDensity_whole / meanDensity_streets;
+      auto dyn_thresh =
+          std::tanh(ratio) * 3 /
+          10;  // il 3/10 è lì perché è il massimo bordo che voglio includere
       //std::cout << '\t' << " -> Parametro ratio: " << std::setprecision(7) << ratio << '\n';
       //std::cout << '\t' << " -> Parametro dyn_thresh: " << std::setprecision(7) << dyn_thresh << '\n';
-      if (meanDensity_whole + (dyn_thresh) * meanDensity_whole > meanDensity_streets) {
-        //std::cout << '\t' << " -> Sono sul BORDO del cluster" << '\n';
-        if(meanDensity_whole > meanDensity_streets) {
-          //std::cout << '\t' << " -> MINORE della densità massima" << '\n';
-          if(!(redTime > greenTime) && (redSum > greenSum) && (greenTime > delta)) {
-              greenTime -= delta; 
-              redTime += delta;
-              tl.setDelay(std::make_pair(greenTime, redTime));
-            } else if (!(greenTime > redTime) && (greenSum > redSum) && (redTime > delta)) {
-              greenTime += delta;
-              redTime -= delta; 
-              tl.setDelay(std::make_pair(greenTime, redTime));
-            } else { 
-              //std::cout << '\t' << " -> NON sono entrato negli if precedenti" << '\n';
-              tl.setDelay(std::make_pair(greenTime, redTime)); 
-            }
-            //std::cout << '\t' << " -> greenTime: " << static_cast<unsigned int>(greenTime) << '\n';
-            //std::cout << '\t' << " -> redTime: " << static_cast<unsigned int>(redTime) << '\n';
-            //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
+      if (meanDensity_whole + (dyn_thresh)*meanDensity_whole > meanDensity_streets) {
+        //std::cout << '\t' << " -> I'm on the cluster's border" << '\n';
+        if (meanDensity_whole > meanDensity_streets) {
+          //std::cout << '\t' << " -> LESS than max density" << '\n';
+          if (!(redTime > greenTime) && (redSum > greenSum) && (greenTime > delta)) {
+            greenTime -= delta;
+            redTime += delta;
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else if (!(greenTime > redTime) && (greenSum > redSum) && (redTime > delta)) {
+            greenTime += delta;
+            redTime -= delta;
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          } else {
+            //std::cout << '\t' << " -> NOT entered into previous ifs" << '\n';
+            tl.setDelay(std::make_pair(greenTime, redTime));
+          }
+          //std::cout << '\t' << " -> greenTime: " << static_cast<unsigned int>(greenTime) << '\n';
+          //std::cout << '\t' << " -> redTime: " << static_cast<unsigned int>(redTime) << '\n';
+          //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
         } else {
-          //std::cout << '\t' << " -> MAGGIORE della densità massima" << '\n';
-          if(!(redTime > greenTime) && (redSum > greenSum) && (greenTime > ratio * delta)) {
+          //std::cout << '\t' << " -> GREATER than max density" << '\n';
+          if (!(redTime > greenTime) && (redSum > greenSum) &&
+              (greenTime > ratio * delta)) {
             greenTime -= dyn_thresh * delta;  //
             redTime += delta;
             tl.setDelay(std::make_pair(greenTime, redTime));
-          } else if (!(greenTime > redTime) && (greenSum > redSum) && (redTime > ratio * delta)) {
+          } else if (!(greenTime > redTime) && (greenSum > redSum) &&
+                     (redTime > ratio * delta)) {
             greenTime += delta;
-            redTime -= dyn_thresh * delta;      //
+            redTime -= dyn_thresh * delta;  //
             tl.setDelay(std::make_pair(greenTime, redTime));
           } else if (!(redTime > greenTime) && (redSum < greenSum) && (redTime > delta)) {
-            greenTime += dyn_thresh * delta;    //
+            greenTime += dyn_thresh * delta;  //
             redTime -= delta;
             tl.setDelay(std::make_pair(greenTime, redTime));
-          } else if (!(redTime < greenTime) && (redSum > greenSum) && (greenTime > delta)) {
+          } else if (!(redTime < greenTime) && (redSum > greenSum) &&
+                     (greenTime > delta)) {
             greenTime -= delta;
-            redTime += dyn_thresh * delta;      //
+            redTime += dyn_thresh * delta;  //
             tl.setDelay(std::make_pair(greenTime, redTime));
           } else {
             //std::cout << '\t' << " -> NON sono entrato negli if precedenti" << '\n';
@@ -819,23 +817,27 @@ namespace dsm {
           //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
         }
       } else {
-        //std::cout << '\t' << " -> Sono DENTRO il cluster" << '\n';
+        //std::cout << '\t' << " -> I'm INTO the cluster" << '\n';
         //std::cout << '\t' << " -> modTime: " << tl.modTime() << '\n';
-      } 
+      }
     }
     for (auto& [id, element] : m_streetTails) {
       element = 0;
     }
   }
 
-
   template <typename Id, typename Size, typename Delay>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::addAgent(const Agent<Id, Size, Delay>& agent) {
+    if (this->m_agents.size() + 1 > this->m_graph.maxCapacity()) {
+      throw std::overflow_error(buildLog(
+          std::format("Graph its already holding the max possible number of agents ({})",
+                      this->m_graph.maxCapacity())));
+    }
     if (this->m_agents.contains(agent.id())) {
       throw std::invalid_argument(
-          buildLog("Agent " + std::to_string(agent.id()) + " already exists."));
+          buildLog(std::format("Agent with id {} already exists.", agent.id())));
     }
     this->m_agents.emplace(agent.id(), std::make_unique<Agent<Id, Size, Delay>>(agent));
   }
@@ -843,9 +845,14 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::addAgent(std::unique_ptr<Agent<Id, Size, Delay>> agent) {
+    if (this->m_agents.size() + 1 > this->m_graph.maxCapacity()) {
+      throw std::overflow_error(buildLog(
+          std::format("Graph its already holding the max possible number of agents ({})",
+                      this->m_graph.maxCapacity())));
+    }
     if (this->m_agents.contains(agent->id())) {
       throw std::invalid_argument(
-          buildLog("Agent " + std::to_string(agent->id()) + " already exists."));
+          buildLog(std::format("Agent with id {} already exists.", agent->id())));
     }
     this->m_agents.emplace(agent->id(), std::move(agent));
   }
@@ -855,16 +862,21 @@ namespace dsm {
   void Dynamics<Id, Size, Delay>::addAgents(Id itineraryId,
                                             Size nAgents,
                                             std::optional<Id> srcNodeId) {
+    if (this->m_agents.size() + nAgents > this->m_graph.maxCapacity()) {
+      throw std::overflow_error(buildLog(
+          std::format("Graph its already holding the max possible number of agents ({})",
+                      this->m_graph.maxCapacity())));
+    }
     auto itineraryIt{m_itineraries.find(itineraryId)};
     if (itineraryIt == m_itineraries.end()) {
       throw std::invalid_argument(
-          buildLog("Itinerary " + std::to_string(itineraryId) + " not found"));
+          buildLog(std::format("Itinerary with id {} not found", itineraryId)));
     }
     Size agentId{0};
     if (!this->m_agents.empty()) {
       agentId = this->m_agents.rbegin()->first + 1;
     }
-    for (auto i{0}; i < nAgents; ++i, ++agentId) {
+    for (Size i{0}; i < nAgents; ++i, ++agentId) {
       this->addAgent(Agent<Id, Size, Delay>{agentId, itineraryId});
       if (srcNodeId.has_value()) {
         this->m_agents[agentId]->setSourceNodeId(srcNodeId.value());
@@ -893,6 +905,11 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::addAgents(std::span<Agent<Id, Size, Delay>> agents) {
+    if (this->m_agents.size() + agents.size() > this->m_graph.maxCapacity()) {
+      throw std::overflow_error(buildLog(
+          std::format("Graph its already holding the max possible number of agents ({})",
+                      this->m_graph.maxCapacity())));
+    }
     std::ranges::for_each(agents, [this](const auto& agent) -> void {
       this->m_agents.push_back(std::make_unique(agent));
     });
@@ -903,9 +920,14 @@ namespace dsm {
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::addAgentsUniformly(Size nAgents,
                                                      std::optional<Id> itineraryId) {
+    if (this->m_agents.size() + nAgents > this->m_graph.maxCapacity()) {
+      throw std::overflow_error(buildLog(
+          std::format("Graph its already holding the max possible number of agents ({})",
+                      this->m_graph.maxCapacity())));
+    }
     if (this->m_itineraries.empty()) {
       // TODO: make this possible for random agents
-      throw std::runtime_error(
+      throw std::invalid_argument(
           buildLog("It is not possible to add random agents without itineraries."));
     }
     const bool randomItinerary{!itineraryId.has_value()};
@@ -951,7 +973,7 @@ namespace dsm {
     auto agentIt{m_agents.find(agentId)};
     if (agentIt == m_agents.end()) {
       throw std::invalid_argument(
-          buildLog("Agent " + std::to_string(agentId) + " not found."));
+          buildLog(std::format("Agent with id {} not found.", agentId)));
     }
     m_agents.erase(agentId);
   }
@@ -1344,5 +1366,3 @@ namespace dsm {
   void SecondOrderDynamics<Id, Size>::setSpeed() {}
 
 };  // namespace dsm
-
-#endif

From 4af2c1c818ddbc0e267707fe86d40f6bcb279f78 Mon Sep 17 00:00:00 2001
From: Gregorio Berselli <gregorio_2000@hotmail.it>
Date: Thu, 3 Oct 2024 22:21:44 +0200
Subject: [PATCH 04/19] Merge Node.hpp with main branch

---
 src/dsm/headers/Node.hpp | 46 +++++++++++++++++++++++-----------------
 1 file changed, 27 insertions(+), 19 deletions(-)

diff --git a/src/dsm/headers/Node.hpp b/src/dsm/headers/Node.hpp
index 43e78da80..cb1c17b7e 100644
--- a/src/dsm/headers/Node.hpp
+++ b/src/dsm/headers/Node.hpp
@@ -9,8 +9,7 @@
 ///               - TrafficLight: represents a traffic light intersection node
 ///             - Roundabout: represents a roundabout node with a queue of agents
 
-#ifndef Node_hpp
-#define Node_hpp
+#pragma once
 
 #include <functional>
 #include <utility>
@@ -18,6 +17,7 @@
 #include <optional>
 #include <set>
 #include <map>
+#include <format>
 
 #include "../utility/Logger.hpp"
 #include "../utility/queue.hpp"
@@ -151,8 +151,10 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   void Node<Id, Size>::setCapacity(Size capacity) {
     if (capacity < m_agents.size()) {
-      throw std::runtime_error(
-          buildLog("Node capacity is smaller than the current queue size"));
+      throw std::runtime_error(buildLog(
+          std::format("Node capacity ({}) is smaller than the current queue size ({}).",
+                      capacity,
+                      m_agents.size())));
     }
     NodeConcept<Id, Size>::setCapacity(capacity);
   }
@@ -161,11 +163,12 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   void Node<Id, Size>::addAgent(double angle, Id agentId) {
     if (m_agents.size() == this->m_capacity) {
-      throw std::runtime_error(buildLog("Node is full"));
+      throw std::runtime_error(buildLog("Node is full."));
     }
     for (auto const [angle, id] : m_agents) {
       if (id == agentId) {
-        throw std::runtime_error(buildLog("Agent is already on the node."));
+        throw std::runtime_error(
+            buildLog(std::format("Agent with id {} is already on the node.", agentId)));
       }
     }
     auto iAngle{static_cast<int16_t>(angle * 100)};
@@ -177,11 +180,12 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   void Node<Id, Size>::addAgent(Id agentId) {
     if (m_agents.size() == this->m_capacity) {
-      throw std::runtime_error(buildLog("Node is full"));
+      throw std::runtime_error(buildLog("Node is full."));
     }
     for (auto const [angle, id] : m_agents) {
       if (id == agentId) {
-        throw std::runtime_error(buildLog("Agent is already on the node."));
+        throw std::runtime_error(
+            buildLog(std::format("Agent with id {} is already on the node.", id)));
       }
     }
     int lastKey{0};
@@ -201,7 +205,8 @@ namespace dsm {
         return;
       }
     }
-    throw std::runtime_error(buildLog("Agent is not on the node"));
+    throw std::runtime_error(
+        buildLog(std::format("Agent with id {} is not on the node", agentId)));
   }
 
   template <typename Id, typename Size>
@@ -308,9 +313,11 @@ namespace dsm {
       }
     }
     m_delay = std::make_pair(delay, delay);
-    if (delay < 5) { 
-      m_modTime = 2; 
-    } else { m_modTime = 0; }
+    if (delay < 5) {
+      m_modTime = 2;
+    } else {
+      m_modTime = 0;
+    }
   }
   template <typename Id, typename Size, typename Delay>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
@@ -330,8 +337,10 @@ namespace dsm {
       m_modTime = 2;
     } else if (std::abs(delay.first - delay.second) < 10) {
       m_modTime = 0;
-    } else if ((std::abs(delay.first - delay.second) > 10 || std::abs(delay.first - delay.second) == 10) 
-          && (std::abs(delay.first - delay.second) < 40 || std::abs(delay.first - delay.second) == 40)) {
+    } else if ((std::abs(delay.first - delay.second) > 10 ||
+                std::abs(delay.first - delay.second) == 10) &&
+               (std::abs(delay.first - delay.second) < 40 ||
+                std::abs(delay.first - delay.second) == 40)) {
       m_modTime = 1;
     } else if (std::abs(delay.first - delay.second) > 40) {
       m_modTime = 2;
@@ -457,11 +466,12 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   void Roundabout<Id, Size>::enqueue(Id agentId) {
     if (m_agents.size() == this->m_capacity) {
-      throw std::runtime_error(buildLog("Roundabout is full"));
+      throw std::runtime_error(buildLog("Roundabout is full."));
     }
     for (const auto id : m_agents) {
       if (id == agentId) {
-        throw std::runtime_error(buildLog("Agent is already on the roundabout."));
+        throw std::runtime_error(buildLog(
+            std::format("Agent with id {} is already on the roundabout.", agentId)));
       }
     }
     m_agents.push(agentId);
@@ -470,12 +480,10 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   Id Roundabout<Id, Size>::dequeue() {
     if (m_agents.empty()) {
-      throw std::runtime_error(buildLog("Roundabout is empty"));
+      throw std::runtime_error(buildLog("Roundabout is empty."));
     }
     Id agentId{m_agents.front()};
     m_agents.pop();
     return agentId;
   }
 };  // namespace dsm
-
-#endif

From 6cc1e8d5ecec2fff89f58d22bde8c79c7bfa98c9 Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Fri, 4 Oct 2024 11:22:44 +0200
Subject: [PATCH 05/19] Fix stalingrado example build

---
 examples/stalingrado.cpp | 25 +++++++++++++------------
 1 file changed, 13 insertions(+), 12 deletions(-)

diff --git a/examples/stalingrado.cpp b/examples/stalingrado.cpp
index b09e10b0c..76efc2213 100644
--- a/examples/stalingrado.cpp
+++ b/examples/stalingrado.cpp
@@ -14,16 +14,17 @@
 #include <thread>
 #include <atomic>
 
-using unit = uint32_t;
+std::atomic<unsigned int> progress{0};
 
-std::atomic<unit> progress{0};
+using Unit = unsigned int;
+using Delay = uint8_t;
 
-using Graph = dsm::Graph<unit, unit>;
-using Itinerary = dsm::Itinerary<unit>;
-using Dynamics = dsm::FirstOrderDynamics<unit, unit, unit>;
-using TrafficLight = dsm::TrafficLight<unit, unit, unit>;
-using Street = dsm::Street<unit, unit>;
-using SpireStreet = dsm::SpireStreet<unit, unit>;
+using Graph = dsm::Graph<Unit, Unit>;
+using Itinerary = dsm::Itinerary<Unit>;
+using Dynamics = dsm::FirstOrderDynamics<Unit, Unit, Delay>;
+using Street = dsm::Street<Unit, Unit>;
+using SpireStreet = dsm::SpireStreet<Unit, Unit>;
+using TrafficLight = dsm::TrafficLight<Unit, Unit, Delay>;
 
 void printLoadingBar(int const i, int const n) {
   std::cout << "Loading: " << std::setprecision(2) << std::fixed << (i * 100. / n) << "%"
@@ -34,15 +35,15 @@ void printLoadingBar(int const i, int const n) {
 int main() {
   // Import input data
   std::ifstream ifs{"./data/stalingrado_input.txt"};
-  unit timeUnit{0};
+  Unit timeUnit{0};
   ifs >> timeUnit;
-  std::vector<unit> vehiclesToInsert{};
+  std::vector<Unit> vehiclesToInsert{};
   while (!ifs.eof()) {
-    unit vehicleId{0};
+    Unit vehicleId{0};
     ifs >> vehicleId;
     vehiclesToInsert.push_back(vehicleId);
   }
-  const auto MAX_TIME{static_cast<unit>(timeUnit * vehiclesToInsert.size())};
+  const auto MAX_TIME{static_cast<Unit>(timeUnit * vehiclesToInsert.size())};
 
   // Create the graph
   TrafficLight tl1{1}, tl2{2}, tl3{3}, tl4{4};

From 11e51a44173677dc09ccec3d404e7f35b0b3fc2d Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Fri, 4 Oct 2024 12:53:02 +0200
Subject: [PATCH 06/19] Exclude more paths in examples

---
 .gitignore | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.gitignore b/.gitignore
index 3de1a023b..93193e9bb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -23,8 +23,8 @@
 temp*
 
 # Examples generated files
-examples/sctl/*
-examples/scrb/*
+examples/sctl*
+examples/scrb*
 examples/*.csv
 
 # Utils generated folders

From 9cb79263676d309c2797f180d55789c6eb3c5bf7 Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Fri, 4 Oct 2024 12:53:33 +0200
Subject: [PATCH 07/19] Remove weird line

---
 utils/plotter.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/utils/plotter.py b/utils/plotter.py
index d7a6e9e9b..4981c07f7 100644
--- a/utils/plotter.py
+++ b/utils/plotter.py
@@ -529,7 +529,6 @@ def adjust_dataframe(_df):
 
         df_std = pd.concat(df_array)
         df_std = df_std.groupby(df_std.index).std()
-        df_std.to_csv("aaaaaaaaaaaaaaaaaaa.csv")
         # df_std = df_std.interpolate(method='linear', limit_direction='both')
         df_std = df_std.abs()
 

From 8a62631801a015c807fc0f3b26a17cc4a8d546f7 Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Fri, 4 Oct 2024 14:37:24 +0200
Subject: [PATCH 08/19] Add m_dataUpdatePeriod var (previously hard-coded)

---
 examples/slow_charge_tl.cpp  | 26 +++++++++++++-------------
 src/dsm/headers/Dynamics.hpp | 15 ++++++++++++---
 2 files changed, 25 insertions(+), 16 deletions(-)

diff --git a/examples/slow_charge_tl.cpp b/examples/slow_charge_tl.cpp
index 80065afa4..f8d856603 100644
--- a/examples/slow_charge_tl.cpp
+++ b/examples/slow_charge_tl.cpp
@@ -52,18 +52,9 @@ int main(int argc, char** argv) {
 
   const int SEED = std::stoi(argv[1]);  // seed for random number generator
   const double ERROR_PROBABILITY{std::stod(argv[2])};
-  const std::string BASE_OUT_FOLDER{argv[3]};
+  std::string BASE_OUT_FOLDER{argv[3]};
   const bool OPTIMIZE{std::string(argv[4]) != std::string("0")};
-
-  std::cout << "-------------------------------------------------\n";
-  std::cout << "Input parameters:\n";
-  std::cout << "Seed: " << SEED << '\n';
-  std::cout << "Error probability: " << ERROR_PROBABILITY << '\n';
-  std::cout << "Base output folder: " << BASE_OUT_FOLDER << '\n';
-  if (OPTIMIZE) {
-    std::cout << "Traffic light optimization enabled.\n";
-  }
-  std::cout << "-------------------------------------------------\n";
+  BASE_OUT_FOLDER += OPTIMIZE ? "_op/" : "/";
 
   const std::string IN_MATRIX{"./data/matrix.dat"};       // input matrix file
   const std::string IN_COORDS{"./data/coordinates.dsm"};  // input coords file
@@ -71,10 +62,17 @@ 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
+
+  std::cout << "-------------------------------------------------\n";
+  std::cout << "Input parameters:\n";
+  std::cout << "Seed: " << SEED << '\n';
+  std::cout << "Error probability: " << ERROR_PROBABILITY << '\n';
+  std::cout << "Base output folder: " << BASE_OUT_FOLDER << '\n';
   if (OPTIMIZE) {
-    OUT_FOLDER += "_op/";
+    std::cout << "Traffic light optimization ENABLED.\n";
   }
-  const auto MAX_TIME{static_cast<unsigned int>(1e6)};  // maximum time of simulation
+  std::cout << "-------------------------------------------------\n";
 
   // Clear output folder or create it if it doesn't exist
   if (!fs::exists(BASE_OUT_FOLDER)) {
@@ -228,6 +226,8 @@ int main(int argc, char** argv) {
   // dynamics.setForcePriorities(false);
   dynamics.setSpeedFluctuationSTD(0.1);
   dynamics.setMinSpeedRateo(0.95);
+  if (OPTIMIZE)
+    dynamics.setDataUpdatePeriod(30);  // Store data every 30 time steps
   dynamics.updatePaths();
 
   const auto TM = dynamics.turnMapping();
diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp
index c952ae71a..aa486ddb3 100644
--- a/src/dsm/headers/Dynamics.hpp
+++ b/src/dsm/headers/Dynamics.hpp
@@ -84,6 +84,7 @@ namespace dsm {
     std::vector<unsigned int> m_travelTimes;
     std::unordered_map<Id, Id> m_agentNextStreetId;
     bool m_forcePriorities;
+    std::optional<Delay> m_dataUpdatePeriod;
     std::unordered_map<Id, std::array<unsigned long long, 4>> m_turnCounts;
     std::unordered_map<Id, std::array<long, 4>> m_turnMapping;
     std::unordered_map<Id, unsigned long long> m_streetTails;
@@ -148,6 +149,12 @@ namespace dsm {
     /// @param forcePriorities The flag
     /// @details If true, if an agent cannot move to the next street, the whole node is skipped
     void setForcePriorities(bool forcePriorities) { m_forcePriorities = forcePriorities; }
+    /// @brief Set the data update period.
+    /// @param dataUpdatePeriod Delay, The period
+    /// @details Some data, i.e. the street queue lengths, are stored only after a fixed amount of time which is represented by this variable.
+    void setDataUpdatePeriod(Delay dataUpdatePeriod) {
+      m_dataUpdatePeriod = dataUpdatePeriod;
+    }
 
     /// @brief Update the paths of the itineraries based on the actual travel times
     virtual void updatePaths();
@@ -416,9 +423,11 @@ namespace dsm {
       if (m_agents[agentId]->delay() > 0) {
         continue;
       }
-      if (m_time % 30 == 0) {
-        //m_streetTails[streetId] += street->queue().size();
-        m_streetTails[streetId] += street->waitingAgents().size();
+      if (m_dataUpdatePeriod.has_value()) {
+        if (m_time % m_dataUpdatePeriod.value() == 0) {
+          //m_streetTails[streetId] += street->queue().size();
+          m_streetTails[streetId] += street->waitingAgents().size();
+        }
       }
       m_agents[agentId]->setSpeed(0.);
       const auto& destinationNode{this->m_graph.nodeSet()[street->nodePair().second]};

From 75b025c4b86f6a01d46ff8d52e020cc13d8df025 Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Fri, 4 Oct 2024 14:59:01 +0200
Subject: [PATCH 09/19] Add densityTolerance parameter to
 optimizeTrafficLights()

---
 examples/slow_charge_tl.cpp  |  2 +-
 src/dsm/headers/Dynamics.hpp | 25 ++++++++++++++++++++-----
 2 files changed, 21 insertions(+), 6 deletions(-)

diff --git a/examples/slow_charge_tl.cpp b/examples/slow_charge_tl.cpp
index f8d856603..63ae0cd77 100644
--- a/examples/slow_charge_tl.cpp
+++ b/examples/slow_charge_tl.cpp
@@ -310,7 +310,7 @@ int main(int argc, char** argv) {
     }
     dynamics.evolve(false);
     if (OPTIMIZE && (dynamics.time() % 420 == 0)) {
-      dynamics.optimizeTrafficLights(std::floor(420. / 60), 0.15);
+      dynamics.optimizeTrafficLights(std::floor(420. / 60), 0.15, 3. / 10);
     }
     if (dynamics.time() % 2400 == 0 && nAgents > 0) {
       // auto meanDelta = std::accumulate(deltas.begin(), deltas.end(), 0) /
diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp
index aa486ddb3..607e62346 100644
--- a/src/dsm/headers/Dynamics.hpp
+++ b/src/dsm/headers/Dynamics.hpp
@@ -171,9 +171,12 @@ namespace dsm {
     /// @brief Optimize the traffic lights by changing the green and red times
     /// @param percentage double, The percentage of the TOTAL cycle time to add or subtract to the green time
     /// @param threshold double, The percentage of the mean capacity of the streets used as threshold for the delta between the two tails.
+    /// @param densityTolerance double, The algorithm will consider all streets with density up to densityTolerance*meanDensity
     /// @details The function cycles over the traffic lights and, if the difference between the two tails is greater than
     ///   the threshold multiplied by the mean capacity of the streets, it changes the green and red times of the traffic light, keeping the total cycle time constant.
-    void optimizeTrafficLights(double percentage, double threshold = 0.);
+    void optimizeTrafficLights(double percentage,
+                               double threshold = 0.,
+                               double densityTolerance = 0.);
 
     /// @brief Get the graph
     /// @return const Graph<Id, Size>&, The graph
@@ -720,7 +723,20 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
   void Dynamics<Id, Size, Delay>::optimizeTrafficLights(double percentage,
-                                                        double threshold) {
+                                                        double threshold,
+                                                        double densityTolerance) {
+    if (threshold < 0 || threshold > 1) {
+      throw std::invalid_argument(
+          buildLog(std::format("The threshold parameter is a percentage and must be "
+                               "bounded between 0-1. Inserted value: {}",
+                               threshold)));
+    }
+    if (densityTolerance < 0 || densityTolerance > 1) {
+      throw std::invalid_argument(buildLog(
+          std::format("The densityTolerance parameter is a percentage and must be "
+                      "bounded between 0-1. Inserted value: {}",
+                      densityTolerance)));
+    }
     for (const auto& [nodeId, node] : m_graph.nodeSet()) {
       if (!node->isTrafficLight()) {
         continue;
@@ -772,9 +788,8 @@ namespace dsm {
       //std::cout << '\t' << " -> Mean network density: " << std::setprecision(7) << meanDensity_whole << '\n';
       //std::cout << '\t' << " -> Mean density of 4 outgoing streets: " << std::setprecision(7) << meanDensity_streets << '\n';
       auto ratio = meanDensity_whole / meanDensity_streets;
-      auto dyn_thresh =
-          std::tanh(ratio) * 3 /
-          10;  // il 3/10 è lì perché è il massimo bordo che voglio includere
+      // densityTolerance represents the max border we want to consider
+      auto dyn_thresh = std::tanh(ratio) * densityTolerance;
       //std::cout << '\t' << " -> Parametro ratio: " << std::setprecision(7) << ratio << '\n';
       //std::cout << '\t' << " -> Parametro dyn_thresh: " << std::setprecision(7) << dyn_thresh << '\n';
       if (meanDensity_whole + (dyn_thresh)*meanDensity_whole > meanDensity_streets) {

From 45dd3de11a45797b05d41205025e65118e05d74d Mon Sep 17 00:00:00 2001
From: vero-dav-vero <100243585+vero-dav-vero@users.noreply.github.com>
Date: Fri, 4 Oct 2024 18:34:02 +0200
Subject: [PATCH 10/19] Add slow_charge_tl_vero.cpp

---
 examples/slow_charge_tl_vero.cpp | 462 +++++++++++++++++++++++++++++++
 1 file changed, 462 insertions(+)
 create mode 100644 examples/slow_charge_tl_vero.cpp

diff --git a/examples/slow_charge_tl_vero.cpp b/examples/slow_charge_tl_vero.cpp
new file mode 100644
index 000000000..6c3be477c
--- /dev/null
+++ b/examples/slow_charge_tl_vero.cpp
@@ -0,0 +1,462 @@
+// TODO modificare header
+
+#include "src/dsm/dsm.hpp"
+//#include "src/dsm/dsm-copy.hpp"
+#include <array>
+#include <cmath>
+#include <cstdint>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <numeric>
+#include <random>
+#include <set>
+#include <string>
+#include <filesystem>
+namespace fs = std::filesystem;
+
+#include <thread>
+#include <atomic>
+
+std::atomic_int progress{0};
+uint nAgents{315}; // 315 for error probability 0.3, 450 for error probability 0.05
+
+// uncomment these lines to print densities, flows and speeds
+#define PRINT_DENSITIES
+// #define PRINT_FLOWS
+#define PRINT_OUT_SPIRES
+// #define PRINT_SPEEDS
+// #define PRINT_TP
+// TODO questo è il branch con il nuovo ALGORITMO
+//#define OPTIMIZE
+
+// Compatible with dsm 1.2.1
+constexpr double ERROR_PROBABILITY{0.3}; // seed for random number generator
+constexpr int SEED{69}; // seed for random number generator
+const std::string IN_MATRIX{"./matrix.dat"}; // input matrix file
+const std::string IN_COORDS{"./coordinates.dsm"}; // input coords file
+// TODO nuova cartella output
+//const std::string OUT_FOLDER{"./output-tesi/greg-op_seed" + std::to_string(SEED) + "_" + std::to_string(nAgents) + "_gaussiana_m100_std20/"}; // output folder
+const std::string OUT_FOLDER{"./output-tesi/no-op_seed" + std::to_string(SEED) + "_" + std::to_string(nAgents) + "_gaussiana_m60_std10/"}; // output folder
+//const std::string OUT_FOLDER{"./output-tesi/new-op_dyn-vrs-copy2-oi2_seed" + std::to_string(SEED) + "_" + std::to_string(nAgents) + "_gaussiana_m100_std20/"}; // output folder
+constexpr auto MAX_TIME{
+    static_cast<unsigned int>(1e6)}; // maximum time of simulation
+
+using Unit = unsigned int;
+using Delay = uint8_t;
+
+using Graph = dsm::Graph<Unit, Unit>;
+using Itinerary = dsm::Itinerary<Unit>;
+using Dynamics = dsm::FirstOrderDynamics<Unit, Unit, Delay>;
+using Street = dsm::Street<Unit, Unit>;
+using SpireStreet = dsm::SpireStreet<Unit, Unit>;
+using TrafficLight = dsm::TrafficLight<Unit, Unit, Delay>;
+
+void printLoadingBar(int const i, int const n) {
+  std::cout << "Loading: " << std::setprecision(2) << std::fixed
+            << (i * 100. / n) << "%" << '\r';
+  std::cout.flush();
+}
+
+int main() {
+  // Clear output folder or create it if it doesn't exist
+  if (fs::exists(OUT_FOLDER)) {
+    fs::remove_all(OUT_FOLDER);
+  }
+  fs::create_directory(OUT_FOLDER);
+  // Starting
+  std::cout << "Using dsm version: " << dsm::version() << '\n';
+  Graph graph{};
+  std::cout << "Importing matrix.dat...\n";
+  graph.importMatrix(IN_MATRIX, false);
+  graph.importCoordinates(IN_COORDS);
+  graph.buildAdj();
+  const auto dv = graph.adjMatrix().getDegreeVector();
+
+  // graph.addStreet(Street(100002, std::make_pair(0, 108)));
+  // graph.addStreet(Street(100003, std::make_pair(108, 0)));
+  // graph.addStreet(Street(100004, std::make_pair(0, 11)));
+  // graph.addStreet(Street(100005, std::make_pair(11, 0)));
+  // graph.addStreet(Street(100006, std::make_pair(1, 109)));
+  // graph.addStreet(Street(100007, std::make_pair(109, 1)));
+  // graph.addStreet(Street(100008, std::make_pair(2, 110)));
+  // graph.addStreet(Street(100009, std::make_pair(110, 2)));
+  // graph.addStreet(Street(100010, std::make_pair(3, 111)));
+  // graph.addStreet(Street(100011, std::make_pair(111, 3)));
+  // graph.addStreet(Street(100012, std::make_pair(4, 112)));
+  // graph.addStreet(Street(100013, std::make_pair(112, 4)));
+  // graph.addStreet(Street(100014, std::make_pair(5, 113)));
+  // graph.addStreet(Street(100015, std::make_pair(113, 5)));
+  // graph.addStreet(Street(100016, std::make_pair(6, 114)));
+  // graph.addStreet(Street(100017, std::make_pair(114, 6)));
+  // graph.addStreet(Street(100018, std::make_pair(7, 115)));
+  // graph.addStreet(Street(100019, std::make_pair(115, 7)));
+  // graph.addStreet(Street(100020, std::make_pair(8, 116)));
+  // graph.addStreet(Street(100021, std::make_pair(116, 8)));
+  // graph.addStreet(Street(100022, std::make_pair(9, 117)));
+  // graph.addStreet(Street(100023, std::make_pair(117, 9)));
+  // graph.addStreet(Street(100024, std::make_pair(10, 118)));
+  // graph.addStreet(Street(100025, std::make_pair(118, 10)));
+  // graph.addStreet(Street(100026, std::make_pair(11, 119)));
+  // graph.addStreet(Street(100027, std::make_pair(119, 11)));
+  // graph.addStreet(Street(100028, std::make_pair(12, 23)));
+  // graph.addStreet(Street(100029, std::make_pair(23, 12)));
+  // graph.addStreet(Street(100030, std::make_pair(24, 35)));
+  // graph.addStreet(Street(100069, std::make_pair(35, 24)));
+  // graph.addStreet(Street(100031, std::make_pair(36, 47)));
+  // graph.addStreet(Street(100032, std::make_pair(47, 36)));
+  // graph.addStreet(Street(100033, std::make_pair(48, 59)));
+  // graph.addStreet(Street(100034, std::make_pair(59, 48)));
+  // graph.addStreet(Street(100035, std::make_pair(60, 71)));
+  // graph.addStreet(Street(100036, std::make_pair(71, 60)));
+  // graph.addStreet(Street(100037, std::make_pair(72, 83)));
+  // graph.addStreet(Street(100038, std::make_pair(83, 72)));
+  // graph.addStreet(Street(100039, std::make_pair(84, 95)));
+  // graph.addStreet(Street(100040, std::make_pair(95, 84)));
+  // graph.addStreet(Street(100041, std::make_pair(96, 107)));
+  // graph.addStreet(Street(100042, std::make_pair(107, 96)));
+  // graph.addStreet(Street(100043, std::make_pair(108, 119)));
+  // graph.addStreet(Street(100044, std::make_pair(119, 108)));
+
+
+  // graph.buildAdj();
+
+  std::cout << "Number of nodes: " << graph.nodeSet().size() << '\n';
+  std::cout << "Number of streets: " << graph.streetSet().size() << '\n';
+
+  std::cout << "Traffic Lightning the simulation...\n";
+  for (Unit i{0}; i < graph.nodeSet().size(); ++i) {
+    graph.makeTrafficLight<Delay>(i);
+  }
+  std::cout << "Making every street a spire...\n";
+  for (const auto &[id, street] : graph.streetSet()) {
+    graph.makeSpireStreet(id);
+  }
+  // check isSpire for each street
+  for (const auto &[id, street] : graph.streetSet()) {
+    if (!street->isSpire()) {
+      std::cerr << "Street " << id << " is not a spire.\n";
+    }
+  }
+  std::cout << "Setting street parameters..." << '\n';
+  for (const auto &[streetId, street] : graph.streetSet()) {
+    street->setLength(2e3);
+    street->setCapacity(225);
+    street->setTransportCapacity(1);
+    street->setMaxSpeed(13.9);
+  }
+  // TODO cambia i parametri dei semafori
+  const auto meanTime = 60.;
+  const auto stdTime = 10.;
+  const auto &adj = graph.adjMatrix();
+  const auto &degreeVector = adj.getDegreeVector();
+  // create gaussian random number generator
+  std::random_device rd;
+  std::mt19937 gen(rd());
+  gen.seed(64313);
+  std::normal_distribution d(meanTime, stdTime);
+  std::array<uint8_t, 2> sda{0, 0};
+  auto random = [&d, &gen]() { return std::round(d(gen)); };
+  std::cout << "Setting traffic light parameters..." << '\n';
+  for (const auto &[nodeId, node] : graph.nodeSet()) {
+    auto &tl = dynamic_cast<TrafficLight &>(*node);
+    tl.setCapacity(degreeVector(nodeId));
+    double value = -1.;
+    while (value < 0.) {
+      value = random();
+    }
+    tl.setDelay(static_cast<Delay>(value));
+    const auto &col = adj.getCol(nodeId, true);
+    std::set<Unit> streets;
+    const auto id = col.begin();
+    const auto &refLat =
+        graph.nodeSet()
+            .at(graph.streetSet().at(id->first)->nodePair().second)
+            ->coords()
+            .value()
+            .first;
+    for (const auto &[c, value] : col) {
+      const auto &lat = graph.nodeSet()
+                            .at(graph.streetSet().at(c)->nodePair().first)
+                            ->coords()
+                            .value()
+                            .first;
+      // std::cout << "Lat: " << lat << " RefLat: " << refLat << '\n';
+      if (lat == refLat) {
+        streets.emplace(c);
+      }
+    }
+    tl.setStreetPriorities(streets);
+    ++sda[streets.size() - 1];
+    // std::cout << "Node id: " << nodeId << " has " << streets.size()
+    //           << "streets.\n";
+  }
+  std::cout << "Nodes with one street: " << static_cast<int>(sda[0]) << '\n';
+  std::cout << "Nodes with two streets: " << static_cast<int>(sda[1]) << '\n';
+  std::cout << "Done." << std::endl;
+
+  std::cout << "Creating dynamics...\n";
+
+  Dynamics dynamics{graph};
+  Unit n{0};
+  for (const auto &[nodeId, degree] : dv) {
+    if (degree < 4) {
+      dynamics.addItinerary(Itinerary{n, nodeId});
+      ++n;
+    }
+  }
+  std::cout << "Number of exits: " << n << '\n';
+
+  dynamics.setSeed(SEED);
+  dynamics.setErrorProbability(ERROR_PROBABILITY);
+  // dynamics.setMaxFlowPercentage(0.69);
+  // dynamics.setForcePriorities(false);
+  dynamics.setSpeedFluctuationSTD(0.1);
+  dynamics.setMinSpeedRateo(0.95);
+  dynamics.updatePaths();
+
+  const auto TM = dynamics.turnMapping();
+
+  std::cout << "Done." << std::endl;
+  std::cout << "Running simulation...\n";
+
+  std::ofstream out(OUT_FOLDER + "data.csv");
+  out << "time;n_agents;mean_speed;mean_speed_err;mean_density;mean_density_"
+         "err;mean_flow;mean_flow_err;mean_traveltime;mean_traveltime_err;mean_flow_spires;mean_flow_spires_err\n";
+#ifdef PRINT_DENSITIES
+  std::ofstream streetDensity(OUT_FOLDER + "densities.csv");
+  streetDensity << "time;";
+  for (const auto &[id, street] : dynamics.graph().streetSet()) {
+    streetDensity << id << ';';
+  }
+  streetDensity << '\n';
+#endif
+#ifdef PRINT_FLOWS
+  std::ofstream streetFlow(OUT_FOLDER + "flows.csv");
+  streetFlow << "time;";
+  for (const auto &[id, street] : dynamics.graph().streetSet()) {
+    streetFlow << id << ';';
+  }
+  streetFlow << '\n';
+#endif
+#ifdef PRINT_SPEEDS
+  std::ofstream streetSpeed(OUT_FOLDER + "speeds.csv");
+  streetSpeed << "time;";
+  for (const auto &[id, street] : dynamics.graph().streetSet()) {
+    streetSpeed << id << ';';
+  }
+  streetSpeed << '\n';
+#endif
+#ifdef PRINT_OUT_SPIRES
+  std::ofstream outSpires(OUT_FOLDER + "out_spires.csv");
+  std::ofstream inSpires(OUT_FOLDER + "in_spires.csv");
+  outSpires << "time;";
+  inSpires << "time;";
+  for (const auto &[id, street] : dynamics.graph().streetSet()) {
+    outSpires << id << ';';
+    inSpires << id << ';';
+  }
+  outSpires << '\n';
+  inSpires << '\n';
+#endif
+#ifdef PRINT_TP
+  std::ofstream outTP(OUT_FOLDER + "turn_probabilities.csv");
+  outTP << "time;";
+  for (const auto &[id, street] : dynamics.graph().streetSet()) {
+    outTP << id << ';';
+  }
+  outTP << '\n';
+#endif
+#ifdef OPTIMIZE
+  std::ofstream modTime(OUT_FOLDER + "modTimes.csv");
+  std::ofstream tl_counter(OUT_FOLDER + "tl_counter.csv");
+    modTime << "time;";
+    for (const auto &[nodeId, node] : dynamics.graph().nodeSet()) {
+      modTime << nodeId << ';';
+    }
+    modTime << '\n';
+    tl_counter << "time;counter" << '\n';
+#endif
+
+  int deltaAgents{std::numeric_limits<int>::max()};
+  int previousAgents{0};
+  
+  // std::vector<int> deltas;
+
+  // lauch progress bar
+  std::thread t([]() {
+    while (progress < MAX_TIME) {
+      printLoadingBar(progress, MAX_TIME);
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+  });
+  // dynamics.addAgentsUniformly(20000);
+  while (dynamics.time() < MAX_TIME) {
+    if (dynamics.time() < MAX_TIME && nAgents > 0) {
+      if (dynamics.time() % 60 == 0) {
+        dynamics.addAgentsUniformly(nAgents);
+      }
+    }
+    dynamics.evolve(false);
+
+#ifdef OPTIMIZE
+    if (dynamics.time() % 420 == 0) { //420
+      std::cout << '\t' << "-> Chiamata alla funzione optimizeTrafficLights" <<'\n';
+      dynamics.optimizeTrafficLights(std::floor(420. / 60), 0.15);
+      modTime << dynamics.time() << ';';
+      tl_counter << dynamics.time() << ';';
+      int counter{0};
+      for (const auto &[nodeId, node] : dynamics.graph().nodeSet()) {
+        auto &tl = dynamic_cast<TrafficLight &>(*node);
+        modTime << tl.modTime() << ';';
+        if (tl.modTime() > 0) {
+          counter++;
+        }
+      }
+      modTime << std::endl;
+      tl_counter << counter;
+      tl_counter << std::endl;
+    }
+#endif
+    if (dynamics.time() % 2400 == 0 && nAgents > 0) {
+      // auto meanDelta = std::accumulate(deltas.begin(), deltas.end(), 0) /
+      // deltas.size();
+      deltaAgents = dynamics.agents().size() - 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();
+      // deltas.clear();
+    }
+
+    // if (meanDensity.mean > 0.7) {
+    //   nAgents = 0;
+    // }
+
+    if (dynamics.time() % 300 == 0) {
+      // printLoadingBar(dynamics.time(), MAX_TIME);
+      // deltaAgents = std::labs(dynamics.agents().size() - previousAgents);
+#ifdef PRINT_OUT_SPIRES
+      outSpires << dynamics.time() << ';';
+      inSpires << dynamics.time() << ';';
+      for (const auto &[id, street] : dynamics.graph().streetSet()) {
+        auto &spire = dynamic_cast<SpireStreet &>(*street);
+        outSpires << spire.outputCounts(false) << ';';
+        inSpires << spire.inputCounts(false) << ';';
+      }
+      outSpires << std::endl;
+      inSpires << std::endl;
+#endif
+      const auto &meanSpeed{dynamics.streetMeanSpeed()};
+      const auto &meanDensity{dynamics.streetMeanDensity()};
+      const auto &meanFlow{dynamics.streetMeanFlow()};
+      const auto &meanTravelTime{dynamics.meanTravelTime()};
+      const auto &meanSpireFlow{dynamics.meanSpireOutputFlow()};
+
+      out << dynamics.time() << ';' << dynamics.agents().size() << ';'
+          << meanSpeed.mean << ';' << meanSpeed.std << ';' << meanDensity.mean
+          << ';' << meanDensity.std << ';' << meanFlow.mean << ';'
+          << meanFlow.std << ';' << meanTravelTime.mean << ';'
+          << meanTravelTime.std << ';' << meanSpireFlow.mean << ';'
+          << meanSpireFlow.std << std::endl;
+      // deltas.push_back(deltaAgents);
+      // previousAgents = dynamics.agents().size();
+#ifdef PRINT_TP
+      const auto &tc{dynamics.turnCounts()};
+      outTP << dynamics.time() << ';';
+      for (const auto &[id, street] : dynamics.graph().streetSet()) {
+        const auto& probs{tc.at(id)};
+        outTP << '[';
+        const auto& nextStreets = TM.at(id);
+        bool first = true;
+        for (auto i = 0; i < 4; ++i) {
+          if (nextStreets[i] < 0) {
+            continue;
+          }
+          if (!first) {
+            outTP << ',';
+          }
+          outTP << '(';
+          outTP << nextStreets[i] << ',';
+          outTP << probs[i];
+          outTP << ')';
+          first = false;
+        }
+        // for (const auto& prob: probs) {
+        //   outTP << prob;
+        //   if (&prob != &probs.back()) {
+        //     outTP << ',';
+        //   }
+        // }
+        outTP << "];";
+      }
+      outTP << std::endl;
+#endif
+    }
+    if (dynamics.time() % 10 == 0) {
+#ifdef PRINT_DENSITIES
+      streetDensity << dynamics.time() << ';';
+      for (const auto &[id, street] : dynamics.graph().streetSet()) {
+        streetDensity << street->density() << ';';
+      }
+      streetDensity << std::endl;
+#endif
+#ifdef PRINT_FLOWS
+      streetFlow << dynamics.time() << ';';
+      for (const auto &[id, street] : dynamics.graph().streetSet()) {
+        const auto &meanSpeed = dynamics.streetMeanSpeed(id);
+        if (meanSpeed.has_value()) {
+          streetFlow << meanSpeed.value() * street->density() << ';';
+        } else {
+          streetFlow << 0 << ';';
+        }
+      }
+      streetFlow << std::endl;
+#endif
+#ifdef PRINT_SPEEDS
+      streetSpeed << dynamics.time() << ';';
+      for (const auto &[id, street] : dynamics.graph().streetSet()) {
+        const auto &meanSpeed = dynamics.streetMeanSpeed(id);
+        if (meanSpeed.has_value()) {
+          streetSpeed << meanSpeed.value() << ';';
+        } else {
+          streetSpeed << 0 << ';';
+        }
+      }
+      streetSpeed << std::endl;
+#endif
+    }
+    ++progress;
+  }
+  out.close();
+#ifdef PRINT_DENSITIES
+  streetDensity.close();
+#endif
+#ifdef PRINT_FLOWS
+  streetFlow.close();
+#endif
+#ifdef PRINT_SPEEDS
+  streetSpeed.close();
+#endif
+#ifdef PRINT_OUT_SPIRES
+  outSpires.close();
+  inSpires.close();
+#endif
+  // std::cout << std::endl;
+  // std::map<uint8_t, std::string> turnNames{
+  //     {0, "left"}, {1, "straight"}, {2, "right"}, {3, "u-turn"}};
+  // const auto prob = dynamics.turnProbabilities();
+  // uint8_t i{0};
+  // for (auto value: prob) {
+  //   std::cout << "Probability of turning " << std::quoted(turnNames[i]) << ": " << value * 100 << "%\n";
+  //   ++i;
+  // }
+  t.join();
+  std::cout << '\n';
+  std::cout << "Done." << std::endl;
+
+  return 0;
+}

From 2225833821073feda06e67091e2cba0cb0531ebe Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Sat, 5 Oct 2024 13:59:23 +0200
Subject: [PATCH 11/19] Remove simulation file

---
 examples/slow_charge_tl_vero.cpp | 462 -------------------------------
 1 file changed, 462 deletions(-)
 delete mode 100644 examples/slow_charge_tl_vero.cpp

diff --git a/examples/slow_charge_tl_vero.cpp b/examples/slow_charge_tl_vero.cpp
deleted file mode 100644
index 6c3be477c..000000000
--- a/examples/slow_charge_tl_vero.cpp
+++ /dev/null
@@ -1,462 +0,0 @@
-// TODO modificare header
-
-#include "src/dsm/dsm.hpp"
-//#include "src/dsm/dsm-copy.hpp"
-#include <array>
-#include <cmath>
-#include <cstdint>
-#include <fstream>
-#include <iomanip>
-#include <iostream>
-#include <numeric>
-#include <random>
-#include <set>
-#include <string>
-#include <filesystem>
-namespace fs = std::filesystem;
-
-#include <thread>
-#include <atomic>
-
-std::atomic_int progress{0};
-uint nAgents{315}; // 315 for error probability 0.3, 450 for error probability 0.05
-
-// uncomment these lines to print densities, flows and speeds
-#define PRINT_DENSITIES
-// #define PRINT_FLOWS
-#define PRINT_OUT_SPIRES
-// #define PRINT_SPEEDS
-// #define PRINT_TP
-// TODO questo è il branch con il nuovo ALGORITMO
-//#define OPTIMIZE
-
-// Compatible with dsm 1.2.1
-constexpr double ERROR_PROBABILITY{0.3}; // seed for random number generator
-constexpr int SEED{69}; // seed for random number generator
-const std::string IN_MATRIX{"./matrix.dat"}; // input matrix file
-const std::string IN_COORDS{"./coordinates.dsm"}; // input coords file
-// TODO nuova cartella output
-//const std::string OUT_FOLDER{"./output-tesi/greg-op_seed" + std::to_string(SEED) + "_" + std::to_string(nAgents) + "_gaussiana_m100_std20/"}; // output folder
-const std::string OUT_FOLDER{"./output-tesi/no-op_seed" + std::to_string(SEED) + "_" + std::to_string(nAgents) + "_gaussiana_m60_std10/"}; // output folder
-//const std::string OUT_FOLDER{"./output-tesi/new-op_dyn-vrs-copy2-oi2_seed" + std::to_string(SEED) + "_" + std::to_string(nAgents) + "_gaussiana_m100_std20/"}; // output folder
-constexpr auto MAX_TIME{
-    static_cast<unsigned int>(1e6)}; // maximum time of simulation
-
-using Unit = unsigned int;
-using Delay = uint8_t;
-
-using Graph = dsm::Graph<Unit, Unit>;
-using Itinerary = dsm::Itinerary<Unit>;
-using Dynamics = dsm::FirstOrderDynamics<Unit, Unit, Delay>;
-using Street = dsm::Street<Unit, Unit>;
-using SpireStreet = dsm::SpireStreet<Unit, Unit>;
-using TrafficLight = dsm::TrafficLight<Unit, Unit, Delay>;
-
-void printLoadingBar(int const i, int const n) {
-  std::cout << "Loading: " << std::setprecision(2) << std::fixed
-            << (i * 100. / n) << "%" << '\r';
-  std::cout.flush();
-}
-
-int main() {
-  // Clear output folder or create it if it doesn't exist
-  if (fs::exists(OUT_FOLDER)) {
-    fs::remove_all(OUT_FOLDER);
-  }
-  fs::create_directory(OUT_FOLDER);
-  // Starting
-  std::cout << "Using dsm version: " << dsm::version() << '\n';
-  Graph graph{};
-  std::cout << "Importing matrix.dat...\n";
-  graph.importMatrix(IN_MATRIX, false);
-  graph.importCoordinates(IN_COORDS);
-  graph.buildAdj();
-  const auto dv = graph.adjMatrix().getDegreeVector();
-
-  // graph.addStreet(Street(100002, std::make_pair(0, 108)));
-  // graph.addStreet(Street(100003, std::make_pair(108, 0)));
-  // graph.addStreet(Street(100004, std::make_pair(0, 11)));
-  // graph.addStreet(Street(100005, std::make_pair(11, 0)));
-  // graph.addStreet(Street(100006, std::make_pair(1, 109)));
-  // graph.addStreet(Street(100007, std::make_pair(109, 1)));
-  // graph.addStreet(Street(100008, std::make_pair(2, 110)));
-  // graph.addStreet(Street(100009, std::make_pair(110, 2)));
-  // graph.addStreet(Street(100010, std::make_pair(3, 111)));
-  // graph.addStreet(Street(100011, std::make_pair(111, 3)));
-  // graph.addStreet(Street(100012, std::make_pair(4, 112)));
-  // graph.addStreet(Street(100013, std::make_pair(112, 4)));
-  // graph.addStreet(Street(100014, std::make_pair(5, 113)));
-  // graph.addStreet(Street(100015, std::make_pair(113, 5)));
-  // graph.addStreet(Street(100016, std::make_pair(6, 114)));
-  // graph.addStreet(Street(100017, std::make_pair(114, 6)));
-  // graph.addStreet(Street(100018, std::make_pair(7, 115)));
-  // graph.addStreet(Street(100019, std::make_pair(115, 7)));
-  // graph.addStreet(Street(100020, std::make_pair(8, 116)));
-  // graph.addStreet(Street(100021, std::make_pair(116, 8)));
-  // graph.addStreet(Street(100022, std::make_pair(9, 117)));
-  // graph.addStreet(Street(100023, std::make_pair(117, 9)));
-  // graph.addStreet(Street(100024, std::make_pair(10, 118)));
-  // graph.addStreet(Street(100025, std::make_pair(118, 10)));
-  // graph.addStreet(Street(100026, std::make_pair(11, 119)));
-  // graph.addStreet(Street(100027, std::make_pair(119, 11)));
-  // graph.addStreet(Street(100028, std::make_pair(12, 23)));
-  // graph.addStreet(Street(100029, std::make_pair(23, 12)));
-  // graph.addStreet(Street(100030, std::make_pair(24, 35)));
-  // graph.addStreet(Street(100069, std::make_pair(35, 24)));
-  // graph.addStreet(Street(100031, std::make_pair(36, 47)));
-  // graph.addStreet(Street(100032, std::make_pair(47, 36)));
-  // graph.addStreet(Street(100033, std::make_pair(48, 59)));
-  // graph.addStreet(Street(100034, std::make_pair(59, 48)));
-  // graph.addStreet(Street(100035, std::make_pair(60, 71)));
-  // graph.addStreet(Street(100036, std::make_pair(71, 60)));
-  // graph.addStreet(Street(100037, std::make_pair(72, 83)));
-  // graph.addStreet(Street(100038, std::make_pair(83, 72)));
-  // graph.addStreet(Street(100039, std::make_pair(84, 95)));
-  // graph.addStreet(Street(100040, std::make_pair(95, 84)));
-  // graph.addStreet(Street(100041, std::make_pair(96, 107)));
-  // graph.addStreet(Street(100042, std::make_pair(107, 96)));
-  // graph.addStreet(Street(100043, std::make_pair(108, 119)));
-  // graph.addStreet(Street(100044, std::make_pair(119, 108)));
-
-
-  // graph.buildAdj();
-
-  std::cout << "Number of nodes: " << graph.nodeSet().size() << '\n';
-  std::cout << "Number of streets: " << graph.streetSet().size() << '\n';
-
-  std::cout << "Traffic Lightning the simulation...\n";
-  for (Unit i{0}; i < graph.nodeSet().size(); ++i) {
-    graph.makeTrafficLight<Delay>(i);
-  }
-  std::cout << "Making every street a spire...\n";
-  for (const auto &[id, street] : graph.streetSet()) {
-    graph.makeSpireStreet(id);
-  }
-  // check isSpire for each street
-  for (const auto &[id, street] : graph.streetSet()) {
-    if (!street->isSpire()) {
-      std::cerr << "Street " << id << " is not a spire.\n";
-    }
-  }
-  std::cout << "Setting street parameters..." << '\n';
-  for (const auto &[streetId, street] : graph.streetSet()) {
-    street->setLength(2e3);
-    street->setCapacity(225);
-    street->setTransportCapacity(1);
-    street->setMaxSpeed(13.9);
-  }
-  // TODO cambia i parametri dei semafori
-  const auto meanTime = 60.;
-  const auto stdTime = 10.;
-  const auto &adj = graph.adjMatrix();
-  const auto &degreeVector = adj.getDegreeVector();
-  // create gaussian random number generator
-  std::random_device rd;
-  std::mt19937 gen(rd());
-  gen.seed(64313);
-  std::normal_distribution d(meanTime, stdTime);
-  std::array<uint8_t, 2> sda{0, 0};
-  auto random = [&d, &gen]() { return std::round(d(gen)); };
-  std::cout << "Setting traffic light parameters..." << '\n';
-  for (const auto &[nodeId, node] : graph.nodeSet()) {
-    auto &tl = dynamic_cast<TrafficLight &>(*node);
-    tl.setCapacity(degreeVector(nodeId));
-    double value = -1.;
-    while (value < 0.) {
-      value = random();
-    }
-    tl.setDelay(static_cast<Delay>(value));
-    const auto &col = adj.getCol(nodeId, true);
-    std::set<Unit> streets;
-    const auto id = col.begin();
-    const auto &refLat =
-        graph.nodeSet()
-            .at(graph.streetSet().at(id->first)->nodePair().second)
-            ->coords()
-            .value()
-            .first;
-    for (const auto &[c, value] : col) {
-      const auto &lat = graph.nodeSet()
-                            .at(graph.streetSet().at(c)->nodePair().first)
-                            ->coords()
-                            .value()
-                            .first;
-      // std::cout << "Lat: " << lat << " RefLat: " << refLat << '\n';
-      if (lat == refLat) {
-        streets.emplace(c);
-      }
-    }
-    tl.setStreetPriorities(streets);
-    ++sda[streets.size() - 1];
-    // std::cout << "Node id: " << nodeId << " has " << streets.size()
-    //           << "streets.\n";
-  }
-  std::cout << "Nodes with one street: " << static_cast<int>(sda[0]) << '\n';
-  std::cout << "Nodes with two streets: " << static_cast<int>(sda[1]) << '\n';
-  std::cout << "Done." << std::endl;
-
-  std::cout << "Creating dynamics...\n";
-
-  Dynamics dynamics{graph};
-  Unit n{0};
-  for (const auto &[nodeId, degree] : dv) {
-    if (degree < 4) {
-      dynamics.addItinerary(Itinerary{n, nodeId});
-      ++n;
-    }
-  }
-  std::cout << "Number of exits: " << n << '\n';
-
-  dynamics.setSeed(SEED);
-  dynamics.setErrorProbability(ERROR_PROBABILITY);
-  // dynamics.setMaxFlowPercentage(0.69);
-  // dynamics.setForcePriorities(false);
-  dynamics.setSpeedFluctuationSTD(0.1);
-  dynamics.setMinSpeedRateo(0.95);
-  dynamics.updatePaths();
-
-  const auto TM = dynamics.turnMapping();
-
-  std::cout << "Done." << std::endl;
-  std::cout << "Running simulation...\n";
-
-  std::ofstream out(OUT_FOLDER + "data.csv");
-  out << "time;n_agents;mean_speed;mean_speed_err;mean_density;mean_density_"
-         "err;mean_flow;mean_flow_err;mean_traveltime;mean_traveltime_err;mean_flow_spires;mean_flow_spires_err\n";
-#ifdef PRINT_DENSITIES
-  std::ofstream streetDensity(OUT_FOLDER + "densities.csv");
-  streetDensity << "time;";
-  for (const auto &[id, street] : dynamics.graph().streetSet()) {
-    streetDensity << id << ';';
-  }
-  streetDensity << '\n';
-#endif
-#ifdef PRINT_FLOWS
-  std::ofstream streetFlow(OUT_FOLDER + "flows.csv");
-  streetFlow << "time;";
-  for (const auto &[id, street] : dynamics.graph().streetSet()) {
-    streetFlow << id << ';';
-  }
-  streetFlow << '\n';
-#endif
-#ifdef PRINT_SPEEDS
-  std::ofstream streetSpeed(OUT_FOLDER + "speeds.csv");
-  streetSpeed << "time;";
-  for (const auto &[id, street] : dynamics.graph().streetSet()) {
-    streetSpeed << id << ';';
-  }
-  streetSpeed << '\n';
-#endif
-#ifdef PRINT_OUT_SPIRES
-  std::ofstream outSpires(OUT_FOLDER + "out_spires.csv");
-  std::ofstream inSpires(OUT_FOLDER + "in_spires.csv");
-  outSpires << "time;";
-  inSpires << "time;";
-  for (const auto &[id, street] : dynamics.graph().streetSet()) {
-    outSpires << id << ';';
-    inSpires << id << ';';
-  }
-  outSpires << '\n';
-  inSpires << '\n';
-#endif
-#ifdef PRINT_TP
-  std::ofstream outTP(OUT_FOLDER + "turn_probabilities.csv");
-  outTP << "time;";
-  for (const auto &[id, street] : dynamics.graph().streetSet()) {
-    outTP << id << ';';
-  }
-  outTP << '\n';
-#endif
-#ifdef OPTIMIZE
-  std::ofstream modTime(OUT_FOLDER + "modTimes.csv");
-  std::ofstream tl_counter(OUT_FOLDER + "tl_counter.csv");
-    modTime << "time;";
-    for (const auto &[nodeId, node] : dynamics.graph().nodeSet()) {
-      modTime << nodeId << ';';
-    }
-    modTime << '\n';
-    tl_counter << "time;counter" << '\n';
-#endif
-
-  int deltaAgents{std::numeric_limits<int>::max()};
-  int previousAgents{0};
-  
-  // std::vector<int> deltas;
-
-  // lauch progress bar
-  std::thread t([]() {
-    while (progress < MAX_TIME) {
-      printLoadingBar(progress, MAX_TIME);
-      std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    }
-  });
-  // dynamics.addAgentsUniformly(20000);
-  while (dynamics.time() < MAX_TIME) {
-    if (dynamics.time() < MAX_TIME && nAgents > 0) {
-      if (dynamics.time() % 60 == 0) {
-        dynamics.addAgentsUniformly(nAgents);
-      }
-    }
-    dynamics.evolve(false);
-
-#ifdef OPTIMIZE
-    if (dynamics.time() % 420 == 0) { //420
-      std::cout << '\t' << "-> Chiamata alla funzione optimizeTrafficLights" <<'\n';
-      dynamics.optimizeTrafficLights(std::floor(420. / 60), 0.15);
-      modTime << dynamics.time() << ';';
-      tl_counter << dynamics.time() << ';';
-      int counter{0};
-      for (const auto &[nodeId, node] : dynamics.graph().nodeSet()) {
-        auto &tl = dynamic_cast<TrafficLight &>(*node);
-        modTime << tl.modTime() << ';';
-        if (tl.modTime() > 0) {
-          counter++;
-        }
-      }
-      modTime << std::endl;
-      tl_counter << counter;
-      tl_counter << std::endl;
-    }
-#endif
-    if (dynamics.time() % 2400 == 0 && nAgents > 0) {
-      // auto meanDelta = std::accumulate(deltas.begin(), deltas.end(), 0) /
-      // deltas.size();
-      deltaAgents = dynamics.agents().size() - 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();
-      // deltas.clear();
-    }
-
-    // if (meanDensity.mean > 0.7) {
-    //   nAgents = 0;
-    // }
-
-    if (dynamics.time() % 300 == 0) {
-      // printLoadingBar(dynamics.time(), MAX_TIME);
-      // deltaAgents = std::labs(dynamics.agents().size() - previousAgents);
-#ifdef PRINT_OUT_SPIRES
-      outSpires << dynamics.time() << ';';
-      inSpires << dynamics.time() << ';';
-      for (const auto &[id, street] : dynamics.graph().streetSet()) {
-        auto &spire = dynamic_cast<SpireStreet &>(*street);
-        outSpires << spire.outputCounts(false) << ';';
-        inSpires << spire.inputCounts(false) << ';';
-      }
-      outSpires << std::endl;
-      inSpires << std::endl;
-#endif
-      const auto &meanSpeed{dynamics.streetMeanSpeed()};
-      const auto &meanDensity{dynamics.streetMeanDensity()};
-      const auto &meanFlow{dynamics.streetMeanFlow()};
-      const auto &meanTravelTime{dynamics.meanTravelTime()};
-      const auto &meanSpireFlow{dynamics.meanSpireOutputFlow()};
-
-      out << dynamics.time() << ';' << dynamics.agents().size() << ';'
-          << meanSpeed.mean << ';' << meanSpeed.std << ';' << meanDensity.mean
-          << ';' << meanDensity.std << ';' << meanFlow.mean << ';'
-          << meanFlow.std << ';' << meanTravelTime.mean << ';'
-          << meanTravelTime.std << ';' << meanSpireFlow.mean << ';'
-          << meanSpireFlow.std << std::endl;
-      // deltas.push_back(deltaAgents);
-      // previousAgents = dynamics.agents().size();
-#ifdef PRINT_TP
-      const auto &tc{dynamics.turnCounts()};
-      outTP << dynamics.time() << ';';
-      for (const auto &[id, street] : dynamics.graph().streetSet()) {
-        const auto& probs{tc.at(id)};
-        outTP << '[';
-        const auto& nextStreets = TM.at(id);
-        bool first = true;
-        for (auto i = 0; i < 4; ++i) {
-          if (nextStreets[i] < 0) {
-            continue;
-          }
-          if (!first) {
-            outTP << ',';
-          }
-          outTP << '(';
-          outTP << nextStreets[i] << ',';
-          outTP << probs[i];
-          outTP << ')';
-          first = false;
-        }
-        // for (const auto& prob: probs) {
-        //   outTP << prob;
-        //   if (&prob != &probs.back()) {
-        //     outTP << ',';
-        //   }
-        // }
-        outTP << "];";
-      }
-      outTP << std::endl;
-#endif
-    }
-    if (dynamics.time() % 10 == 0) {
-#ifdef PRINT_DENSITIES
-      streetDensity << dynamics.time() << ';';
-      for (const auto &[id, street] : dynamics.graph().streetSet()) {
-        streetDensity << street->density() << ';';
-      }
-      streetDensity << std::endl;
-#endif
-#ifdef PRINT_FLOWS
-      streetFlow << dynamics.time() << ';';
-      for (const auto &[id, street] : dynamics.graph().streetSet()) {
-        const auto &meanSpeed = dynamics.streetMeanSpeed(id);
-        if (meanSpeed.has_value()) {
-          streetFlow << meanSpeed.value() * street->density() << ';';
-        } else {
-          streetFlow << 0 << ';';
-        }
-      }
-      streetFlow << std::endl;
-#endif
-#ifdef PRINT_SPEEDS
-      streetSpeed << dynamics.time() << ';';
-      for (const auto &[id, street] : dynamics.graph().streetSet()) {
-        const auto &meanSpeed = dynamics.streetMeanSpeed(id);
-        if (meanSpeed.has_value()) {
-          streetSpeed << meanSpeed.value() << ';';
-        } else {
-          streetSpeed << 0 << ';';
-        }
-      }
-      streetSpeed << std::endl;
-#endif
-    }
-    ++progress;
-  }
-  out.close();
-#ifdef PRINT_DENSITIES
-  streetDensity.close();
-#endif
-#ifdef PRINT_FLOWS
-  streetFlow.close();
-#endif
-#ifdef PRINT_SPEEDS
-  streetSpeed.close();
-#endif
-#ifdef PRINT_OUT_SPIRES
-  outSpires.close();
-  inSpires.close();
-#endif
-  // std::cout << std::endl;
-  // std::map<uint8_t, std::string> turnNames{
-  //     {0, "left"}, {1, "straight"}, {2, "right"}, {3, "u-turn"}};
-  // const auto prob = dynamics.turnProbabilities();
-  // uint8_t i{0};
-  // for (auto value: prob) {
-  //   std::cout << "Probability of turning " << std::quoted(turnNames[i]) << ": " << value * 100 << "%\n";
-  //   ++i;
-  // }
-  t.join();
-  std::cout << '\n';
-  std::cout << "Done." << std::endl;
-
-  return 0;
-}

From e110bbcf80f4eab9e63958d9a9987459b79c7f75 Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Sat, 5 Oct 2024 13:59:46 +0200
Subject: [PATCH 12/19] Fix Makefile

---
 examples/Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/Makefile b/examples/Makefile
index 768a631d4..c13c5ed59 100644
--- a/examples/Makefile
+++ b/examples/Makefile
@@ -1,5 +1,5 @@
 slow_charge_tl:
-	./slow_charge_tl.out 69 0.3 ./sctl/ 0
+	./slow_charge_tl.out 69 0.3 ./sctl 0
 slow_charge_rb:
 	./slow_charge_rb.out 69 0.3 ./scrb/
 stalingrado:

From 9a638fa24ab83e855508792a88567368782d5db2 Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Sat, 5 Oct 2024 14:00:20 +0200
Subject: [PATCH 13/19] Optimize new code

---
 src/dsm/headers/Dynamics.hpp | 30 ++++++++++++++----------------
 1 file changed, 14 insertions(+), 16 deletions(-)

diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp
index 607e62346..7dc8a9857 100644
--- a/src/dsm/headers/Dynamics.hpp
+++ b/src/dsm/headers/Dynamics.hpp
@@ -737,6 +737,7 @@ namespace dsm {
                       "bounded between 0-1. Inserted value: {}",
                       densityTolerance)));
     }
+    const auto meanDensityGlob = streetMeanDensity().mean;  // Measurement<double>
     for (const auto& [nodeId, node] : m_graph.nodeSet()) {
       if (!node->isTrafficLight()) {
         continue;
@@ -772,29 +773,26 @@ namespace dsm {
       // If the difference is not less than the threshold
       //    - Check that the incoming streets have a density less than the mean one (eventually + tolerance): I want to avoid being into the cluster, better to be out or on the border
       //    - If the previous check fails, do nothing
-      auto meanDensity_whole = streetMeanDensity().mean;  // Measurement<double>
-      double densitySum{};
-      int i{};
-      // Store the ids of outgoing streets
-      for (const auto& [streetId, street] : m_graph.streetSet()) {
-        if (street->nodePair().first == nodeId) {
-          i++;
-          auto density = street->density();
-          densitySum += density;
+      double meanDensity_streets{0.};
+      {
+        // Store the ids of outgoing streets
+        const auto& row{m_graph.adjMatrix().getRow(nodeId, true)};
+        for (const auto& [streetId, _] : row) {
+          meanDensity_streets += m_graph.streetSet()[streetId]->density();
         }
+        // Take the mean density of the outgoing streets
+        meanDensity_streets /= row.size();
       }
-      // Take the mean density of the outgoing streets
-      auto meanDensity_streets = densitySum / i;
-      //std::cout << '\t' << " -> Mean network density: " << std::setprecision(7) << meanDensity_whole << '\n';
+      //std::cout << '\t' << " -> Mean network density: " << std::setprecision(7) << meanDensityGlob << '\n';
       //std::cout << '\t' << " -> Mean density of 4 outgoing streets: " << std::setprecision(7) << meanDensity_streets << '\n';
-      auto ratio = meanDensity_whole / meanDensity_streets;
+      const auto ratio = meanDensityGlob / meanDensity_streets;
       // densityTolerance represents the max border we want to consider
-      auto dyn_thresh = std::tanh(ratio) * densityTolerance;
+      const auto dyn_thresh = std::tanh(ratio) * densityTolerance;
       //std::cout << '\t' << " -> Parametro ratio: " << std::setprecision(7) << ratio << '\n';
       //std::cout << '\t' << " -> Parametro dyn_thresh: " << std::setprecision(7) << dyn_thresh << '\n';
-      if (meanDensity_whole + (dyn_thresh)*meanDensity_whole > meanDensity_streets) {
+      if (meanDensityGlob * (1. + dyn_thresh) > meanDensity_streets) {
         //std::cout << '\t' << " -> I'm on the cluster's border" << '\n';
-        if (meanDensity_whole > meanDensity_streets) {
+        if (meanDensityGlob > meanDensity_streets) {
           //std::cout << '\t' << " -> LESS than max density" << '\n';
           if (!(redTime > greenTime) && (redSum > greenSum) && (greenTime > delta)) {
             greenTime -= delta;

From 9e8459a3f9ebf0a368cef9eed15af6504d21a02c Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Wed, 9 Oct 2024 14:40:55 +0200
Subject: [PATCH 14/19] Change percentage param name to nCycles in
 optimizeTrafficLights

---
 src/dsm/headers/Dynamics.hpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp
index 35f1e6a20..842f364bb 100644
--- a/src/dsm/headers/Dynamics.hpp
+++ b/src/dsm/headers/Dynamics.hpp
@@ -169,12 +169,12 @@ namespace dsm {
     /// @param reinsert_agents If true, the agents are reinserted in the simulation after they reach their destination
     virtual void evolve(bool reinsert_agents = false);
     /// @brief Optimize the traffic lights by changing the green and red times
-    /// @param percentage double, The percentage of the TOTAL cycle time to add or subtract to the green time
+    /// @param nCycles Delay, The number of cycles (times agents are being added) between two calls of this function
     /// @param threshold double, The percentage of the mean capacity of the streets used as threshold for the delta between the two tails.
     /// @param densityTolerance double, The algorithm will consider all streets with density up to densityTolerance*meanDensity
     /// @details The function cycles over the traffic lights and, if the difference between the two tails is greater than
     ///   the threshold multiplied by the mean capacity of the streets, it changes the green and red times of the traffic light, keeping the total cycle time constant.
-    void optimizeTrafficLights(double percentage,
+    void optimizeTrafficLights(Delay nCycles,
                                double threshold = 0.,
                                double densityTolerance = 0.);
 
@@ -740,7 +740,7 @@ namespace dsm {
   template <typename Id, typename Size, typename Delay>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              is_numeric_v<Delay>)
-  void Dynamics<Id, Size, Delay>::optimizeTrafficLights(double percentage,
+  void Dynamics<Id, Size, Delay>::optimizeTrafficLights(Delay nCycles,
                                                         double threshold,
                                                         double densityTolerance) {
     if (threshold < 0 || threshold > 1) {
@@ -779,7 +779,7 @@ namespace dsm {
         }
       }
       const Delay delta =
-          std::floor(std::abs(static_cast<int>(greenQueue - redQueue)) / percentage);
+          std::floor(std::abs(static_cast<double>(greenQueue - redQueue)) / nCycles);
       if (delta == 0) {
         continue;
       }

From 818b9e4cd1496e12941aa57dc08bb7f209cc09a5 Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Wed, 23 Oct 2024 12:07:40 +0200
Subject: [PATCH 15/19] Division security

---
 src/dsm/headers/Dynamics.hpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp
index 842f364bb..10ce31acf 100644
--- a/src/dsm/headers/Dynamics.hpp
+++ b/src/dsm/headers/Dynamics.hpp
@@ -799,7 +799,10 @@ namespace dsm {
           meanDensity_streets += m_graph.streetSet()[streetId]->density();
         }
         // Take the mean density of the outgoing streets
-        meanDensity_streets /= row.size();
+        const auto nStreets = row.size();
+        if (nStreets > 1) {
+          meanDensity_streets /= nStreets;
+        }
       }
       //std::cout << '\t' << " -> Mean network density: " << std::setprecision(7) << meanDensityGlob << '\n';
       //std::cout << '\t' << " -> Mean density of 4 outgoing streets: " << std::setprecision(7) << meanDensity_streets << '\n';

From 6cc5cedfdce46d3bf5c0b072c265893b438ffbe2 Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Wed, 23 Oct 2024 17:43:04 +0200
Subject: [PATCH 16/19] Fix computations with unsigned values

---
 src/dsm/headers/Dynamics.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp
index 10ce31acf..2fa3976bd 100644
--- a/src/dsm/headers/Dynamics.hpp
+++ b/src/dsm/headers/Dynamics.hpp
@@ -779,7 +779,7 @@ namespace dsm {
         }
       }
       const Delay delta =
-          std::floor(std::abs(static_cast<double>(greenQueue - redQueue)) / nCycles);
+          std::floor(std::fabs(static_cast<int>(greenQueue - redQueue)) / nCycles);
       if (delta == 0) {
         continue;
       }

From 4df7d5ee0d8debf5a7586e5eb707b473eefae581 Mon Sep 17 00:00:00 2001
From: grufoony <gregorio_2000@hotmail.it>
Date: Sat, 26 Oct 2024 11:00:33 +0200
Subject: [PATCH 17/19] Revert Node.hpp to main version

---
 src/dsm/headers/Node.hpp | 26 --------------------------
 1 file changed, 26 deletions(-)

diff --git a/src/dsm/headers/Node.hpp b/src/dsm/headers/Node.hpp
index cb1c17b7e..3cf8e913c 100644
--- a/src/dsm/headers/Node.hpp
+++ b/src/dsm/headers/Node.hpp
@@ -225,12 +225,6 @@ namespace dsm {
     std::optional<std::pair<Delay, Delay>> m_delay;
     Delay m_counter;
     Delay m_phase;
-    /// @brief Variabile che mi dice come sono in rapporto tra di loro il tempo di verde e il tempo di rosso
-    /// @details Valori possibili: 0, 1, 2 (fino a che non verrà modificata)
-    ///           - se 0: |redTime - greenTime| < 10 && redTime > 5 && greenTime > 5
-    ///           - se 1: |redtime - greenTime| \geq 10 && |redTime - greenTime| < 40 && redTime > 5 && greenTime > 5
-    ///           - se 2: |redTime - greenTime| \geq 40 || redTime < 5 || greenTime < 5
-    int m_modTime{7};
 
   public:
     TrafficLight() = delete;
@@ -278,9 +272,6 @@ namespace dsm {
     /// @return std::optional<Delay> The node's delay
     std::optional<std::pair<Delay, Delay>> delay() const { return m_delay; }
     Delay counter() const { return m_counter; }
-    /// @brief Get the node's modTime
-    /// @return int. The node's modTime
-    int modTime() const { return m_modTime; }
     /// @brief Returns true if the traffic light is green
     /// @return bool True if the traffic light is green
     bool isGreen() const;
@@ -313,11 +304,6 @@ namespace dsm {
       }
     }
     m_delay = std::make_pair(delay, delay);
-    if (delay < 5) {
-      m_modTime = 2;
-    } else {
-      m_modTime = 0;
-    }
   }
   template <typename Id, typename Size, typename Delay>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
@@ -333,18 +319,6 @@ namespace dsm {
       }
     }
     m_delay = std::move(delay);
-    if (delay.first < 5 || delay.second < 5) {
-      m_modTime = 2;
-    } else if (std::abs(delay.first - delay.second) < 10) {
-      m_modTime = 0;
-    } else if ((std::abs(delay.first - delay.second) > 10 ||
-                std::abs(delay.first - delay.second) == 10) &&
-               (std::abs(delay.first - delay.second) < 40 ||
-                std::abs(delay.first - delay.second) == 40)) {
-      m_modTime = 1;
-    } else if (std::abs(delay.first - delay.second) > 40) {
-      m_modTime = 2;
-    }
   }
   template <typename Id, typename Size, typename Delay>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&

From 78f086fc74e08284175d074ac6b50408f7babb5e Mon Sep 17 00:00:00 2001
From: Grufoony <gregorio_2000@hotmail.it>
Date: Mon, 4 Nov 2024 11:37:49 +0100
Subject: [PATCH 18/19] Add test for changing traffic light timings

---
 test/Test_dynamics.cpp | 46 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 46 insertions(+)

diff --git a/test/Test_dynamics.cpp b/test/Test_dynamics.cpp
index 35bdf2a00..f7da1f5e4 100644
--- a/test/Test_dynamics.cpp
+++ b/test/Test_dynamics.cpp
@@ -426,6 +426,52 @@ TEST_CASE("Dynamics") {
       }
     }
   }
+  SUBCASE("Traffic Lights optimization algorithm") {
+    GIVEN("A dynamics object with a traffic light intersection") {
+      TrafficLight tl{1};
+      tl.setDelay(4);
+      double length{90.}, max_speed{15.};
+      Street s_01{1, 10, length, max_speed, std::make_pair(0, 1)};
+      Street s_10{5, 5, length, max_speed, std::make_pair(1, 0)};
+      Street s_12{7, 5, length, max_speed, std::make_pair(1, 2)};
+      Street s_21{11, 10, length, max_speed, std::make_pair(2, 1)};
+      Street s_13{8, 5, length, max_speed, std::make_pair(1, 3)};
+      Street s_31{16, 10, length, max_speed, std::make_pair(3, 1)};
+      Street s_14{9, 5, length, max_speed, std::make_pair(1, 4)};
+      Street s_41{21, 10, length, max_speed, std::make_pair(4, 1)};
+      tl.addStreetPriority(1);
+      tl.addStreetPriority(7);
+      Graph graph2;
+      graph2.addNode(std::make_unique<TrafficLight>(tl));
+      graph2.addStreets(s_01, s_10, s_12, s_21, s_13, s_31, s_14, s_41);
+      graph2.buildAdj();
+      Dynamics dynamics{graph2};
+      Itinerary it_0{0, 0}, it_1{1, 2}, it_2{2, 3}, it_3{3, 4};
+      dynamics.addItinerary(it_0);
+      dynamics.addItinerary(it_1);
+      dynamics.addItinerary(it_2);
+      dynamics.addItinerary(it_3);
+      dynamics.updatePaths();
+      dynamics.addAgents(0, 7, 2);
+      dynamics.addAgents(1, 7, 0);
+      dynamics.addAgents(2, 2, 4);
+      dynamics.addAgents(3, 2, 3);
+      dynamics.setDataUpdatePeriod(1);
+      WHEN("We evolve the dynamics and optimize traffic lights") {
+        for (int i = 0; i < 15; ++i) {
+          dynamics.evolve(false);
+        }
+        dynamics.optimizeTrafficLights(2, 0.1, 1.);
+        THEN("Green and red time are different") {
+          const auto timing =
+              dynamic_cast<TrafficLight&>(*dynamics.graph().nodeSet().at(1))
+                  .delay()
+                  .value();
+          CHECK(timing.first != timing.second);
+        }
+      }
+    }
+  }
   SUBCASE("Roundabout") {
     GIVEN(
         "A dynamics object with four streets, one agent for each street, two itineraries "

From fb0700bf57b72eebe558b8964491b477d9c74a29 Mon Sep 17 00:00:00 2001
From: Grufoony <gregorio_2000@hotmail.it>
Date: Mon, 4 Nov 2024 12:24:24 +0100
Subject: [PATCH 19/19] Add another test condition for traffic lights
 optimization

---
 test/Test_dynamics.cpp | 38 ++++++++++++++++++++++++++++----------
 1 file changed, 28 insertions(+), 10 deletions(-)

diff --git a/test/Test_dynamics.cpp b/test/Test_dynamics.cpp
index f7da1f5e4..f803c8700 100644
--- a/test/Test_dynamics.cpp
+++ b/test/Test_dynamics.cpp
@@ -430,17 +430,18 @@ TEST_CASE("Dynamics") {
     GIVEN("A dynamics object with a traffic light intersection") {
       TrafficLight tl{1};
       tl.setDelay(4);
+      tl.setPhase(3);
       double length{90.}, max_speed{15.};
       Street s_01{1, 10, length, max_speed, std::make_pair(0, 1)};
-      Street s_10{5, 5, length, max_speed, std::make_pair(1, 0)};
-      Street s_12{7, 5, length, max_speed, std::make_pair(1, 2)};
+      Street s_10{5, 10, length, max_speed, std::make_pair(1, 0)};
+      Street s_12{7, 10, length, max_speed, std::make_pair(1, 2)};
       Street s_21{11, 10, length, max_speed, std::make_pair(2, 1)};
-      Street s_13{8, 5, length, max_speed, std::make_pair(1, 3)};
+      Street s_13{8, 10, length, max_speed, std::make_pair(1, 3)};
       Street s_31{16, 10, length, max_speed, std::make_pair(3, 1)};
-      Street s_14{9, 5, length, max_speed, std::make_pair(1, 4)};
+      Street s_14{9, 10, length, max_speed, std::make_pair(1, 4)};
       Street s_41{21, 10, length, max_speed, std::make_pair(4, 1)};
       tl.addStreetPriority(1);
-      tl.addStreetPriority(7);
+      tl.addStreetPriority(11);
       Graph graph2;
       graph2.addNode(std::make_unique<TrafficLight>(tl));
       graph2.addStreets(s_01, s_10, s_12, s_21, s_13, s_31, s_14, s_41);
@@ -454,20 +455,37 @@ TEST_CASE("Dynamics") {
       dynamics.updatePaths();
       dynamics.addAgents(0, 7, 2);
       dynamics.addAgents(1, 7, 0);
-      dynamics.addAgents(2, 2, 4);
-      dynamics.addAgents(3, 2, 3);
       dynamics.setDataUpdatePeriod(1);
       WHEN("We evolve the dynamics and optimize traffic lights") {
-        for (int i = 0; i < 15; ++i) {
+        for (int i = 0; i < 8; ++i) {
           dynamics.evolve(false);
         }
-        dynamics.optimizeTrafficLights(2, 0.1, 1.);
+        dynamics.optimizeTrafficLights(2, 0.1, 0.);
         THEN("Green and red time are different") {
           const auto timing =
               dynamic_cast<TrafficLight&>(*dynamics.graph().nodeSet().at(1))
                   .delay()
                   .value();
-          CHECK(timing.first != timing.second);
+          CHECK(timing.first > timing.second);
+        }
+      }
+      WHEN(
+          "We evolve the dynamics and optimize traffic lights with outgoing streets "
+          "full") {
+        dynamics.addAgents(0, 5, 1);
+        dynamics.addAgents(1, 5, 1);
+        dynamics.addAgents(2, 5, 1);
+        dynamics.addAgents(3, 5, 1);
+        for (int i = 0; i < 15; ++i) {
+          dynamics.evolve(false);
+        }
+        dynamics.optimizeTrafficLights(2, 0.1, 0.);
+        THEN("Green and red time are equal") {
+          const auto timing =
+              dynamic_cast<TrafficLight&>(*dynamics.graph().nodeSet().at(1))
+                  .delay()
+                  .value();
+          CHECK_EQ(timing.first, timing.second);
         }
       }
     }