A little refactoring and started implementing BFS. (Not ready yet)