A* – AStar

	private Function<Node, Double> heuristic;
	private Function<Node, Double> cost;

	public ASTAR(Function<Node, Double> heuristic, Function<Node, Double> cost) {
		this.heuristic = heuristic;
		this.cost = cost;
	}

	//A class to store heuristic and costs and also the possibility to compare each other.
	private class CostPair implements Comparable<CostPair> {
		private double heuristic;
		private double cost;

		private CostPair(double heuristic, double cost) {
			this.heuristic = heuristic;
			this.cost = cost;
		}

		@Override
		public int compareTo(CostPair other) {
			return Double.compare(heuristic + cost, other.heuristic + other.cost);
		}
	}

	public Node search(Node start, Predicate<Node> endPredicate) {
		// Check if the start node is the endPredicate
		if (endPredicate.test(start))
			return start;
		
		// The queue containing all the nodes we have to search for
		StablePriorityQueue<CostPair, Node> nodeQueue = new StablePriorityQueue<>();
		// Add the start node as the head. The priority does not matter since we
		// remove it anyway.
		nodeQueue.add(new Pair<>(new CostPair(heuristic.apply(start), cost.apply(start)), start));
		// Adjacent, definition from the handbook.pdf contain all neighbors.
		// That means, that we would search in a node which we already searched for at another position.
		// Because of the insertion and lookup time O(1) in both situations, we chose the HashSt.
		HashSet<Node> checkedNodes = new HashSet<>();

		// Repeat till we have no element in the queue left.
		while (nodeQueue.size() > 0) {
			Pair<CostPair, Node> currentNode = nodeQueue.poll();
			if (endPredicate.test(currentNode.s))
				return currentNode.s;
			// Nothing found. Add all it's children
			currentNode.s.adjacent().stream().filter(node -> !checkedNodes.contains(node))
					.forEach(node -> {
						CostPair pair = new CostPair(heuristic.apply(node), currentNode.f.cost + cost.apply(node));
						nodeQueue.add(new Pair<>(pair, node));
						checkedNodes.add(currentNode.s); //Add the node to the hashset, so that we do not search for that again.
					});
		}

		// We cant reach that state, but just in case return null.
		return null;
	}

private Function<Node, Double> heuristic;

private Function<Node, Double> cost;

public ASTAR(Function<Node, Double> heuristic, Function<Node, Double> cost) {

this.heuristic = heuristic;

this.cost = cost;

}

//A class to store heuristic and costs and also the possibility to compare each other.

private class CostPair implements Comparable<CostPair> {

private double heuristic;

private double cost;

private CostPair(double heuristic, double cost) {

this.heuristic = heuristic;

this.cost = cost;

}

@Override

public int compareTo(CostPair other) {

return Double.compare(heuristic + cost, other.heuristic + other.cost);

}

public Node search(Node start, Predicate<Node> endPredicate) {

// Check if the start node is the endPredicate

if (endPredicate.test(start))

return start;

// The queue containing all the nodes we have to search for

StablePriorityQueue<CostPair, Node> nodeQueue = new StablePriorityQueue<>();

// Add the start node as the head. The priority does not matter since we

// remove it anyway.

nodeQueue.add(new Pair<>(new CostPair(heuristic.apply(start), cost.apply(start)), start));

// Adjacent, definition from the handbook.pdf contain all neighbors.

// That means, that we would search in a node which we already searched for at another position.

// Because of the insertion and lookup time O(1) in both situations, we chose the HashSt.

HashSet<Node> checkedNodes = new HashSet<>();

// Repeat till we have no element in the queue left.

while (nodeQueue.size() > 0) {

Pair<CostPair, Node> currentNode = nodeQueue.poll();

if (endPredicate.test(currentNode.s))

return currentNode.s;

// Nothing found. Add all it's children

currentNode.s.adjacent().stream().filter(node -> !checkedNodes.contains(node))

.forEach(node -> {

CostPair pair = new CostPair(heuristic.apply(node), currentNode.f.cost + cost.apply(node));

nodeQueue.add(new Pair<>(pair, node));

checkedNodes.add(currentNode.s); //Add the node to the hashset, so that we do not search for that again.

});

}

// We cant reach that state, but just in case return null.

return null;

}