Breadth First Search - Java

Question

I have as a school excersice to implement breadth first search in java. I have implemented almost everything but the problem is that my search is not working and I cant find the problem :( So Im asking you to advice me and give me some guidlines on where the eventual problem could be.

public ArrayList<SearchNode> search(Problem p) {
        // The frontier is a queue of expanded SearchNodes not processed yet
        frontier = new NodeQueue();
        /// The explored set is a set of nodes that have been processed 
        explored = new HashSet<SearchNode>();
        // The start state is given
        GridPos startState = (GridPos) p.getInitialState();
        // Initialize the frontier with the start state  
        frontier.addNodeToFront(new SearchNode(startState));

        // Path will be empty until we find the goal.
        path = new ArrayList<SearchNode>();

        // The start NODE

        SearchNode node = new SearchNode(startState); 

        // Check if startState = GoalState??
        if(p.isGoalState(startState)){
           path.add(new SearchNode(startState)); 
           return path; 
        }


        do {  

          node = frontier.removeFirst();
          explored.add(node); 

          ArrayList reachable = new ArrayList<GridPos>();
          reachable = p.getReachableStatesFrom(node.getState()); 

          SearchNode child; 

          for(int i = 0; i< reachable.size(); i++){

              child = new SearchNode((GridPos)reachable.get(i)); 

              if(!(explored.contains(child) || frontier.contains(child))){
                  if(p.isGoalState(child.getState())){
                      path = child.getPathFromRoot() ;  
                      return path; 
                  }
                  frontier.addNodeToFront(child); 
              }

          }
        }while(!frontier.isEmpty());


        return path;
    }

Thank you

Answer 1

frontier.addNodeToFront(child);

assuming the rest of your code (getReachableStatesFrom(), etc) is correct, adding elements to the front of your queue will cause your code to execute as a depth first search.