Fix memory leak
working version
Fix bfs algo + output