Breadth first almost working.
Collision to be added.