Add BFS implementation
Fix undirected BFS issue