graph: improvement according to code review

Author: rxdu

include/graph/impl/edge_impl.hpp

@@ -10,6 +10,8 @@
 #ifndef EDGE_IMPL_HPP
 #define EDGE_IMPL_HPP
 
+#include <iostream>
+
 namespace xmotion {
 
 template <typename State, typename Transition, typename StateIndexer>

include/graph/impl/vertex_impl.hpp

@@ -10,6 +10,8 @@
 #ifndef VERTEX_IMPL_HPP
 #define VERTEX_IMPL_HPP
 
+#include <iostream>
+
 namespace xmotion {
 
 template <typename State, typename Transition, typename StateIndexer>

include/graph/search/bfs.hpp

@@ -12,6 +12,8 @@
 #define BFS_HPP
 
 #include <limits>
+#include <queue>
+#include <unordered_set>
 #include "graph/search/search_algorithm.hpp"
 #include "graph/search/search_strategy.hpp"
 
@@ -182,11 +184,32 @@ class BFS final {
         auto start_it = graph->FindVertex(start);
         if (start_it == graph->vertex_end()) return false;
 
-        auto strategy = MakeBfsStrategy<State, Transition, StateIndexer>();
-        auto dummy_goal = graph->vertex_end();  // No specific goal - traverse all
+        // Manual BFS traversal
+        using VertexIteratorType = decltype(start_it);
+        std::queue<VertexIteratorType> q;
+        std::unordered_set<int64_t> visited;
+        
+        q.push(start_it);
+        visited.insert(start_it->vertex_id);
 
-        SearchAlgorithm<decltype(strategy), State, Transition, StateIndexer>
-            ::Search(graph, context, start_it, dummy_goal, strategy);
+        while (!q.empty()) {
+            auto current = q.front();
+            q.pop();
+            
+            // Mark as visited in context
+            auto& current_info = context.GetSearchInfo(current);
+            current_info.SetChecked(true);
+            
+            for (const auto& edge : current->edges_to) {
+                auto neighbor = edge.dst;
+                int64_t neighbor_id = neighbor->vertex_id;
+                
+                if (visited.find(neighbor_id) == visited.end()) {
+                    q.push(neighbor);
+                    visited.insert(neighbor_id);
+                }
+            }
+        }
 
         return true;
     }