Finished code

Code works, but needs to be optimized for DFS