Bugfix in autoMapStreetLanes function

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 = 11;
+static constexpr uint8_t DSM_VERSION_PATCH = 12;
 
 static auto const DSM_VERSION =
     std::format("{}.{}.{}", DSM_VERSION_MAJOR, DSM_VERSION_MINOR, DSM_VERSION_PATCH);

src/dsm/headers/Road.hpp

@@ -4,6 +4,7 @@
 
 #include <memory>
 #include <optional>
+#include <set>
 #include <string>
 
 namespace dsm {
@@ -15,6 +16,7 @@ namespace dsm {
     int m_nLanes;
     std::string m_name;
     int m_priority;
+    std::set<Id> m_forbiddenTurns;  // Stores the forbidden turns (road ids)
 
   public:
     /// @brief Construct a new Road object
@@ -50,6 +52,12 @@ namespace dsm {
     /// @brief Set the road's priority
     /// @param priority The road's priority
     void setPriority(int priority);
+    /// @brief Add a road id to the forbidden turns
+    /// @param roadId The road id to add
+    void addForbiddenTurn(Id roadId);
+    /// @brief Replace the road's forbidden turns with the given set
+    /// @param forbiddenTurns The set of forbidden turns
+    void setForbiddenTurns(std::set<Id> const& forbiddenTurns);
 
     /// @brief Get the length, in meters
     /// @return double The length, in meters
@@ -66,6 +74,11 @@ namespace dsm {
     /// @brief Get the priority
     /// @return int The priority
     int priority() const;
+    /// @brief Get the road's forbidden turns
+    /// @return std::set<Id> The road's forbidden turns
+    /// @details The forbidden turns are the road ids that are not allowed to be used by the agents
+    ///          when they are on the road.
+    std::set<Id> const& forbiddenTurns() const;
 
     virtual int nAgents() const = 0;
     virtual int nMovingAgents() const = 0;

src/dsm/headers/RoadDynamics.hpp

@@ -362,8 +362,8 @@
                 pair.second->deltaAngle(this->graph().edge(previousStreetId)->angle())};
             if (std::abs(delta) < std::numbers::pi) {
               if (delta < 0.) {
-                m_turnMapping[pair.first][dsm::Direction::RIGHT] = previousStreetId;
-                ;  // right
+                m_turnMapping[pair.first][dsm::Direction::RIGHT] =
+                    previousStreetId;  // right
               } else if (delta > 0.) {
                 m_turnMapping[pair.first][dsm::Direction::LEFT] =
                     previousStreetId;  // left
@@ -502,6 +502,18 @@
       }
     }
     assert(!possibleMoves.empty());
+    if (streetId.has_value()) {
+      auto const& pStreet{this->graph().edge(*streetId)};
+      for (auto const& foirbiddenStreetId : pStreet->forbiddenTurns()) {
+        auto const& pForbiddenStreet{this->graph().edge(foirbiddenStreetId)};
+        // if possible moves contains the forbidden street, remove it
+        auto it = std::find(
+            possibleMoves.begin(), possibleMoves.end(), pForbiddenStreet->target());
+        if (it != possibleMoves.end()) {
+          possibleMoves.erase(it);
+        }
+      }
+    }
     std::uniform_int_distribution<Size> moveDist{
         0, static_cast<Size>(possibleMoves.size() - 1)};
     // while loop to avoid U turns in non-roundabout junctions
@@ -1049,7 +1061,7 @@
                   auto const deltaAngle{pNextStreet->deltaAngle(pStreet->angle())};
                   if (std::abs(deltaAngle) < std::numbers::pi) {
                     // Lanes are counted as 0 is the far right lane
-                    if (std::abs(deltaAngle) < std::numbers::pi / 4) {
+                    if (std::abs(deltaAngle) < std::numbers::pi / 8) {
                       std::vector<double> weights;
                       for (auto const& queue : pStreet->exitQueues()) {
                         weights.push_back(1. / (queue.size() + 1));

src/dsm/headers/Street.hpp

@@ -169,7 +169,6 @@ namespace dsm {
   /// @tparam Id The type of the street's id
   /// @tparam Size The type of the street's capacity
   class SpireStreet : public Street, public Counter {
-  private:
   public:
     using Street::Street;
     SpireStreet(Street&& street) : Street(std::move(street)) {}

src/dsm/sources/Road.cpp

@@ -49,6 +49,11 @@
   }
   double Road::meanVehicleLength() { return m_meanVehicleLength; }
 
+  void Road::addForbiddenTurn(Id roadId) { m_forbiddenTurns.insert(roadId); }
+  void Road::setForbiddenTurns(std::set<Id> const& forbiddenTurns) {
+    m_forbiddenTurns = forbiddenTurns;
+  }
+
   void Road::setMaxSpeed(double speed) {
     if (speed < 0.) {
       Logger::error(
@@ -66,4 +71,5 @@
   int Road::nLanes() const { return m_nLanes; }
   std::string Road::name() const { return m_name; }
   int Road::priority() const { return m_priority; }
+  std::set<Id> const& Road::forbiddenTurns() const { return m_forbiddenTurns; }
 };  // namespace dsm

src/dsm/sources/RoadNetwork.cpp

@@ -66,6 +66,19 @@
       }
       newStreetIds.emplace(streetId, newStreetId);
     }
+    std::for_each(
+        m_edges.cbegin(), m_edges.cend(), [this, &newStreetIds](auto const& pair) {
+          auto const& pStreet{pair.second};
+          auto const& forbiddenTurns{pStreet->forbiddenTurns()};
+          if (forbiddenTurns.empty()) {
+            return;
+          }
+          std::set<Id> newForbiddenTurns;
+          for (auto const& streetId : forbiddenTurns) {
+            newForbiddenTurns.insert(newStreetIds.at(streetId));
+          }
+          pStreet->setForbiddenTurns(newForbiddenTurns);
+        });
     for (const auto& [nodeId, node] : m_nodes) {
       // This is probably not the best way to do this
       if (node->isIntersection()) {
@@ -264,10 +277,9 @@
           [this, &pair, &outNeighbours, &maxPriority](auto const& inNodeId) {
             auto const& pInStreet{m_edges.at(inNodeId * m_nodes.size() + pair.first)};
             auto const nLanes{pInStreet->nLanes()};
             if (nLanes == 1) {
               return;
             }
-            // auto const& laneMapping{pInStreet->laneMapping()};
             std::multiset<Direction> allowedTurns;
             std::for_each(
                 outNeighbours.cbegin(),
@@ -276,14 +288,17 @@
                     auto const& outNodeId) {
                   auto const& pOutStreet{
                       m_edges.at(pair.first * m_nodes.size() + outNodeId)};
-                  if (pOutStreet->target() == pInStreet->source()) {
+                  if (pOutStreet->target() == pInStreet->source() ||
+                      pInStreet->forbiddenTurns().contains(pOutStreet->id())) {
                     return;
                   }
                   auto const deltaAngle{pOutStreet->deltaAngle(pInStreet->angle())};
                   auto const& outOppositeStreet{this->street(pair.first, outNodeId)};
+                  if (!outOppositeStreet) {
+                    return;
+                  }
                   // Actually going straight means remain on the same road, thus...
-                  if (outOppositeStreet &&
-                      ((pInStreet->priority() == maxPriority) ==
+                  if (((pInStreet->priority() == maxPriority) ==
                        (outOppositeStreet->get()->priority() == maxPriority)) &&
                       !allowedTurns.contains(Direction::STRAIGHT)) {
                     Logger::debug(
@@ -298,43 +313,29 @@
                     // allowedTurns.emplace(Direction::STRAIGHT);
                     // return;
                     // }
-                  } else if (std::abs(deltaAngle) < 5 * std::numbers::pi / 6) {
+                  } else if (std::abs(deltaAngle) < std::numbers::pi) {
                     // Logger::debug(std::format("Angle in {} - angle out {}",
                     //                           pInStreet->angle(),
                     //                           pOutStreet->angle()));
                     // Logger::debug(std::format("Delta: {}", deltaAngle));
-                    if (deltaAngle < -std::numbers::pi / 6.) {
-                      Logger::debug(
-                          std::format("Street {} can turn RIGHT", pInStreet->id()));
+                    if (std::abs(deltaAngle) < std::numbers::pi / 8) {
+                      Logger::debug(std::format("Street {} -> {} can turn STRAIGHT",
+                                                pInStreet->source(),
+                                                pInStreet->target()));
+                      allowedTurns.emplace(Direction::STRAIGHT);
+                    } else if (deltaAngle < 0.) {
+                      Logger::debug(std::format("Street {} -> {} can turn RIGHT",
+                                                pInStreet->source(),
+                                                pInStreet->target()));
                       allowedTurns.emplace(Direction::RIGHT);
-                    } else if (deltaAngle > std::numbers::pi / 6.) {
-                      Logger::debug(
-                          std::format("Street {} can turn LEFT", pInStreet->id()));
+                    } else if (deltaAngle > 0.) {
+                      Logger::debug(std::format("Street {} -> {} can turn LEFT",
+                                                pInStreet->source(),
+                                                pInStreet->target()));
                       allowedTurns.emplace(Direction::LEFT);
-                    } else {
-                      Logger::debug(
-                          std::format("Street {} can go STRAIGHT", pInStreet->id()));
-                      if (!allowedTurns.contains(Direction::STRAIGHT)) {
-                        allowedTurns.emplace(Direction::STRAIGHT);
-                      } else if (deltaAngle > 0.) {
-                        allowedTurns.emplace(Direction::LEFT);
-                      } else {
-                        allowedTurns.emplace(Direction::RIGHT);
-                      }
                     }
                   }
                 });
-            // if (!allowedTurns.contains(Direction::RIGHT)) {
-            //   std::multiset<Direction> updatedTurns;
-            //   for (auto turn : allowedTurns) {
-            //     if (turn > Direction::RIGHT && turn <= Direction::UTURN) {
-            //       updatedTurns.insert(static_cast<Direction>(turn - 1));
-            //     } else {
-            //       updatedTurns.insert(turn);
-            //     }
-            //   }
-            //   allowedTurns = std::move(updatedTurns);  // Replace with the updated set
-            // }
             while (allowedTurns.size() < static_cast<size_t>(nLanes)) {
               if (allowedTurns.contains(Direction::STRAIGHT)) {
                 allowedTurns.emplace(Direction::STRAIGHT);
@@ -347,11 +348,36 @@
               }
             }
             // If allowedTurns contains all RIGHT, STRAIGHT and LEFT, transform RIGHT into RIGHTANDSTRAIGHT
-            if (allowedTurns.contains(Direction::STRAIGHT) &&
-                allowedTurns.contains(Direction::RIGHT) &&
-                allowedTurns.contains(Direction::LEFT)) {
-              allowedTurns.erase(Direction::RIGHT);
-              allowedTurns.emplace(Direction::RIGHTANDSTRAIGHT);
+            if (allowedTurns.size() > static_cast<size_t>(nLanes)) {
+              if (pair.second->isTrafficLight()) {
+                auto& tl = dynamic_cast<TrafficLight&>(*pair.second);
+                auto const& cycles{tl.cycles()};
+                if (cycles.contains(pInStreet->id())) {
+                  if (cycles.size() == static_cast<size_t>(nLanes)) {
+                    // Replace with the traffic light cycles
+                    Logger::debug(
+                        std::format("Using traffic light {} cycles for street {} -> {}",
+                                    tl.id(),
+                                    pInStreet->source(),
+                                    pInStreet->target()));
+                    auto const& cycle{cycles.at(pInStreet->id())};
+                    allowedTurns.clear();
+                    for (auto const& [direction, cycle] : cycle) {
+                      allowedTurns.emplace(direction);
+                    }
+                  } else if (cycles.at(pInStreet->id())
+                                 .contains(Direction::LEFTANDSTRAIGHT)) {
+                    allowedTurns.erase(Direction::LEFT);
+                    allowedTurns.erase(Direction::STRAIGHT);
+                    allowedTurns.emplace(Direction::LEFTANDSTRAIGHT);
+                  } else if (cycles.at(pInStreet->id())
+                                 .contains(Direction::RIGHTANDSTRAIGHT)) {
+                    allowedTurns.erase(Direction::RIGHT);
+                    allowedTurns.erase(Direction::STRAIGHT);
+                    allowedTurns.emplace(Direction::RIGHTANDSTRAIGHT);
+                  }
+                }
+              }
             }
             switch (nLanes) {
               case 1:
@@ -361,11 +387,44 @@
                 if (allowedTurns.contains(Direction::STRAIGHT) &&
                     allowedTurns.contains(Direction::RIGHT) &&
                     allowedTurns.contains(Direction::LEFT)) {
-                  break;
+                  if (pair.second->isTrafficLight()) {
+                    auto& tl = dynamic_cast<TrafficLight&>(*pair.second);
+                    auto const& cycles{tl.cycles()};
+                    if (cycles.contains(pInStreet->id())) {
+                      auto const& cycle{cycles.at(pInStreet->id())};
+                      if (cycle.contains(Direction::LEFTANDSTRAIGHT) &&
+                          cycle.contains(Direction::RIGHT)) {
+                        allowedTurns.erase(Direction::LEFT);
+                        allowedTurns.erase(Direction::STRAIGHT);
+                        allowedTurns.emplace(Direction::LEFTANDSTRAIGHT);
+                        break;
+                      }
+                    }
+                  }
+                  allowedTurns.clear();
+                  allowedTurns.emplace(Direction::RIGHTANDSTRAIGHT);
+                  allowedTurns.emplace(Direction::LEFT);
+                }
+                if (allowedTurns.size() > 2) {
+                  // Remove duplicates
+                  std::set<Direction> uniqueDirections;
+                  std::copy(allowedTurns.begin(),
+                            allowedTurns.end(),
+                            std::inserter(uniqueDirections, uniqueDirections.begin()));
+                  allowedTurns.clear();
+                  std::copy(uniqueDirections.begin(),
+                            uniqueDirections.end(),
+                            std::inserter(allowedTurns, allowedTurns.begin()));
                 }
                 [[fallthrough]];
               default:
                 assert(allowedTurns.size() == static_cast<size_t>(nLanes));
+                // Logger::info(
+                //     std::format("Street {}->{} with {} lanes and {} allowed turns",
+                //                 pInStreet->source(),
+                //                 pInStreet->target(),
+                //                 nLanes,
+                //                 allowedTurns.size()));
                 std::vector<Direction> newMapping(nLanes);
                 auto it{allowedTurns.cbegin()};
                 for (size_t i{0}; i < allowedTurns.size(); ++i, ++it) {