content: package aima.core.robotics;
  
  import java.util.Iterator;
  import java.util.LinkedHashSet;
  import java.util.Set;
  
  import aima.core.probability.bayes.approx.ParticleFiltering;
  import aima.core.probability.domain.FiniteIntegerDomain;
  import aima.core.probability.util.ProbUtil;
  import aima.core.probability.util.RandVar;
  import aima.core.robotics.datatypes.IMclMove;
  import aima.core.robotics.datatypes.IMclPose;
  import aima.core.robotics.datatypes.IMclRangeReading;
  import aima.core.robotics.datatypes.IMclVector;
  import aima.core.util.Randomizer;
  import aima.core.util.Util;
  
  /**
   * Artificial Intelligence A Modern Approach (3rd Edition): page 982.<br>
   * <br>
   * 
   * <pre>
   * function MONTE-CARLO-LOCALIZATION(a, z, N, P(X'|X, v, w), P(z|z*), m) returns a set of samples for the next time step
   * inputs: a, robot velocities v and w
   *         z, range scan z<sub>1</sub>,..., z<sub>M</sub>
   *         P(X'|X,v,w), motion model
   *         P(z|z*), range sensor noise model
   *         m, 2D map of the environment
   * persistent: S, a vector of samples of size N
   * local variables: W, a vector of weights of size N
   *                  S', a temporary vector of particles of size N
   * </pre>
   * 
   * Figure 25.9 A Monte-Carlo-Localization algorithm using a range-scan sensor model with independent noise.
   * The Monte-Carlo-Localization is an extension of a {@link ParticleFiltering} as stated on page 982.
   * This is true for the functionality but this implementation can not extend the implementation of the ParticleFiltering
   * as both implementations only contain the actual algorithm as a single method.
   * <br/><br/>
   * The update cycle of the algorithm is executed by the method {@code localize} for the given set of samples, move and vector of range readings.
   * Before calling this method, a set of samples can be generated through the method {@code generateCloud}, which represents the initialization phase of the pseudocode, for the given size N.
   * This removes the need of specifying the size N on every call of {@code localize} as this information is already contained in the set itself.
   * The method {@code localize} is divided into these two parts implemented each by a single method:
   * <ol>
   * <li>{@code applyMove} represents the first line of the update cycle. It moves all samples according to the move / motion model.</li>
   * <li>{@code weightSamples} represents the second to second last line of the update cycle. A vector of weights is created by this method for the given range scans by comparing every range scan to a ray cast with the correspondent sample through the range sensor noise model.</li>
   * </ol>
   * The WEIGHTED-SAMPLE-WITH-REPLACEMENT is implemented by the method {@code extendedWeightedSampleWithReplacement}. This implementation contains the addition of a cutoff value. All particles having a weight below this cutoff are ignored.
   * <br/><br/>
   * It is possible to reduce the steps needed for the localization by tweaking the  sample count and the parameter {@code cutOff}.
   * <br/><br/>
   * @author Arno von Borries
   * @author Jan Phillip Kretzschmar
   * @author Andreas Walscheid
   * 
   * @param <P> a pose implementing {@link IMclPose}.
   * @param <V> an n-1-dimensional vector implementing {@link IMclVector}, where n is the dimensionality of the environment. This vector describes the angle between two rays in the environment.
   * @param <M> a movement (or sequence of movements) of the robot implementing {@link IMclMove}. 
   * @param <R> a range measurement implementing {@link IMclRangeReading}.
   */
  public final class MonteCarloLocalization<P extends IMclPose<P,V,M>, V extends IMclVector, M extends IMclMove<M>, R extends IMclRangeReading<R,V>> {
  	
  	private static final String SAMPLE_INDEXES_NAME = "SAMPLE_INDEXES";
  	
  	private final IMclMap<P,V,M,R> map;
  	private final Randomizer randomizer;
  	
  	private RandVar sampleIndexes;
  	private double weightCutOff;
  	
  	/**
  	 * @param map an instance of a class implementing {@link IMclMap}.
  	 * @param randomizer a {@link Randomizer} that is used for re-sampling.
  	 */
  	public MonteCarloLocalization(IMclMap<P,V,M,R> map, Randomizer randomizer) {
  		this.map = map;
  		this.randomizer = randomizer;
  	}
  	
  	/**
  	 * Sets the minimum weight of the particles.
  	 * @param cutOff the minimum weight below which the corresponding particle gets removed during the {@code resample()} step. Set to zero when in doubt.
  	 */
  	public void setWeightCutOff(double cutOff) {
  		this.weightCutOff = cutOff;
  	}
  	
  	/**
  	 * Applies a move to the samples, creating a new {@link Set}.
  	 * @param samples the samples the move will be applied to.
  	 * @param move the move to be applied to the samples.
  	 * @return a new set of size N containing the moved samples.
  	 */
  	protected Set<P> applyMove(Set<P> samples, M move) {
  		Set<P> newSamples = new LinkedHashSet<P>();
  		for(P sample: samples) {
  			newSamples.add(sample.applyMovement(move.generateNoise()));
  		}
  		return newSamples;
  	}
  	
  	/**
  	 * Weights the samples by a given vector of range scans.
  	 * @param samples the samples to be weighted.
  	 * @param rangeReadings the vector containing all range scans.
  	 * @return a vector of weights of size N.
  	 */
  	protected double[] weightSamples(Set<P> samples, R[] rangeReadings) {
  		Iterator<P> samplesIterator = samples.iterator();
  		double[] w = new double[samples.size()];
  		for(int j=0;j<samples.size();j++) {
  			P sample = samplesIterator.next();
  			if(map.isPoseValid(sample)) {
  				w[j] = 1.0d;
  				for(int i=0;i<rangeReadings.length;i++) {
  					w[j] = w[j] * rangeReadings[i].calculateWeight(map.rayCast(sample.addAngle(rangeReadings[i].getAngle())));
  				}
  			} else {
  				w[j] = 0.0d;
  			}
  		}
  		return w;
  	}
  	
  	/**
  	 * Taken {@code weightedSampleWithReplacement} out of {@link ParticleFiltering} and extended by a minimum weight.
  	 * @param samples the samples to be re-sampled.
  	 * @param w the probability distribution on the samples.
  	 * @return the new set of samples.
  	 */
  	@SuppressWarnings("unchecked")
  	protected Set<P> extendedWeightedSampleWithReplacement(Set<P> samples, double[] w) {
  		int i = 0;
  		for(;i<samples.size();i++) {
  			if(w[i] > weightCutOff) break;
  		}
  		if(i >= samples.size()) return generateCloud(samples.size()); /*If all particleCloud are below weightCutOff, generate a new set of samples, as we are lost.*/
  		/*WEIGHTED-SAMPLE-WITH-REPLACEMENT:*/
  		double[] normalizedW = Util.normalize(w);
  		Set<P> newSamples = new LinkedHashSet<P>();
  		Object[] array = samples.toArray(new Object[0]);
  		for(i=0; i < samples.size(); i++) {
  			final int selectedSample = (Integer) ProbUtil.sample(randomizer.nextDouble(),sampleIndexes,normalizedW);
  			newSamples.add(((P) array[selectedSample]).clone());
  		}
  		return newSamples;
  	}
  	
  	/**
  	 * This method is the initialization phase of the algorithm. It has to be called to generate a set of samples of count N.
  	 * @param N the count of samples.
  	 * @return a set containing N samples.
  	 */
  	public Set<P> generateCloud(int N) {
  		Set<P>samples = new LinkedHashSet<P>();
  		Integer[] indexes = new Integer[N];
  		for(int i=0;i<N;i++) {
  			samples.add(map.randomPose());
  			indexes[i] = i;
  		}
  		sampleIndexes = new RandVar(SAMPLE_INDEXES_NAME, new FiniteIntegerDomain(indexes));
  		return samples;
  	}
  	
  	/**
  	 * Executes the update cycle of the Monte-Carlo-Localization for the given parameters.
  	 * @param samples the sample cloud.
  	 * @param move the move to be applied to the cloud.
  	 * @param rangeReadings the range scan that has been performed after the move has ended.
  	 * @return a new Set containing updated samples. {@code null} is returned if {@code samples} is {@code null}.
  	 */
  	public Set<P> localize(Set<P> samples, M move, R[] rangeReadings) {
  		if(samples == null) return null;/*initialization phase = call generateCloud*/
  		Set<P> newSamples = applyMove(samples, move);/*motion model*/
  		double[] w = weightSamples(newSamples, rangeReadings);/*range sensor noise model*/
  		newSamples = extendedWeightedSampleWithReplacement(newSamples, w);
  		return newSamples;
  	}
  }
  