public static GoalActionSequence findGoalRoute(Level l, Agent agent,
			Box box, Field agentFromField, Field agentToField,
			Field boxFromField, Field boxToField,
			Collection<Field> blockedFields) {
		dir boxDir = null;

		GoalSequenceNode root = new GoalSequenceNode(boxFromField,
				agentFromField, null);

		LinkedList<GoalSequenceNode> queue = new LinkedList<GoalSequenceNode>();

		// prune looped states (if agent and box ends up in a state already
		// explored)
		HashMap<Field, ArrayList<Field>> closedSet = new HashMap<Field, ArrayList<Field>>();

		// adding initial state to list set of explored states
		ArrayList<Field> tempList = new ArrayList<Field>();
		tempList.add(boxFromField);
		closedSet.put(agentFromField, tempList);

		// Add a closed set.
		queue.add(root);
		GoalSequenceNode currentNode = queue.poll();

		while (currentNode != null
				&& (currentNode.boxLocation != boxToField || currentNode.agentLocation != agentToField)) {

			boxDir = Agent.getBoxDirection(currentNode.agentLocation,
					currentNode.boxLocation);

			ArrayList<Command> foundCommands = addPossibleBoxCommandsForDirection(
					boxDir, currentNode.agentLocation, currentNode.boxLocation,
					blockedFields);

			for (Command command : foundCommands) {
				Field boxLocation = null;
				Field agentLocation = null;

				if (command.cmd.equals("Push")) {
					agentLocation = currentNode.boxLocation;
					boxLocation = currentNode.boxLocation.neighbors[command.dir2
							.ordinal()];
				} else {
					boxLocation = currentNode.agentLocation;
					agentLocation = currentNode.agentLocation.neighbors[command.dir1
							.ordinal()];
				}

				// Do we already have a way to get to this state?
				if (closedSet.containsKey(agentLocation)) {

					if (closedSet.get(agentLocation).contains(boxLocation)) {
						continue;

					} else { // the agent has been here before but without the
								// box in same location
						closedSet.get(agentLocation).add(boxLocation);
					}
				} else { // neither the agent or the box has been here before.
							// Update DS and create node in BTtree:
					ArrayList<Field> tempListe = new ArrayList<Field>();
					tempListe.add(boxLocation);
					closedSet.put(agentLocation, tempListe);
				}

				GoalSequenceNode node = new GoalSequenceNode(boxLocation,
						agentLocation, command);

				node.parent = currentNode;

				queue.add(node);

			}
			if (queue.isEmpty()) { // we have searched to the end without
									// finding a solution. Move boxes to get
									// access to goals
				return null;
			}
			currentNode = queue.poll();
		}

		GoalActionSequence returnSequence = new GoalActionSequence();
		returnSequence.setAgentLocation(currentNode.agentLocation);
		returnSequence.setBoxLocation(currentNode.boxLocation);

		ArrayList<Command> commands = new ArrayList<Command>();

		while (currentNode.parent != null) {
			commands.add(currentNode.action);
			currentNode = currentNode.parent;
		}

		Collections.reverse(commands);

		returnSequence.setCommands(commands);

		return returnSequence;
	}