Fix possible buffer overflow
Add initial BFS routing