Refactor TrafficLights non-local optimization + add utility functions

src/dsm/dsm.hpp

@@ -6,7 +6,7 @@
 
 static constexpr uint8_t DSM_VERSION_MAJOR = 2;
 static constexpr uint8_t DSM_VERSION_MINOR = 6;
-static constexpr uint8_t DSM_VERSION_PATCH = 25;
+static constexpr uint8_t DSM_VERSION_PATCH = 26;
 
 static auto const DSM_VERSION =
     std::format("{}.{}.{}", DSM_VERSION_MAJOR, DSM_VERSION_MINOR, DSM_VERSION_PATCH);

src/dsm/headers/RoadDynamics.hpp

@@ -738,7 +738,7 @@
                     continue;
                   }
                   auto const& pAgentTemp2{pStreetTemp->queue(i).front()};
-                  if (!pAgentTemp2->streetId().has_value()) {
+                  if (!pAgentTemp2->nextStreetId().has_value()) {
                     continue;
                   }
                   auto const& otherDirection{
@@ -828,6 +828,9 @@
           }
           continue;
         }
+        if (!pAgentTemp->streetId().has_value()) {
+          Logger::error(std::format("Agent {} has no street id", pAgentTemp->id()));
+        }
         auto const& nextStreet{this->graph().edge(pAgentTemp->nextStreetId().value())};
         if (nextStreet->isFull()) {
           continue;
@@ -885,7 +888,7 @@
           it = intersection.agents().erase(it);
           break;
         }
       } else if (pNode->isRoundabout()) {
         auto& roundabout = dynamic_cast<Roundabout&>(*pNode);
         if (roundabout.agents().empty()) {
           return;
@@ -1027,6 +1030,13 @@
         0, static_cast<Size>(this->itineraries().size() - 1)};
     std::uniform_int_distribution<Size> streetDist{
         0, static_cast<Size>(this->graph().nEdges() - 1)};
+    if (this->nAgents() + nAgents > this->graph().maxCapacity()) {
+      Logger::error(std::format(
+          "Cannot add {} agents. The graph has reached its maximum capacity of {}",
+          nAgents,
+          this->graph().maxCapacity()));
+      return;
+    }
     for (Size i{0}; i < nAgents; ++i) {
       if (bRandomItinerary) {
         auto itineraryIt{this->itineraries().cbegin()};
@@ -1039,8 +1049,7 @@
         Size step = streetDist(this->m_generator);
         std::advance(streetIt, step);
         streetId = streetIt->first;
-      } while (this->graph().edge(streetId)->isFull() &&
-               this->nAgents() < this->graph().maxCapacity());
+      } while (this->graph().edge(streetId)->isFull());
       const auto& street{this->graph().edge(streetId)};
       auto pAgent{std::make_unique<Agent>(this->time(), itineraryId, street->source())};
       pAgent->setStreetId(streetId);
@@ -1253,15 +1262,17 @@
                 auto const streetId{sourceId * N + pNode->id()};
                 auto const& pStreet{this->graph().edge(streetId)};
                 if (bUpdateData && pNode->isTrafficLight()) {
-                  if (!m_queuesAtTrafficLights.contains(streetId)) {
+                  if (!m_queuesAtTrafficLights.contains(pStreet->id())) {
                     auto& tl = dynamic_cast<TrafficLight&>(*pNode);
-                    for (auto const& [streetId, pair] : tl.cycles()) {
+                    assert(!tl.cycles().empty());
+                    for (auto const& [id, pair] : tl.cycles()) {
                       for (auto const& [direction, cycle] : pair) {
-                        m_queuesAtTrafficLights[streetId].emplace(direction, 0.);
+                        m_queuesAtTrafficLights[id].emplace(direction, 0.);
                       }
                     }
                   }
-                  for (auto& [direction, value] : m_queuesAtTrafficLights.at(streetId)) {
+                  for (auto& [direction, value] :
+                       m_queuesAtTrafficLights.at(pStreet->id())) {
                     value += pStreet->nExitingAgents(direction, true);
                   }
                 }
@@ -1396,7 +1407,7 @@
     Dynamics<RoadNetwork>::m_evolve();
   }
 
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
   void RoadDynamics<delay_t>::m_trafficlightSingleTailOptimizer(
       double const& beta, std::optional<std::ofstream>& logStream) {
@@ -1518,7 +1529,7 @@
             } else {
               if (direction == Direction::LEFT ||
                   direction == Direction::LEFTANDSTRAIGHT) {
                 greenTimes[3] += cycle.greenTime();
                 ++n[3];
               } else {
                 greenTimes[2] += cycle.greenTime();
@@ -1807,87 +1818,71 @@
         Logger::error(std::format("Could not open log file: {}", logFile));
       }
     }
-    if (optimizationType == TrafficLightOptimization::SINGLE_TAIL) {
-      this->m_trafficlightSingleTailOptimizer(threshold, logStream);
-    } else if (optimizationType == TrafficLightOptimization::DOUBLE_TAIL) {
-      if (threshold < 0) {
-        Logger::error(std::format("The threshold parameter ({}) must be greater than 0.",
-                                  threshold));
-      }
-      auto const nCycles{static_cast<double>(this->time() - m_previousOptimizationTime) /
-                         m_dataUpdatePeriod.value()};
-      for (const auto& [nodeId, pNode] : this->graph().nodes()) {
+    this->m_trafficlightSingleTailOptimizer(threshold, logStream);
+    if (optimizationType == TrafficLightOptimization::DOUBLE_TAIL) {
+      // Try to synchronize congested traffic lights
+      std::unordered_map<Id, double> densities;
+      for (auto const& [nodeId, pNode] : this->graph().nodes()) {
         if (!pNode->isTrafficLight()) {
           continue;
         }
-        if (logStream.has_value()) {
-          *logStream << std::format("\tTraffic Light {}\n", nodeId);
-        }
-        auto& tl = dynamic_cast<TrafficLight&>(*pNode);
-        auto const& streetPriorities = tl.streetPriorities();
-        auto const meanGreenFraction{tl.meanGreenTime(true) / tl.cycleTime()};
-        auto const meanRedFraction{tl.meanGreenTime(false) / tl.cycleTime()};
-
-        double inputGreenSum{0.}, inputRedSum{0.};
-        auto const N{this->graph().nNodes()};
-        auto column = this->graph().adjacencyMatrix().getCol(nodeId);
-        for (const auto& sourceId : column) {
-          auto const streetId = sourceId * N + nodeId;
-          auto const& pStreet{this->graph().edge(streetId)};
-          if (streetPriorities.contains(streetId)) {
-            for (auto const& pair : m_queuesAtTrafficLights.at(streetId)) {
-              inputGreenSum += pair.second / pStreet->nLanes();
-            }
-          } else {
-            for (auto const& pair : m_queuesAtTrafficLights.at(streetId)) {
-              inputRedSum += pair.second / pStreet->nLanes();
-            }
+        double density{0.}, n{0.};
+        auto const& inNeighbours{this->graph().adjacencyMatrix().getCol(nodeId)};
+        for (auto const& sourceId : inNeighbours) {
+          if (!this->graph().node(sourceId)->isTrafficLight()) {
+            continue;
           }
+          density += this->graph()
+                         .edge(sourceId * this->graph().nNodes() + nodeId)
+                         ->density(true);
+          ++n;
         }
-        inputGreenSum /= meanGreenFraction;
-        inputRedSum /= meanRedFraction;
-        auto const inputDifference{(inputGreenSum - inputRedSum) / nCycles};
-        delay_t const delta = std::round(std::abs(inputDifference) / column.size());
-        auto const greenTime = tl.minGreenTime(true);
-        auto const redTime = tl.minGreenTime(false);
-        // 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
-        double outputGreenSum{0.}, outputRedSum{0.};
-        for (auto const& targetId : this->graph().adjacencyMatrix().getRow(nodeId)) {
-          auto const streetId = nodeId * N + targetId;
-          if (!m_queuesAtTrafficLights.contains(streetId)) {
+        density /= n;
+        densities[nodeId] = density;
+      }
+      // Sort densities map from big to small values
+      std::vector<std::pair<Id, double>> sortedDensities(densities.begin(),
+                                                         densities.end());
+
+      // Sort by density descending
+      std::sort(sortedDensities.begin(),
+                sortedDensities.end(),
+                [](auto const& a, auto const& b) { return a.second > b.second; });
+      std::unordered_set<Id> optimizedNodes;
+
+      for (auto const& [nodeId, density] : sortedDensities) {
+        auto const& inNeighbours{this->graph().adjacencyMatrix().getCol(nodeId)};
+        for (auto const& sourceId : inNeighbours) {
+          if (!densities.contains(sourceId) || optimizedNodes.contains(sourceId)) {
             continue;
           }
-          auto const& pStreet{this->graph().edge(streetId)};
-          if (streetPriorities.contains(streetId)) {
-            for (auto const& pair : m_queuesAtTrafficLights.at(streetId)) {
-              outputGreenSum += pair.second / pStreet->nLanes();
-            }
-          } else {
-            for (auto const& pair : m_queuesAtTrafficLights.at(streetId)) {
-              outputGreenSum += pair.second / pStreet->nLanes();
-            }
-          }
-        }
-        auto const outputDifference{(outputGreenSum - outputRedSum) / nCycles};
-        if ((inputDifference * outputDifference > 0) ||
-            std::max(std::abs(inputDifference), std::abs(outputDifference)) < threshold ||
-            delta == 0) {
-          tl.resetCycles();
-          continue;
-        }
-        if (std::abs(inputDifference) > std::abs(outputDifference)) {
-          if ((inputDifference > 0) && (redTime > delta)) {
-            tl.increaseGreenTimes(delta);
-          } else if ((inputDifference < 0) && (greenTime > delta)) {
-            tl.decreaseGreenTimes(delta);
+          auto const& neighbourDensity{densities.at(sourceId)};
+          if (neighbourDensity < 1.3 * density) {
+            continue;
           }
-        } else {
-          if ((outputDifference < 0) && (redTime > delta)) {
-            tl.increaseGreenTimes(delta);
-          } else if ((outputDifference > 0) && (greenTime > delta)) {
-            tl.decreaseGreenTimes(delta);
+          // Try to green-wave the situation
+          auto& tl{static_cast<TrafficLight&>(*(this->graph().node(sourceId)))};
+          auto const& pStreet{
+              this->graph().edge(sourceId * this->graph().nNodes() + nodeId)};
+          tl.increasePhases(pStreet->length() /
+                            (pStreet->maxSpeed() * (1. - 0.6 * pStreet->density(true))));
+          optimizedNodes.insert(sourceId);
+          if (logStream.has_value()) {
+            std::string logMsg =
+                std::format("\tNew cycles for TL {} ({}):\n", tl.id(), tl.cycleTime());
+            for (auto const& [streetId, pair] : tl.cycles()) {
+              auto const& pStreet{this->graph().edge(streetId)};
+              logMsg += std::format(
+                  "\t\tStreet {} -> {}: ", pStreet->source(), pStreet->target());
+              for (auto const& [direction, cycle] : pair) {
+                logMsg += std::format("{}= ({} {}) ",
+                                      directionToString[direction],
+                                      cycle.greenTime(),
+                                      cycle.phase());
+              }
+              logMsg += '\n';
+            }
+            *logStream << logMsg << '\n';
           }
         }
       }

src/dsm/headers/RoadNetwork.hpp

@@ -130,6 +130,8 @@ namespace dsm {
     /// - coilcode: An integer code to identify the coil located on the street
     void importOSMEdges(const std::string& fileName);
 
+    void importTrafficLights(const std::string& fileName);
+
     /// @brief Export the graph's nodes to a csv-like file separated with ';'
     /// @param path The path to the file to export the nodes to
     /// @details The file format is csv-like, with the first line being the column names: id;lon;lat

src/dsm/headers/TrafficLight.hpp

@@ -134,6 +134,8 @@ namespace dsm {
     /// @brief Decrease the green times of the traffic light for priority streets and increase the green times for non-priority streets
     /// @param delta Delay, the time to increase or decrease the green times
     void decreaseGreenTimes(Delay const delta);
+
+    void increasePhases(Delay const phase);
     /// @brief Get the traffic light's cycles
     /// @return std::unordered_map<Id, std::unordered_map<Direction, TrafficLightCycle>> const& The traffic light's cycles
     inline std::unordered_map<Id, std::unordered_map<Direction, TrafficLightCycle>> const&

src/dsm/sources/RoadNetwork.cpp

@@ -151,7 +151,7 @@
         higherNLanes = std::max(higherNLanes, nLan);
         lowerNLanes = std::min(lowerNLanes, nLan);
       }
       if (tl.streetPriorities().empty()) {
         /*************************************************************
          * 1. Check for street names with multiple occurrences
          * ***********************************************************/
@@ -197,8 +197,30 @@
         }
       }
       if (tl.streetPriorities().empty()) {
-        Logger::warning(std::format("Failed to auto-init Traffic Light {}", id));
-        continue;
+        /*************************************************************
+         * 3. Check for streets with opposite angles
+         * ***********************************************************/
+        auto const& streetId = streetAngles.begin()->first;
+        auto const& angle = streetAngles.begin()->second;
+        for (auto const& [streetId2, angle2] : streetAngles) {
+          if (std::abs(angle - angle2) > 0.75 * std::numbers::pi) {
+            tl.addStreetPriority(streetId);
+            tl.addStreetPriority(streetId2);
+            break;
+          }
+        }
+      }
+      if (tl.streetPriorities().empty()) {
+        Logger::warning(
+            std::format("Failed to auto-init Traffic Light {} - going random", id));
+        // Assign first and third keys of capacity map
+        auto it = capacities.begin();
+        auto const& firstKey = it->first;
+        ++it;
+        ++it;
+        auto const& thirdKey = it->first;
+        tl.addStreetPriority(firstKey);
+        tl.addStreetPriority(thirdKey);
       }
 
       // Assign cycles
@@ -521,7 +543,7 @@
       }
       auto const& pNode{m_nodes.at(nodeId)};
       pNode->setCapacity(value);
       value = 0.;
       for (const auto& targetId : m_adjacencyMatrix.getRow(nodeId)) {
         auto const streetId{nodeId * N + targetId};
         auto const& pStreet{m_edges.at(streetId)};
@@ -926,6 +948,52 @@
 
     Logger::info(std::format("Successfully imported {} edges", nEdges()));
   }
+  void RoadNetwork::importTrafficLights(const std::string& fileName) {
+    std::ifstream file{fileName};
+    if (!file.is_open()) {
+      throw std::invalid_argument(
+          Logger::buildExceptionMessage("Cannot find file: " + fileName));
+    }
+    std::string line;
+    std::getline(file, line);  // skip first line
+    while (!file.eof()) {
+      std::getline(file, line);
+      if (line.empty()) {
+        continue;
+      }
+      std::unordered_map<Id, Delay> storedGreenTimes;
+      std::istringstream iss{line};
+      std::string strId, streetSource, strCycleTime, strGT;
+      // id;streetSource;streetTarget;cycleTime;greenTime
+      std::getline(iss, strId, ';');
+      std::getline(iss, streetSource, ';');
+      std::getline(iss, strCycleTime, ';');
+      std::getline(iss, strGT, '\n');
+
+      auto const cycleTime{static_cast<Delay>(std::stoul(strCycleTime))};
+      // Cast node(id) to traffic light
+      auto& pNode{m_nodes.at(m_nodeMapping.at(strId))};
+      if (!pNode->isTrafficLight()) {
+        pNode = std::make_unique<TrafficLight>(
+            pNode->id(), cycleTime, pNode->coords().value());
+      }
+      auto& tl = static_cast<TrafficLight&>(*pNode);
+      auto const srcId{m_nodeMapping.at(streetSource)};
+      auto const streetId{srcId * m_nodes.size() + pNode->id()};
+      auto const greenTime{static_cast<Delay>(std::stoul(strGT))};
+      if (!storedGreenTimes.contains(pNode->id())) {
+        storedGreenTimes.emplace(pNode->id(), greenTime);
+      }
+      auto const storedGT{storedGreenTimes.at(pNode->id())};
+      if (storedGT == greenTime) {
+        auto cycle = TrafficLightCycle(greenTime, 0);
+        tl.setCycle(streetId, dsm::Direction::ANY, cycle);
+      } else {
+        auto cycle = TrafficLightCycle(greenTime, storedGT);
+        tl.setCycle(streetId, dsm::Direction::ANY, cycle);
+      }
+    }
+  }
 
   void RoadNetwork::exportNodes(std::string const& path) {
     // assert that path ends with ".csv"

src/dsm/sources/TrafficLight.cpp

@@ -6,11 +6,16 @@
 
 namespace dsm {
   bool TrafficLightCycle::isGreen(Delay const cycleTime, Delay const counter) const {
-    auto const rest{(m_phase + m_greenTime) / cycleTime};
-    if (rest) {
-      return (counter < rest) || (counter >= m_phase);
+    auto const greenStart = m_phase % cycleTime;
+    auto const greenEnd = (m_phase + m_greenTime) % cycleTime;
+
+    if (greenStart < greenEnd) {
+      // Normal case: green does not wrap around
+      return (counter >= greenStart) && (counter < greenEnd);
+    } else {
+      // Wraparound case: green spans cycle boundary
+      return (counter >= greenStart) || (counter < greenEnd);
     }
-    return (counter >= m_phase) && (counter < m_phase + m_greenTime);
   }
 
   bool TrafficLight::m_allowFreeTurns{true};
@@ -128,10 +133,20 @@
         for (auto& [direction, cycle] : cycles) {
           cycle = TrafficLightCycle(cycle.greenTime() - delta, cycle.phase() + delta);
         }
       }
     }
   }
 
+  void TrafficLight::increasePhases(Delay const phase) {
+    for (auto& [streetId, cycles] : m_cycles) {
+      for (auto& [direction, cycle] : cycles) {
+        // Module new phase with cycleTime
+        auto newPhase{(phase + cycle.phase()) % m_cycleTime};
+        cycle = TrafficLightCycle(cycle.greenTime(), newPhase);
+      }
+    }
+  }
+
   void TrafficLight::decreaseGreenTimes(Delay const delta) {
     for (auto& [streetId, cycles] : m_cycles) {
       if (!m_streetPriorities.contains(streetId)) {