content: package aima.core.search.framework.qsearch;
  
  import aima.core.search.framework.Node;
  import aima.core.search.framework.NodeFactory;
  import aima.core.search.framework.problem.BidirectionalProblem;
  import aima.core.search.framework.problem.Problem;
  import aima.core.util.Tasks;
  
  import java.util.*;
  
  /**
   * Artificial Intelligence A Modern Approach (3rd Edition): page 90.<br>
   * <br>
   * Bidirectional search.<br>
   * <br>
   * The strategy of this search implementation is inspired by the description of
   * the bidirectional search algorithm i.e. 'Bidirectional search is implemented
   * by replacing the goal test with a check to see whether the frontiers of the
   * two searches intersect;'. But to gain some worst-case guarantees with respect
   * to solution quality (see below), the goal test of the original and the
   * reverse problem are replaced by a check, whether the node's state was already
   * explored in the other problem. Only one frontier is used which allows to use
   * the same queue search interface as known from other search implementations.
   * This implementation can be combined with many abstractions of search, e.g.
   * BreadthFirstSearch, UniformCostSearch, or even AStarSearch.
   *
   * @param <S> The type used to represent states
   * @param <A> The type of the actions to be used to navigate through the state space
   *
   * @author Ruediger Lunde
   */
  public class BidirectionalSearch<S, A> extends QueueSearch<S, A> {
  
  	private Queue<Node<S, A>> frontier;
  	private final static int ORG_P_IDX = 0;
  	private final static int REV_P_IDX = 1;
  
  	/**
  	 * Controls whether all actions of the reverse problem are tested to be
  	 * reversible. This shouldn't be necessary for a correctly implemented
  	 * bidirectional problem. But in case this is not guaranteed, the test is
  	 * helpful to avoid failures.
  	 */
  	private boolean isReverseActionTestEnabled = true;
  
  	// index 0: original problem, index 1: reverse problem
  	private final List<Map<S, ExtendedNode<S, A>>> explored;
  	private ExtendedNode<S, A> goalStateNode;
  
  	public BidirectionalSearch() {
  		this(new NodeFactory<>());
  	}
  
  	public BidirectionalSearch(NodeFactory<S, A> nodeFactory) {
  		super(nodeFactory);
  		explored = new ArrayList<>(2);
  		explored.add(new HashMap<>());
  		explored.add(new HashMap<>());
  	}
  
  	/**
  	 * Implements an approximation algorithm for bidirectional problems with
  	 * exactly one initial and one goal state. The algorithm guarantees the
  	 * following: If the queue is ordered by path costs (uniform cost search),
  	 * the path costs of the solution will be less or equal to the costs of the
  	 * best solution multiplied with two. Especially, if all step costs are
  	 * equal and the reverse problem provides reverse actions for all actions of
  	 * the original problem, the path costs of the result will exceed the
  	 * optimal path by the costs of one step at maximum.
  	 * 
  	 * @param problem
  	 *            a bidirectional search problem
  	 * @param frontier
  	 *            the data structure to be used to decide which node to be
  	 *            expanded next
  	 * 
  	 * @return a list of actions to the goal if the goal was found, a list
  	 *         containing a single NoOp Action if already at the goal, or an
  	 *         empty list if the goal could not be found.
  	 */
  	public Optional<Node<S, A>> findNode(Problem<S, A> problem, Queue<Node<S, A>> frontier) {
  		assert (problem instanceof BidirectionalProblem);
  
  		nodeFactory.useParentLinks(true); // bidirectional search needs parents!
  		this.frontier = frontier;
  		clearMetrics();
  		explored.get(ORG_P_IDX).clear();
  		explored.get(REV_P_IDX).clear();
  
  		Problem<S, A> orgP = ((BidirectionalProblem<S, A>) problem).getOriginalProblem();
  		Problem<S, A> revP = ((BidirectionalProblem<S, A>) problem).getReverseProblem();
  		ExtendedNode<S, A> initStateNode;
  		initStateNode = new ExtendedNode<>(nodeFactory.createNode(orgP.getInitialState()), ORG_P_IDX);
  		goalStateNode = new ExtendedNode<>(nodeFactory.createNode(revP.getInitialState()), REV_P_IDX);
  
  		if (orgP.getInitialState().equals(revP.getInitialState()))
  			return getSolution(orgP, initStateNode, goalStateNode);
  
  		// initialize the frontier using the initial state of the problem
  		addToFrontier(initStateNode);
  		addToFrontier(goalStateNode);
  
  		while (!isFrontierEmpty() && !Tasks.currIsCancelled()) {
  			// choose a leaf node and remove it from the frontier
  			ExtendedNode<S, A> node = (ExtendedNode<S, A>) removeFromFrontier();
  			ExtendedNode<S, A> nodeFromOtherProblem;
  
  			// if the node contains a goal state then return the corresponding solution
  			if (!earlyGoalTest && (nodeFromOtherProblem = getCorrespondingNodeFromOtherProblem(node)) != null)
  				return getSolution(orgP, node, nodeFromOtherProblem);
  
  			// expand the chosen node and add successor nodes to the frontier
  			for (Node<S, A> s : nodeFactory.getSuccessors(node, problem)) {
  				ExtendedNode<S, A> successor = new ExtendedNode<>(s, node.getProblemIndex());
  				if (!isReverseActionTestEnabled || node.getProblemIndex() == ORG_P_IDX
  						|| getReverseAction(orgP, successor) != null) {
  
  					if (earlyGoalTest
  							&& (nodeFromOtherProblem = getCorrespondingNodeFromOtherProblem(successor)) != null)
  						return getSolution(orgP, successor, nodeFromOtherProblem);
  
  					addToFrontier(successor);
  				}
  			}
  		}
  		// if the frontier is empty then return failure
  		return Optional.empty();
  	}
  
  	/**
  	 * Enables a check for all actions offered by the reverse problem whether
  	 * there exists a corresponding action of the original problem. Default
  	 * value is true.
  	 */
  	public void setReverseActionTestEnabled(boolean b) {
  		isReverseActionTestEnabled = b;
  	}
  
  	/**
  	 * Inserts the node at the tail of the frontier if the corresponding state
  	 * is not yet explored.
  	 */
  	protected void addToFrontier(Node<S, A> node) {
  		if (!isExplored(node)) {
  			frontier.add(node);
  			updateMetrics(frontier.size());
  		}
  	}
  
  	/**
  	 * Cleans up the head of the frontier, removes the first node of a
  	 * non-explored state from the head of the frontier, adds it to the
  	 * corresponding explored map, and returns the node.
  	 * 
  	 * @return A node of a not yet explored state.
  	 */
  	protected Node<S, A> removeFromFrontier() {
  		cleanUpFrontier(); // not really necessary because isFrontierEmpty should be called before...
  		Node<S, A> result = frontier.remove();
  		updateMetrics(frontier.size());
  		// add the node to the explored set of the corresponding problem
  		setExplored(result);
  		return result;
  	}
  
  	/**
  	 * Pops nodes of already explored states from the head of the frontier and
  	 * checks whether there are still some nodes left.
  	 */
  	protected boolean isFrontierEmpty() {
  		cleanUpFrontier();
  		updateMetrics(frontier.size());
  		return frontier.isEmpty();
  	}
  
  	/**
  	 * Helper method which removes nodes of already explored states from the
  	 * head of the frontier.
  	 */
  	private void cleanUpFrontier() {
  		while (!frontier.isEmpty() && isExplored(frontier.peek()))
  			frontier.remove();
  	}
  
  	/**
  	 * Computes a node whose sequence of recursive parents corresponds to a
  	 * sequence of actions which leads from the initial state of the original
  	 * problem to the state of node1 and then to the initial state of the
  	 * reverse problem, following reverse actions to parents of node2. Note that
  	 * both nodes must be linked to the same state. Success is not guaranteed if
  	 * some actions cannot be reversed.
  	 */
  	private Optional<Node<S, A>> getSolution(Problem<S, A> orgP, ExtendedNode<S, A> node1, ExtendedNode<S, A> node2) {
  		assert node1.getState().equals(node2.getState());
  		
  		Node<S, A> orgNode = node1.getProblemIndex() == ORG_P_IDX ? node1 : node2;
  		Node<S, A> revNode = node1.getProblemIndex() == REV_P_IDX ? node1 : node2;
  
  		while (revNode.getParent() != null) {
  			A action = getReverseAction(orgP, revNode);
  			if (action != null) {
  				S nextState = revNode.getParent().getState();
  				double stepCosts = orgP.getStepCosts(revNode.getState(), action, nextState);
  				orgNode = nodeFactory.createNode(nextState, orgNode, action, stepCosts);
  				revNode = revNode.getParent();
  			} else {
  				return Optional.empty();
  			}
  		}
  		metrics.set(METRIC_PATH_COST, orgNode.getPathCost());
  		return Optional.of(orgNode);
  	}
  
  	/**
  	 * Returns the action which leads from the state of <code>node</code> to the
  	 * state of the node's parent, if such an action exists in problem <code>orgP</code>.
  	 */
  	private A getReverseAction(Problem<S, A> orgP, Node<S, A> node) {
  		S currState = node.getState();
  		S nextState = node.getParent().getState();
  
  		for (A action : orgP.getActions(currState)) {
  			S aResult = orgP.getResult(currState, action);
  			if (nextState.equals(aResult))
  				return action;
  		}
  		return null;
  	}
  
  	private boolean isExplored(Node<S, A> node) {
  		ExtendedNode<S, A> eNode = (ExtendedNode<S, A>) node;
  		return explored.get(eNode.getProblemIndex()).containsKey(eNode.getState());
  	}
  
  	private void setExplored(Node<S, A> node) {
  		ExtendedNode<S, A> eNode = (ExtendedNode<S, A>) node;
  		explored.get(eNode.getProblemIndex()).put(eNode.getState(), eNode);
  	}
  
  	private ExtendedNode<S, A> getCorrespondingNodeFromOtherProblem(ExtendedNode<S, A> node) {
  		ExtendedNode<S, A> result = explored.get(1 - node.getProblemIndex()).get(node.getState());
  
  		// Caution: The goal test of the original problem should always include
  		// the root node of the reverse problem as that node might not yet have
  		// been explored yet. This is important if the reverse problem does not
  		// provide reverse actions for all original problem actions.
  		if (result == null && node.getProblemIndex() == ORG_P_IDX && node.getState() == goalStateNode.getState())
  			result = goalStateNode;
  		return result;
  	}
  
  	/**
  	 * Maintains the usual node data and additionally the index of the problem
  	 * to which the node belongs.
  	 * 
  	 * @author Ruediger Lunde
  	 *
  	 */
  	private static class ExtendedNode<S, A> extends Node<S, A> {
  
  		int problemIndex;
  
  		ExtendedNode(Node<S, A> node, int problemIndex) {
  			super(node.getState(), node.getParent(), node.getAction(), node.getPathCost());
  			this.problemIndex = problemIndex;
  		}
  
  		int getProblemIndex() {
  			return problemIndex;
  		}
  
  		@Override
  		public String toString() {
  			return "[" + getState() + ":" + problemIndex + "]";
  		}
  	}
  }
  