Add autoMapStreetLanes function to RoadNetwork class

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

src/dsm/headers/RoadDynamics.hpp

@@ -1556,7 +1556,7 @@ namespace dsm {
           continue;
         }
         auto& spire = dynamic_cast<SpireStreet&>(*pStreet);
-        file << separator << spire.code();
+        file << separator << streetId;
       }
       file << std::endl;
     }

src/dsm/headers/RoadNetwork.hpp

@@ -85,6 +85,9 @@ namespace dsm {
     /// Street priorities are assigned considering the number of lanes and the speed limit.
     /// Traffic Lights with an input degree lower than 3 are converted to standard intersections.
     void initTrafficLights();
+    /// @brief Automatically re-maps street lanes basing on network's topology
+    /// @details For example, if one street has the right turn forbidden, then the right lane becomes a straight one
+    void autoMapStreetLanes();
 
     /// @brief Import the graph's adjacency matrix from a file.
     /// If the file is not of a supported format, it will read the file as a matrix with the first two elements being

src/dsm/sources/RoadNetwork.cpp

@@ -239,6 +239,77 @@
       });
     }
   }
+  void RoadNetwork::autoMapStreetLanes() {
+    std::for_each(m_nodes.cbegin(), m_nodes.cend(), [this](auto const& pair) {
+      auto const& inNeighbours{m_adjacencyMatrix.getCol(pair.first)};
+      auto const& outNeighbours{m_adjacencyMatrix.getRow(pair.first)};
+      std::for_each(
+          inNeighbours.cbegin(),
+          inNeighbours.cend(),
+          [this, &pair, &outNeighbours](auto const& inNodeId) {
+            auto const& pInStreet{m_edges.at(inNodeId * m_nodes.size() + pair.first)};
+            // auto const& laneMapping{pInStreet->laneMapping()};
+            std::multiset<Direction> allowedTurns;
+            std::for_each(
+                outNeighbours.cbegin(),
+                outNeighbours.cend(),
+                [this, &pair, &pInStreet, &allowedTurns](auto const& outNodeId) {
+                  auto const& pOutStreet{
+                      m_edges.at(pair.first * m_nodes.size() + outNodeId)};
+                  auto const deltaAngle{pOutStreet->deltaAngle(pInStreet->angle())};
+                  // Logger::debug(std::format("Angle in {} - angle out {}", pInStreet->angle(), pOutStreet->angle()));
+                  // Logger::debug(std::format("Delta: {}", deltaAngle));
+                  if (std::abs(deltaAngle) < 5 * std::numbers::pi / 6) {
+                    if (deltaAngle < -std::numbers::pi / 6.) {
+                      Logger::debug(
+                          std::format("Street {} can turn RIGHT", pInStreet->id()));
+                      allowedTurns.emplace(Direction::RIGHT);
+                    } else if (deltaAngle > std::numbers::pi / 6.) {
+                      Logger::debug(
+                          std::format("Street {} can turn LEFT", pInStreet->id()));
+                      allowedTurns.emplace(Direction::LEFT);
+                    } else {
+                      Logger::debug(
+                          std::format("Street {} can go STRAIGHT", pInStreet->id()));
+                      allowedTurns.emplace(Direction::STRAIGHT);
+                    }
+                  }
+                });
+            auto const nLanes{pInStreet->nLanes()};
+            while (allowedTurns.size() < static_cast<size_t>(nLanes)) {
+              if (allowedTurns.contains(Direction::STRAIGHT)) {
+                allowedTurns.emplace(Direction::STRAIGHT);
+              } else if (allowedTurns.contains(Direction::RIGHT)) {
+                allowedTurns.emplace(Direction::RIGHT);
+              } else if (allowedTurns.contains(Direction::LEFT)) {
+                allowedTurns.emplace(Direction::LEFT);
+              } else {
+                allowedTurns.emplace(Direction::ANY);
+              }
+            }
+            switch (nLanes) {
+              case 1:
+                // Leaving Direction::ANY for one lane streets is the less painful option
+                break;
+              case 2:
+                if (allowedTurns.contains(Direction::STRAIGHT) &&
+                    allowedTurns.contains(Direction::RIGHT) &&
+                    allowedTurns.contains(Direction::LEFT)) {
+                  break;
+                }
+                [[fallthrough]];
+              default:
+                assert(allowedTurns.size() == static_cast<size_t>(nLanes));
+                std::vector<Direction> newMapping(nLanes);
+                auto it{allowedTurns.cbegin()};
+                for (size_t i{0}; i < allowedTurns.size(); ++i, ++it) {
+                  newMapping[i] = *it;
+                }
+                pInStreet->setLaneMapping(newMapping);
+            }
+          });
+    });
+  }
 
   void RoadNetwork::buildAdj() {
     // find max values in streets node pairs

src/dsm/sources/Street.cpp

@@ -53,6 +53,13 @@
   void Street::setLaneMapping(std::vector<Direction> const& laneMapping) {
     assert(laneMapping.size() == static_cast<size_t>(m_nLanes));
     m_laneMapping = laneMapping;
+    std::string strLaneMapping;
+    std::for_each(
+        laneMapping.cbegin(), laneMapping.cend(), [&strLaneMapping](auto const item) {
+          strLaneMapping += std::format("{} - ", static_cast<int>(item));
+        });
+    Logger::debug(
+        std::format("New lane mapping for street {} is: {}", m_id, strLaneMapping));
   }
   void Street::setQueue(dsm::queue<std::unique_ptr<Agent>> queue, size_t index) {
     assert(index < m_exitQueues.size());

test/Test_graph.cpp

@@ -95,7 +95,49 @@
     CHECK(graph.adjacencyMatrix().contains(0, 2));
     CHECK_FALSE(graph.adjacencyMatrix().contains(1, 3));
   }
-
+  SUBCASE("autoMapStreetLanes") {
+    GIVEN("A Graph object") {
+      RoadNetwork graph{};
+      graph.addNode(0, std::make_pair(0., 0.));
+      graph.addNode(1, std::make_pair(0., -1.));
+      graph.addNode(2, std::make_pair(-1., 0.));
+      graph.addNode(3, std::make_pair(1., 1.));
+      graph.addEdge<Street>(1, std::make_pair(0, 1), 1., 1., 3);
+      graph.addEdge<Street>(4, std::make_pair(1, 0), 1., 1., 3);
+      graph.addEdge<Street>(2, std::make_pair(0, 2), 1.);
+      graph.addEdge<Street>(8, std::make_pair(2, 0), 1.);
+      graph.addEdge<Street>(3, std::make_pair(0, 3), 1., 1., 2);
+      graph.addEdge<Street>(12, std::make_pair(3, 0), 1., 1., 2);
+      graph.buildAdj();
+      CHECK_EQ(graph.nEdges(), 6);
+      CHECK_EQ(graph.nNodes(), 4);
+      WHEN("We automatically map street lanes") {
+        // dsm::Logger::setLogLevel(dsm::log_level_t::DEBUG);
+        graph.autoMapStreetLanes();
+        // dsm::Logger::setLogLevel(dsm::log_level_t::INFO);
+        THEN("The lanes are correctly mapped") {
+          CHECK_EQ(graph.edge(1)->laneMapping().size(), 3);
+          CHECK_EQ(graph.edge(1)->laneMapping()[0], dsm::Direction::ANY);
+          CHECK_EQ(graph.edge(1)->laneMapping()[1], dsm::Direction::ANY);
+          CHECK_EQ(graph.edge(1)->laneMapping()[2], dsm::Direction::ANY);
+          CHECK_EQ(graph.edge(4)->laneMapping().size(), 3);
+          CHECK_EQ(graph.edge(4)->laneMapping()[0], dsm::Direction::RIGHT);
+          CHECK_EQ(graph.edge(4)->laneMapping()[1], dsm::Direction::RIGHT);
+          CHECK_EQ(graph.edge(4)->laneMapping()[2], dsm::Direction::LEFT);
+          CHECK_EQ(graph.edge(2)->laneMapping().size(), 1);
+          CHECK_EQ(graph.edge(2)->laneMapping()[0], dsm::Direction::ANY);
+          CHECK_EQ(graph.edge(8)->laneMapping().size(), 1);
+          CHECK_EQ(graph.edge(8)->laneMapping()[0], dsm::Direction::ANY);
+          CHECK_EQ(graph.edge(3)->laneMapping().size(), 2);
+          CHECK_EQ(graph.edge(3)->laneMapping()[0], dsm::Direction::ANY);
+          CHECK_EQ(graph.edge(3)->laneMapping()[1], dsm::Direction::ANY);
+          CHECK_EQ(graph.edge(12)->laneMapping().size(), 2);
+          CHECK_EQ(graph.edge(12)->laneMapping()[0], dsm::Direction::RIGHT);
+          CHECK_EQ(graph.edge(12)->laneMapping()[1], dsm::Direction::LEFT);
+        }
+      }
+    }
+  }
   SUBCASE("importMatrix - dsm") {
     // This tests the importMatrix function over .dsm files
     // GIVEN: a graph