Finished first version of BFS

With Printpath and everything