Initial implementation of breath first search