Fixing self errors and debugging the collision test
Added the first search algorithm, doubling for BFS and DFS