/*
    * Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
    *
    * Licensed under the Apache License, Version 2.0 (the "License");
    * you may not use this file except in compliance with the License.
    * You may obtain a copy of the License at
    *
    *         http://www.apache.org/licenses/LICENSE-2.0
    *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  package com.irurueta.numerical.robust;
  
  import com.irurueta.numerical.LockedException;
  import com.irurueta.numerical.NotReadyException;
  import com.irurueta.sorting.Sorter;
  import com.irurueta.sorting.SortingException;
  
  import java.util.ArrayList;
  import java.util.Arrays;
  import java.util.BitSet;
  
  /**
   * This class implements PROMedS (PROgressive least Median Sample) algorithm
   * to robustly estimate a data model.
   * This algorithm is a mixture between LMedS and PROSAC, taking the best of
   * both.
   * Firstly, it has the advantage that no threshold is required to be set
   * beforehand, the same as LMedS. Threshold to determine inliers is computed
   * dynamically, which helps for an easier setup that is problem independent and
   * depending on the accuracy of the inliers, results will be more accurate than
   * RANSAC or PROSAC, just the same as LMedS.
   * On the other hand, if certain information about the quality of the samples
   * is available, as in PROSAC, the algorithm takes advantage of this additional
   * information to prioritize the samples with higher quality in order to find
   * a solution much faster than RANSAC or LMedS.
   * Finally, if by any chance a threshold to determine inliers is also used, the
   * algorithm will try to get the solution that better fits in a pure median of
   * residuals model or in a threshold based one to determine inliers.
   * Hence, PROMedS can be as fast as PROSAC (which is typically about 100x faster
   * than RANSAC or LMedS), can obtain the same accuracy as LMedS (which can be
   * much better than RANSAC or PROSAC in certain scenarios), and has an easier
   * setup, which is problem independent because no threshold is required to be
   * known beforehand although one can be provided as well.
   *
   * @param <T> type of object to be estimated.
   */
  @SuppressWarnings("Duplicates")
  public class PROMedSRobustEstimator<T> extends RobustEstimator<T> {
      /**
       * Constant defining default confidence of the estimated result, which is
       * 99%. This means that with a probability of 99% estimation will be
       * accurate because chosen sub-samples will be inliers.
       */
      public static final double DEFAULT_CONFIDENCE = 0.99;
  
      /**
       * Default maximum allowed number of iterations.
       */
      public static final int DEFAULT_MAX_ITERATIONS = 5000;
  
      /**
       * Minimum allowed confidence value.
       */
      public static final double MIN_CONFIDENCE = 0.0;
  
      /**
       * Maximum allowed confidence value.
       */
      public static final double MAX_CONFIDENCE = 1.0;
  
      /**
       * Minimum allowed number of iterations.
       */
      public static final int MIN_ITERATIONS = 1;
  
      /**
       * Indicates whether the algorithm must stop prematurely when dynamically
       * computed threshold using median of residuals has a value lower than
       * provided threshold in listener.
       * When this flag is enabled accuracy of PROMedS worsens to a lever similar
       * to PROSAC but the number of iterations is reduced (i.e. less
       * computational cost). If more accuracy is desired at the expense of some
       * additional computation cost, then disable this flag.
       * By default, stop threshold is enabled, so that computational cost is
       * similar to RANSAC and only accuracy gets better if inliers are more
       * accurate.
       */
      public static final boolean DEFAULT_STOP_THRESHOLD_ENABLED = true;
  
      /**
       * Default maximum allowed outliers proportion in the input data. This is
       * used do determine the number of required iterations.
       */
      public static final double DEFAULT_MAX_OUTLIERS_PROPORTION = 0.8;
 
     /**
      * Minimum allowed value for maximum allowed outliers proportion in the
      * input data.
      */
     public static final double MIN_MAX_OUTLIERS_PROPORTION = 0.0;
 
     /**
      * Maximum allowed value for maximum allowed outliers proportion in the
      * input data.
      */
     public static final double MAX_MAX_OUTLIERS_PROPORTION = 1.0;
 
     /**
      * Defines the default value for the maximum probability that a solution
      * with more than inliersNStar in U_nStar exist and was not found after k
      * samples.
      */
     public static final double DEFAULT_ETA0 = 0.05;
 
     /**
      * Minimum allowed value for eta0.
      */
     public static final double MIN_ETA0 = 0.0;
 
     /**
      * Maximum allowed value for eta0.
      */
     public static final double MAX_ETA0 = 1.0;
 
     /**
      * Defines the default value for beta, which is the probability that a
      * match is declared inlier by mistake, i.e. the ratio of the "inlier"
      * surface by the total surface. The inlier surface is a disc with radius
      * 1.96s for homography/displacement computation, or a band with width
      * 1.96*s*2 for epipolar geometry (s is the detection noise), and the total
      * surface is the surface of the image.
      */
     public static final double DEFAULT_BETA = 0.01;
 
     /**
      * Minimum allowed value for beta.
      */
     public static final double MIN_BETA = 0.0;
 
     /**
      * Maximum allowed value for beta.
      */
     public static final double MAX_BETA = 1.0;
 
     /**
      * Default factor to normalize threshold to determine inliers. This factor
      * can be used to increase or lower the dynamically computed threshold so
      * that the algorithm becomes more or less accurate. The stricter the
      * threshold (lower factor), the more time the algorithm will need to
      * converge, if it can converge. By default, the factor is 1.0, which makes
      * the threshold to be computed as the median of residuals.
      */
     public static final double DEFAULT_INLIER_FACTOR = 1.0; //1.5 would also be reasonable
 
     /**
      * Minimum allowed value for inlier factor.
      */
     public static final double MIN_INLER_FACTOR = 0.0;
 
     /**
      * Indicates whether the inlier threshold will be used to find inliers along
      * with their median of residuals.
      */
     public static final boolean DEFAULT_USE_INLIER_THRESHOLD = true;
 
     /**
      * Constant to estimate standard deviation of residuals based on their
      * median.
      */
     public static final double STD_CONSTANT = 1.4826;
 
     /**
      * Chi squared.
      */
     public static final double CHI_SQUARED = 2.706;
 
     /**
      * Amount of confidence expressed as a value between 0 and 1.0 (which is
      * equivalent to 100%). The amount of confidence indicates the probability
      * that the estimated result is correct. Usually this value will be close
      * to 1.0, but not exactly 1.0.
      */
     private double confidence;
 
     /**
      * Maximum allowed number of iterations. When the maximum number of
      * iterations is exceeded, result will not be available, however an
      * approximate result will be available for retrieval.
      */
     private int maxIterations;
 
     /**
      * In this implementation, PROSAC won't stop before having reached the
      * corresponding inliers rate on the complete data set.
      * Maximum allowed outliers proportion in the input data: used to compute
      * nIters (can be as high as 0.95).
      */
     private double maxOutliersProportion;
 
     /**
      * eta0 is the maximum probability that a solution with more than
      * inliersNStar inliers in U_nStar exists and was not found after k
      * samples (typically set to 5%).
      */
     private double eta0;
 
     /**
      * beta is the probability that a match is declared inlier by mistake,
      * i.e. the ratio of the "inlier" surface by the total surface. The
      * inlier surface is a disc with radius 1.96s for homography/displacement
      * computation, or a band with width 1.96s*2 for epipolar geometry (s is
      * the detection noise), and the total surface is the surface of the image
      * YOU MUST ADJUST THIS VALUE, DEPENDING ON YOUR PROBLEM!.
      */
     private double beta;
 
     /**
      * Instance in charge of picking random subsets of samples.
      */
     private SubsetSelector subsetSelector;
 
     /**
      * Number of iterations to be done to obtain required confidence.
      */
     private int iters;
 
     /**
      * Best solution that has been found so far during an estimation.
      */
     private T bestResult;
 
     /**
      * Data related to inliers found for best result.
      */
     private PROMedSInliersData bestInliersData;
 
     /**
      * Indicates whether the algorithm must stop prematurely when dynamically
      * computed threshold using median of residuals has a value lower than
      * provided threshold in listener.
      * When this flag is enabled accuracy of PROMedS worsens to a lever similar
      * to PROSAC but the number of iterations is reduced (i.e. less
      * computational cost). If more accuracy is desired at the expense of some
      * additional computation cost, then disable this flag.
      */
     private boolean stopThresholdEnabled;
 
     /**
      * Factor to normalize threshold to determine inliers. This factor can be
      * used to increase or lower the dynamically computed threshold so that the
      * algorithm becomes more or less accurate. The stricter the threshold
      * (lower factor), the more time the algorithm will need to converge, if
      * it can converge. By default, the factor is 1.0, which makes the threshold
      * to be computed as the median of residuals.
      */
     private double inlierFactor;
 
     /**
      * Flag indicating whether thresholds to determine inliers are used, or if
      * only median of residuals is used. When true, the algorithm will try
      * to fit the best model, otherwise only median of residuals will be used.
      */
     private boolean useInlierThresholds;
 
     /**
      * Constructor.
      */
     public PROMedSRobustEstimator() {
         super();
         confidence = DEFAULT_CONFIDENCE;
         maxIterations = DEFAULT_MAX_ITERATIONS;
         maxOutliersProportion = DEFAULT_MAX_OUTLIERS_PROPORTION;
         eta0 = DEFAULT_ETA0;
         beta = DEFAULT_BETA;
         iters = maxIterations;
         bestResult = null;
         bestInliersData = null;
         stopThresholdEnabled = DEFAULT_STOP_THRESHOLD_ENABLED;
         inlierFactor = DEFAULT_INLIER_FACTOR;
         useInlierThresholds = DEFAULT_USE_INLIER_THRESHOLD;
     }
 
     /**
      * Constructor with listener.
      *
      * @param listener listener to be notified of events such as when estimation
      *                 starts, ends or its progress significantly changes, as well as in charge
      *                 of picking samples and doing per-iteration estimations.
      */
     public PROMedSRobustEstimator(final PROMedSRobustEstimatorListener<T> listener) {
         super(listener);
         confidence = DEFAULT_CONFIDENCE;
         maxIterations = DEFAULT_MAX_ITERATIONS;
         maxOutliersProportion = DEFAULT_MAX_OUTLIERS_PROPORTION;
         eta0 = DEFAULT_ETA0;
         beta = DEFAULT_BETA;
         iters = maxIterations;
         bestResult = null;
         bestInliersData = null;
         stopThresholdEnabled = DEFAULT_STOP_THRESHOLD_ENABLED;
         inlierFactor = DEFAULT_INLIER_FACTOR;
         useInlierThresholds = DEFAULT_USE_INLIER_THRESHOLD;
     }
 
     /**
      * Returns amount of confidence expressed as a value between 0 and 1.0
      * (which is equivalent to 100%). The amount of confidence indicates the
      * probability that the estimated result is correct. Usually this value will
      * be close to 1.0, but not exactly 1.0.
      *
      * @return amount of confidence as a value between 0.0 and 1.0.
      */
     public double getConfidence() {
         return confidence;
     }
 
     /**
      * Sets amount of confidence expressed as a value between 0 and 1.0 (which
      * is equivalent to 100%). The amount of confidence indicates the
      * probability that the estimated result is correct. Usually this value will
      * be close to 1.0, but not exactly 1.0.
      *
      * @param confidence confidence to be set as a value between 0.0 and 1.0.
      * @throws IllegalArgumentException if provided value is not between 0.0 and
      *                                  1.0.
      * @throws LockedException          if this estimator is locked because an estimation
      *                                  is being computed.
      */
     public void setConfidence(final double confidence) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
             throw new IllegalArgumentException();
         }
         this.confidence = confidence;
     }
 
     /**
      * Maximum allowed number of iterations. When the maximum number of
      * iterations is exceeded, result will not be available, however an
      * approximate result will be available for retrieval.
      *
      * @return maximum allowed number of iterations.
      */
     public int getMaxIterations() {
         return maxIterations;
     }
 
     /**
      * Sets maximum allowed number of iterations. When the maximum number of
      * iterations is exceeded, result will not be available, however an
      * approximate result will be available for retrieval.
      *
      * @param maxIterations maximum allowed number of iterations to be set.
      * @throws IllegalArgumentException if provided value is less than 1.
      * @throws LockedException          if this estimator is locked because an estimation
      *                                  is being computed.
      */
     public void setMaxIterations(final int maxIterations) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         if (maxIterations < MIN_ITERATIONS) {
             throw new IllegalArgumentException();
         }
         this.maxIterations = maxIterations;
     }
 
     /**
      * Returns boolean indicating whether the algorithm must stop prematurely
      * when dynamically computed threshold using median of residuals has a value
      * lower than provided threshold in listener.
      * When this flag is enabled accuracy of PROMedS worsens to a lever similar
      * to PROSAC but the number of iterations is reduced (i.e. less
      * computational cost). If more accuracy is desired at the expense of some
      * additional computation cost, then disable this flag.
      *
      * @return true if stop threshold is enabled, false otherwise.
      */
     public boolean isStopThresholdEnabled() {
         return stopThresholdEnabled;
     }
 
     /**
      * Sets boolean indicating whether the algorithm must stop prematurely when
      * dynamically computed threshold using median of residuals has a value
      * lower than provided threshold in listener.
      * When this flag is enabled accuracy of PROMedS worsens to a lever similar
      * to PROSAC but the number of iterations is reduced (i.e. less
      * computational cost). If more accuracy is desired at the expense of some
      * additional computation cost, then disable this flag.
      *
      * @param stopThresholdEnabled true if stop threshold is enabled, false
      *                             otherwise.
      * @throws LockedException if this estimator is locked because an estimation
      *                         is being computed.
      */
     public void setStopThresholdEnabled(final boolean stopThresholdEnabled) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.stopThresholdEnabled = stopThresholdEnabled;
     }
 
     /**
      * Returns factor to normalize or adjust threshold to determine inliers.
      * This factor can be used to increase or lower the dynamically computed
      * threshold so that the algorithm becomes more or less accurate. The
      * stricter the threshold (lower factor), the more time the algorithm will
      * need to converge, if it can converge. By default, the factor is 1.0, which
      * makes the threshold to be computed as the median of residuals.
      *
      * @return factor to normalize threshold to determine inliers.
      */
     public double getInlierFactor() {
         return inlierFactor;
     }
 
     /**
      * Sets factor to normalize or adjust threshold to determine inliers.
      * This factor can be used to increase or lower the dynamically computed
      * threshold so that the algorithm becomes more or less accurate. The
      * stricter the threshold (lower factor), the more time the algorithm will
      * need to converge, if it can converge. By default, the factor is 1.0, which
      * makes the threshold to be computed as the median of residuals.
      *
      * @param inlierFactor inlier factor to be set.
      * @throws IllegalArgumentException if provided value is less or equal than
      *                                  0.0.
      * @throws LockedException          if this estimator is locked because an estimation
      *                                  is being computed.
      */
     public void setInlierFactor(final double inlierFactor) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         if (inlierFactor <= MIN_INLER_FACTOR) {
             throw new IllegalArgumentException();
         }
         this.inlierFactor = inlierFactor;
     }
 
     /**
      * Returns flag indicating whether thresholds to determine inliers are used,
      * or if only median of residuals is used. When true, the algorithm will try
      * to fit the best model, otherwise only median of residuals will be used.
      *
      * @return true if best model is used (threshold or median), otherwise only
      * median of residuals will be used.
      */
     public boolean isUseInlierThresholds() {
         return useInlierThresholds;
     }
 
     /**
      * Sets flag indicating whether thresholds to determine inliers are used, or
      * if only median of residuals is used. When true, the algorithm will try to
      * fit the best model, otherwise only median of residuals will be used.
      *
      * @param useInlierThresholds true if best model is used (threshold or
      *                            median), oitherwise only median of residuals will be used.
      * @throws LockedException if this estimator is locked because an estimation
      *                         is being computed.
      */
     public void setUseInlierThresholds(final boolean useInlierThresholds) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
 
         this.useInlierThresholds = useInlierThresholds;
     }
 
     /**
      * Returns maximum allowed outliers proportion in the input data. This is
      * used to compute number of iterations to be done (nIters). It typically
      * can be as high as 0.95. Higher values, up to 1 are possible but not
      * recommended.
      * In this implementation, PROSAC won't stop before having reached the
      * corresponding inliers rate on the complete data set.
      *
      * @return maximum allowed outliers proportion in the input data.
      */
     public double getMaxOutliersProportion() {
         return maxOutliersProportion;
     }
 
     /**
      * Sets maximum allowed outliers proportion in the input data. This is used
      * to compute number of iterations to be done (nIters). It typically can be
      * as high as 0.95. Higher values, up to 1 are possible but not recommended.
      * In this implementation, PROSAC won't stop before having reached the
      * corresponding inliers rate on the complete data set.
      *
      * @param maxOutliersProportion maximum allowed outliers proportion in the
      *                              input data.
      * @throws IllegalArgumentException if provided value is less than 0.0 or
      *                                  greater than 1.0.
      * @throws LockedException          if this estimator is locked because an estimation
      *                                  is being computed.
      */
     public void setMaxOutliersProportion(final double maxOutliersProportion) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         if (maxOutliersProportion < MIN_MAX_OUTLIERS_PROPORTION
                 || maxOutliersProportion > MAX_MAX_OUTLIERS_PROPORTION) {
             throw new IllegalArgumentException();
         }
 
         this.maxOutliersProportion = maxOutliersProportion;
     }
 
     /**
      * Return eta0, which is the maximum probability that a solution with more
      * than inliersNStar inliers in U_nStar exists and was not found after k
      * samples (typically set to 5%).
      *
      * @return eta0 value.
      */
     public double getEta0() {
         return eta0;
     }
 
     /**
      * Sets eta0, which is the maximum probability that a solution with more
      * than inliersNStar inliers in U_nStar exists and was not found after k
      * samples (typically set to 5%).
      *
      * @param eta0 eta0 value to be set.
      * @throws IllegalArgumentException if provided value is less than 0.0 or
      *                                  greater than 1.0.
      * @throws LockedException          if this estimator is locked because an estimation
      *                                  is being computed.
      */
     public void setEta0(final double eta0) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         if (eta0 < MIN_ETA0 || eta0 > MAX_ETA0) {
             throw new IllegalArgumentException();
         }
 
         this.eta0 = eta0;
     }
 
     /**
      * Returns beta, which is the probability that a match is declared inlier by
      * mistake, i.e. the ratio of the "inlier" surface by the total surface. The
      * inlier surface is a disc with radius 1.96s for homography/displacement
      * computation, or a band with width 1.96s*2 for epipolar geometry (s is
      * the detection noise), and the total surface is the surface of the image
      * YOU MUST ADJUST THIS VALUE, DEPENDING ON YOUR PROBLEM!
      *
      * @return beta value.
      */
     public double getBeta() {
         return beta;
     }
 
     /**
      * Sets beta, which is the probability that a match is declared inlier by
      * mistake, i.e. the ratio of the "inlier" surface by the total surface. The
      * inlier surface is a disc with radius 1.96s for homography/displacement
      * computation, or a band with width 1.96s*2 for epipolar geometry (s is
      * the detection noise), and the total surface is the surface of the image
      * YOU MUST ADJUST THIS VALUE, DEPENDING ON YOUR PROBLEM!
      *
      * @param beta beta value to be set.
      * @throws IllegalArgumentException if provided value is less than 0.0 or
      *                                  greater than 1.0.
      * @throws LockedException          if this estimator is locked because an estimation
      *                                  is being computed.
      */
     public void setBeta(final double beta) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         if (beta < MIN_BETA || beta > MAX_BETA) {
             throw new IllegalArgumentException();
         }
 
         this.beta = beta;
     }
 
     /**
      * Returns number of iterations to be done to obtain required confidence.
      * This does not need to be equal to the actual number of iterations the
      * algorithm finally required to obtain a solution.
      *
      * @return number of iterations to be done to obtain required confidence.
      */
     public int getNIters() {
         return iters;
     }
 
     /**
      * Returns best solution that has been found so far during an estimation.
      *
      * @return best solution that has been found so far during an estimation.
      */
     public T getBestResult() {
         return bestResult;
     }
 
     /**
      * Returns data related to inliers found for best result.
      *
      * @return data related to inliers found for best result.
      */
     protected PROMedSInliersData getBestInliersData() {
         return bestInliersData;
     }
 
     /**
      * Indicates if estimator is ready to start the estimation process.
      *
      * @return true if ready, false otherwise.
      */
     @Override
     public boolean isReady() {
         if (!super.isReady()) {
             return false;
         }
         return (listener instanceof PROMedSRobustEstimatorListener);
     }
 
     /**
      * Robustly estimates an instance of T.
      *
      * @return estimated object.
      * @throws LockedException          if robust estimator is locked.
      * @throws NotReadyException        if provided input data is not enough to start
      *                                  the estimation.
      * @throws RobustEstimatorException if estimation fails for any reason
      *                                  (i.e. numerical instability, no solution available, etc).
      */
     @Override
     public T estimate() throws LockedException, NotReadyException, RobustEstimatorException {
         if (isLocked()) {
             throw new LockedException();
         }
         if (!isReady()) {
             throw new NotReadyException();
         }
 
         try {
             var listener = (PROMedSRobustEstimatorListener<T>) this.listener;
 
             locked = true;
 
             listener.onEstimateStart(this);
 
             // N = CORRESPONDENCES
             final var totalSamples = listener.getTotalSamples();
             final var subsetSize = listener.getSubsetSize();
 
             final var qualityScores = listener.getQualityScores();
             // check for invalid quality scores length
             if (qualityScores.length != totalSamples) {
                 throw new RobustEstimatorException();
             }
 
             var inlierThreshold = 0.0;
             if (useInlierThresholds) {
                 inlierThreshold = listener.getThreshold();
             }
             // obtain indices referring to original samples position after sorting
             // quality scores in descending order
             final var sortedIndices = computeSortedQualityIndices(listener.getQualityScores());
 
             // reusable list that will contain preliminary solutions on each
             // iteration
             final var iterResults = new ArrayList<T>();
             bestResult = null;
             var previousProgress = 0.0f;
             float progress;
             // subset indices obtained from a subset selector
             final var subsetIndices = new int[subsetSize];
             final var residualsTemp = new double[totalSamples];
             // subset indices referred to the real samples positions after taking
             // into account the sorted indices obtained from quality scores
             final var transformedSubsetIndices = new int[subsetSize];
             // array containing inliers efficiently
             final var inliers = new BitSet(totalSamples);
 
             // T_N
             iters = Math.min(computeIterations(1.0 - maxOutliersProportion, subsetSize, confidence),
                     maxIterations);
 
             // termination length
             var sampleSizeStar = totalSamples;
             // number of inliers found within the first
             // nStar data points
             var inliersNStar = 0;
             // best number of inliers found so far
             // (store the model that goes with it)
             var inliersBest = -1;
             // threshold to stop algorithm
             var threshold = Double.MAX_VALUE;
             final var inliersMin = (int) ((1.0 - maxOutliersProportion) * totalSamples);
             // iteration number (t)
             var currentIter = 0;
             // (n) we draw samples from the set U_n
             // of the top n (sampleSize) data points
             var sampleSize = subsetSize;
             // average number of samples "{M_i}_{i=1}^{Tn}"
             // that contains samples from U_n only
             double tn = iters;
             // integer version of Tn
             var tnPrime = 1;
             // number of samples to draw to reach the
             // maximality constraint
             var kNStar = iters;
 
             // initialize Tn
             for (var i = 0; i < subsetSize; i++) {
                 tn *= (double) (sampleSize - i) / (double) (totalSamples - i);
             }
 
             if (subsetSelector == null) {
                 // create new subset selector
                 subsetSelector = SubsetSelector.create(totalSamples);
             } else {
                 // set number of samples to current subset selector
                 subsetSelector.setNumSamples(totalSamples);
             }
 
             // data related to inliers
             var inliersData = new PROMedSInliersData(totalSamples);
             // sorter to compute medians
             final var sorter = Sorter.<Double>create();
 
             // indicates if result improved
             boolean improved;
             var continueIteration = true;
 
             // iterate until the expected number of inliers or the estimated
             // number of iterations is reached
             while (continueIteration) {
                 if (kNStar > 0) {
                     progress = Math.min((float) currentIter / (float) kNStar, 1.0f);
                 } else {
                     progress = 1.0f;
                 }
                 if (progress - previousProgress > progressDelta) {
                     previousProgress = progress;
                     listener.onEstimateProgressChange(this, progress);
                 }
                 currentIter++;
 
                 // choice of the hypothesis generation set
 
                 // The growth function is defined as
                 // g(t) = min{n : TnPrime > t} where n is sampleSize
                 // Thus sampleSize should be incremented if currentIter > TnPrime
                 if ((currentIter > tnPrime) && (sampleSize < sampleSizeStar)) {
                     final var TnPlus1 = (tn * (sampleSize + 1)) / (sampleSize + 1 - subsetSize);
                     sampleSize++;
                     tnPrime += (int) Math.ceil(TnPlus1 - tn);
                     tn = TnPlus1;
                 }
 
                 // Draw semi-random sample
                 if (currentIter > tnPrime) {
                     // during the finishing stage (sampleSize == sampleSizeStar &&
                     // currentIter > TnPrime), draw a standard RANSAC sample
                     // The sample contains subsetSize points selected from U_n at
                     // random
                     subsetSelector.computeRandomSubsets(subsetSize, subsetIndices);
                 } else {
                     // The sample contains subsetSize-1 points selected from
                     // U_sampleSize_1 at random and u_sampleSize
 
                     subsetSelector.computeRandomSubsetsInRange(0, sampleSize, subsetSize, true,
                             subsetIndices);
                 }
 
                 transformIndices(subsetIndices, sortedIndices, transformedSubsetIndices);
 
                 // clear list of preliminary solutions before calling listener
                 iterResults.clear();
                 // compute solution for current iteration
                 listener.estimatePreliminarSolutions(transformedSubsetIndices, iterResults);
 
                 // total number of inliers for a
                 // given result
                 int inliersCurrent;
 
                 // iterate over all solutions that have been found
                 improved = false;
                 for (final var iterResult : iterResults) {
                     // compute inliers
                     computeInliers(iterResult, subsetSize, inlierFactor, useInlierThresholds, inlierThreshold,
                             residualsTemp, listener, sorter, inliersData);
                     inliersCurrent = inliersData.getNumInliers();
 
                     if (inliersData.isMedianResidualImproved()) {
                         improved = true;
 
                         // keep current solution
                         bestResult = iterResult;
 
                         // update estimated thresholds to be used as stop
                         // criterion
                         threshold = inliersData.getEstimatedThreshold();
                     }
 
                     if (inliersCurrent > inliersBest) {
                         // update best number of inliers
                         inliersBest = inliersCurrent;
                         // keep current solution
                         bestResult = iterResult;
 
                         keepInliersData(inliersData, totalSamples);
                         // create new instance after keeping inlier data
                         inliersData = inliersData.createCopy();
 
                         // select new termination length sampleSizeStar if possible
                         // only when a new sample is better than the others found
                         // so far
 
                         // best value found so far in terms of inliers ratio
                         var sampleSizeBest = totalSamples;
                         var inliersSampleSizeBest = inliersCurrent;
                         // test value for the termination length
                         int sampleSizeTest;
                         // number of inliers for that test value
                         int inliersSampleSizeTest;
                         var epsilonSampleSizeBest = (double) inliersSampleSizeBest / (double) sampleSizeBest;
 
                         for (sampleSizeTest = totalSamples, inliersSampleSizeTest = inliersCurrent;
                              sampleSizeTest > subsetSize; sampleSizeTest--) {
                             // Loop invariants:
                             // - inliersSampleSizeTest is the number of inliers
                             //   for the sampleSizeTest first correspondences
                             // - sampleSizeBest is the value between
                             //   sampleSizeTest+1 and totalSamples that maximizes
                             //   the ratio inliersSampleSizeBest/sampleSizeBest
 
                             // - Non-randomness: In >= imin(n*)
                             // - Maximality: the number of samples that were drawn
                             //   so far must be enough so that the probability of
                             //   having missed a set of inliers is below eta=0.01.
                             //   This is the classical RANSAC termination criterion,
                             //   except that it takes into account only the
                             //   sampleSize first samples (not the total number
                             //   of samples)
                             //   kNStar = log(eta0) / log(1 - (inliersNStar/
                             //   sampleSizeStar)^subsetSize
                             //   We have to minimize kNStar, e.g. maximize
                             //   inliersNStar/sampleSizeStar, a straightforward
                             //   implementation would use the following test:
                             //   if(inliersSampleSizeTest > epsilonSampleSizeBest *
                             //   sampleSizeTest){ ... blah blah blah
                             //   However, since In is binomial, and in the case of
                             //   evenly distributed inliers, a better test would be
                             //   to reduce sampleSizeStar only if there's a
                             //   significant improvement in epsilon. Thus, we use a
                             //   Chi-squared test (P=0.10), together with the normal
                             //   approximation to the binomial (mu =
                             //   epsilonSampleSizeStart * sampleSizeTest, sigma =
                             //   sqrt(sampleSizeTest * epsilonSampleSizeStar * (1 -
                             //   epsilonSampleSizeStar))).
                             //   There is a significant difference between the two
                             //   tests (e.g. with the computeInliers function
                             //   provided)
                             //   We do the cheap test first, and the expensive test
                             //   only if the cheap one passes
                             if ((inliersSampleSizeTest * sampleSizeBest > inliersSampleSizeBest * sampleSizeTest)
                                     && (inliersSampleSizeTest > epsilonSampleSizeBest * sampleSizeTest
                                     + Math.sqrt(sampleSizeTest * epsilonSampleSizeBest
                                     * (1.0 - epsilonSampleSizeBest) * CHI_SQUARED))) {
 
                                 if (inliersSampleSizeTest < imin(subsetSize, sampleSizeTest, beta)) {
                                     // equation not satisfied, no need to test for
                                     // smaller sampleSizeTest values anyway
 
                                     // jump out of the for sampleSizeTest loop
                                     break;
                                 }
                                 sampleSizeBest = sampleSizeTest;
                                 inliersSampleSizeBest = inliersSampleSizeTest;
                                 epsilonSampleSizeBest = (double) inliersSampleSizeBest / (double) sampleSizeBest;
                             }
 
                             // prepare for next loop iteration
                             inliersSampleSizeTest -= inliers.get(sortedIndices[sampleSizeTest - 1]) ? 1 : 0;
                         }
 
                         // is the best one we found even better than sampleSizeStar?
                         if (inliersSampleSizeBest * sampleSizeStar > inliersNStar * sampleSizeBest) {
 
                             // update all values
                             sampleSizeStar = sampleSizeBest;
                             inliersNStar = inliersSampleSizeBest;
                             kNStar = computeIterations((double) inliersNStar
                                             / (double) sampleSizeStar, subsetSize, 1.0 - eta0);
                         }
                     }
                 }
 
                 continueIteration = (currentIter < maxIterations);
                 if (useInlierThresholds && stopThresholdEnabled) {
                     // if inlier threshold is being used, and stop threshold is
                     // enabled, then stop the algorithm if threshold determined
                     // by median of residuals has a value lower than inlier
                     // threshold
                     continueIteration &= (threshold > inlierThreshold);
                 }
 
                 if (!improved) {
                     continueIteration &= ((inliersBest < inliersMin) || (currentIter < kNStar))
                             && (currentIter < iters);
                 }
 
                 listener.onEstimateNextIteration(this, currentIter);
             }
 
             // no solution could be found after completing all iterations
             if (bestResult == null) {
                 throw new RobustEstimatorException();
             }
 
             listener.onEstimateEnd(this);
 
             return bestResult;
         } catch (final SubsetSelectorException | SortingException e) {
             throw new RobustEstimatorException(e);
         } finally {
             locked = false;
         }
     }
 
     /**
      * Returns data about inliers once estimation has been done.
      *
      * @return data about inliers or null if estimation has not been done.
      */
     @Override
     public InliersData getInliersData() {
         return getBestInliersData();
     }
 
     /**
      * Returns method being used for robust estimation.
      *
      * @return method being used for robust estimation.
      */
     @Override
     public RobustEstimatorMethod getMethod() {
         return RobustEstimatorMethod.PROMEDS;
     }
 
     /**
      * Transforms indices picked by the subset selector into the indices where
      * samples are actually localed by taking into account their original
      * position before sorting quality scores.
      *
      * @param subsetIndices            indices picked by the subset selector. These are
      *                                 positions after sorting. Must have the subset length.
      * @param sortedIndices            indices relating sorted positions to their original
      *                                 positions. Each position i-th in the array refers to the original
      *                                 position before sorting. Must have the number of samples length.
      * @param transformedSubsetIndices array where result is stored. Must have
      *                                 the subset length.
      */
     private static void transformIndices(
             final int[] subsetIndices, final int[] sortedIndices, final int[] transformedSubsetIndices) {
         final var length = transformedSubsetIndices.length;
         for (var i = 0; i < length; i++) {
             transformedSubsetIndices[i] = sortedIndices[subsetIndices[i]];
         }
     }
 
     /**
      * Computes inliers data for current iteration.
      *
      * @param <T>                 type of result to be estimated.
      * @param iterResult          result to be tested on current iteration.
      * @param subsetSize          subset sample size to be picked on each iteration.
      * @param inlierFactor        factor to adjust threshold to determine whether
      *                            samples are inliers or not.
      * @param useInlierThresholds true to use thresholds to determine inliers,
      *                            false otherwise.
      * @param inlierThreshold     threshold to determine which samples are inliers.
      * @param residualsTemp       temporal array to store residuals, since median
      *                            computation requires modifying the original array.
      * @param listener            listener to obtain residuals for samples.
      * @param sorter              sorter instance to compute median of residuals.
      * @param inliersData         inliers data to be reused on each iteration
      */
     private static <T> void computeInliers(
             final T iterResult, final int subsetSize, final double inlierFactor, final boolean useInlierThresholds,
             final double inlierThreshold, final double[] residualsTemp, final LMedSRobustEstimatorListener<T> listener,
             final Sorter<Double> sorter, final PROMedSInliersData inliersData) {
 
         final var residuals = inliersData.getResiduals();
         final var lmedsInliers = inliersData.getInliersLMedS();
         final var msacInliers = inliersData.getInliersMSAC();
         var bestMedianResidual = inliersData.getBestMedianResidual();
         var medianResidualImproved = false;
 
         final var totalSamples = residuals.length;
 
         for (var i = 0; i < totalSamples; i++) {
             residuals[i] = Math.abs(listener.computeResidual(iterResult, i));
         }
 
         System.arraycopy(residuals, 0, residualsTemp, 0, residuals.length);
         final var medianResidual = sorter.median(residualsTemp);
         if (medianResidual < bestMedianResidual) {
             bestMedianResidual = medianResidual;
             medianResidualImproved = true;
         }
 
         final var standardDeviation = STD_CONSTANT * (1.0 + 5.0 / (totalSamples - subsetSize))
                 * Math.sqrt(medianResidual);
         final var normEstimatedThreshold = inlierFactor * medianResidual;
 
         // determine which points are inliers
         var numInliers = 0;
         // by default if thresholds are not used
         var lmedsInlierModelEnabled = true;
         if (useInlierThresholds) {
             var numInliersMsac = 0;
             var numInliersLmedS = 0;
             for (var i = 0; i < totalSamples; i++) {
                 if (residuals[i] <= normEstimatedThreshold) {
                     numInliersLmedS++;
                     lmedsInliers.set(i);
                 } else {
                     lmedsInliers.clear(i);
                 }
                 if (residuals[i] <= inlierThreshold) {
                     numInliersMsac++;
                     msacInliers.set(i);
                 } else {
                     msacInliers.clear(i);
                 }
             }
 
             // keep model with smaller number of inliers (to be more restrictive)
             lmedsInlierModelEnabled = numInliersLmedS < numInliersMsac;
             numInliers = lmedsInlierModelEnabled ? numInliersLmedS : numInliersMsac;
         } else {
 
             for (var i = 0; i < totalSamples; i++) {
                 if (residuals[i] <= normEstimatedThreshold) {
                     numInliers++;
                     lmedsInliers.set(i);
                 } else {
                     lmedsInliers.clear(i);
                 }
             }
         }
 
         // store values in inliers data, only if residuals improve
         if (medianResidualImproved) {
             inliersData.update(bestMedianResidual, standardDeviation, lmedsInlierModelEnabled, residuals, numInliers,
                     medianResidual, normEstimatedThreshold, true);
         } else {
             inliersData.medianResidualImproved = false;
         }
     }
 
     /**
      * Obtains indices of samples corresponding to samples ordered in descending
      * quality scores.
      *
      * @param qualityScores quality scores associated to each sample to be used
      *                      to obtain indices to sort samples in descending order of quality values.
      * @return indices to sort samples in descending order of quality values.
      * @throws SortingException if sorting fails.
      */
     private static int[] computeSortedQualityIndices(double[] qualityScores) throws SortingException {
         final var sorter = Sorter.<Double>create();
         final var qualityScoresCopy = Arrays.copyOf(qualityScores, qualityScores.length);
         // this method modifies quality scores copy array because it gets sorted
         // in ascending order. Indices contains indices of samples corresponding
         // to quality scores ordered in ascending order
         final var indices = sorter.sortWithIndices(qualityScoresCopy);
 
         // reverse indices so we have indices of samples ordered in descending
         // order of quality
         reverse(indices);
 
         return indices;
     }
 
     /**
      * Reverses provided array.
      *
      * @param array array to be reversed.
      */
     private static void reverse(final int[] array) {
         var length = array.length;
         for (var i = 0; i < length / 2; i++) {
             var temp = array[i];
             var pos = length - 1 - i;
             array[i] = array[pos];
             array[pos] = temp;
         }
     }
 
     /**
      * Computes number of required iterations to achieve required confidence
      * with current probability of inlier and sample subset size.
      *
      * @param probInlier probability of inlier.
      * @param subsetSize sample subset size.
      * @param confidence required confidence of result.
      * @return number of required iterations.
      */
     private static int computeIterations(final double probInlier, final int subsetSize, final double confidence) {
 
         // compute number of times the algorithm needs to be executed depending
         // on number of inliers respect total points to achieve with probability
         // confidence that we have all inliers and probability 1 - confidence
         // that we have some outliers
         final var probSubsetAllInliers = Math.pow(probInlier, subsetSize);
         if (Math.abs(probSubsetAllInliers) < Double.MIN_VALUE || Double.isNaN(probSubsetAllInliers)) {
             return Integer.MAX_VALUE;
         } else {
             final var logProbSomeOutliers = Math.log(1.0 - probSubsetAllInliers);
             if (Math.abs(logProbSomeOutliers) < Double.MIN_VALUE || Double.isNaN(logProbSomeOutliers)) {
                 return Integer.MAX_VALUE;
             } else {
                 return (int) Math.ceil(Math.abs(Math.log(1.0 - confidence) / logProbSomeOutliers));
             }
         }
     }
 
     /**
      * Non randomness states that i-m (where i is the cardinal of the set of
      * inliers for a wrong model) follows the binomial distribution B(n,beta).
      * For n big enough, B(n,beta) approximates to normal distribution N(mu,
      * sigma^2) by the central limit theorem, with mu = n*beta and sigma =
      * sqrt(n*beta*(1 - beta)).
      * Psi, the probability that In_star out of n_star data points are by chance
      * inliers to an arbitrary incorrect model, is set to 0.05 (5%, as in the
      * original paper), and you must change the Chi2 value if you chose a
      * different value for psi.
      *
      * @param subsetSize sample subset size.
      * @param sampleSize total number of samples.
      * @param beta       beta value.
      * @return i-m.
      */
     private static int imin(final int subsetSize, final int sampleSize, final double beta) {
         final var mu = sampleSize * beta;
         final var sigma = Math.sqrt(sampleSize * beta * (1.0 - beta));
 
         return (int) Math.ceil(subsetSize + mu + sigma * Math.sqrt(CHI_SQUARED));
     }
 
     /**
      * Keeps inliers data stored and initializes a new one with proper
      * initial values.
      *
      * @param inliersData  inliers data to be stored.
      * @param totalSamples total number of samples.
      */
     private void keepInliersData(PROMedSInliersData inliersData, final int totalSamples) {
         // keep the best inliers data corresponding to best solution,
         // in case it can be useful along with the result
         bestInliersData = inliersData;
 
         // create new inliers data instance until a new best solution
         // is found
         final var bestMedianResidual = inliersData.getBestMedianResidual();
         inliersData = new PROMedSInliersData(totalSamples);
         // update the best median residual on new instance so that
         // only better solutions that are found later can update
         // inliers data
         inliersData.update(bestMedianResidual, inliersData.getStandardDeviation(),
                 inliersData.isLMedSInlierModelEnabled(), inliersData.getResiduals(), inliersData.getNumInliers(),
                 bestMedianResidual, inliersData.getEstimatedThreshold(), false);
     }
 
     /**
      * Contains data related to inliers estimated in one iteration.
      */
     public static class PROMedSInliersData extends InliersData {
         /**
          * Best median of error found so far taking into account all provided
          * samples.
          */
         private double bestMedianResidual;
 
         /**
          * Standard deviation of error among all provided samples respect to
          * currently estimated result.
          */
         private double standardDeviation;
 
         /**
          * Inliers considering LMedS model.
          */
         private BitSet inliersLmeds;
 
         /**
          * Inliers considering MSAC model.
          */
         private BitSet inliersMsac;
 
         /**
          * Indicates whether LMedS or MSAC inlier model is enabled.
          */
         private boolean lmedsInlierModelEnabled;
 
         /**
          * Median of error found on current iteration among all provided
          * samples.
          */
         private double medianResidual;
 
         /**
          * Estimated threshold to determine whether samples are inliers or not.
          */
         private double estimatedThreshold;
 
         /**
          * Indicates whether median residual computed in current iteration has
          * improved respect to previous iterations.
          */
         private boolean medianResidualImproved;
 
         /**
          * Constructor.
          *
          * @param totalSamples total number of samples.
          */
         PROMedSInliersData(final int totalSamples) {
             bestMedianResidual = standardDeviation = medianResidual = estimatedThreshold = Double.MAX_VALUE;
             inliersLmeds = new BitSet(totalSamples);
             inliersMsac = new BitSet(totalSamples);
             lmedsInlierModelEnabled = true;
             residuals = new double[totalSamples];
             numInliers = 0;
             medianResidualImproved = false;
         }
 
         /**
          * Returns efficient array indicating which samples are considered
          * inliers and which ones aren't.
          *
          * @return array indicating which samples are considered inliers and
          * which ones aren't.
          */
         @Override
         public BitSet getInliers() {
             return lmedsInlierModelEnabled ? inliersLmeds : inliersMsac;
         }
 
         /**
          * Creates a copy of inlier data.
          *
          * @return copy of inlier data.
          */
         PROMedSInliersData createCopy() {
             final var result = new PROMedSInliersData(residuals.length);
             result.bestMedianResidual = bestMedianResidual;
             result.standardDeviation = standardDeviation;
             result.medianResidual = medianResidual;
             result.estimatedThreshold = estimatedThreshold;
             result.inliersLmeds = (BitSet) inliersLmeds.clone();
             result.inliersMsac = (BitSet) inliersMsac.clone();
             result.lmedsInlierModelEnabled = lmedsInlierModelEnabled;
             result.residuals = Arrays.copyOf(residuals, residuals.length);
             result.numInliers = numInliers;
             result.medianResidualImproved = medianResidualImproved;
 
             return result;
         }
 
         /**
          * Returns best median of error found so far taking into account all
          * provided samples.
          *
          * @return best median of error found so far taking into account all
          * provided samples.
          */
         double getBestMedianResidual() {
             return bestMedianResidual;
         }
 
         /**
          * Returns standard deviation of error among all provided samples
          * respect to currently estimated result.
          *
          * @return standard deviation of error among all provided samples
          * respect to currently estimated result.
          */
         double getStandardDeviation() {
             return standardDeviation;
         }
 
         /**
          * Returns inliers considering LMedS model.
          *
          * @return inliers considering LMedS model.
          */
         BitSet getInliersLMedS() {
             return inliersLmeds;
         }
 
         /**
          * Returns inliers considering MSAC model.
          *
          * @return inliers considering MSAC model.
          */
         BitSet getInliersMSAC() {
             return inliersMsac;
         }
 
         /**
          * Returns boolean indicating whether LMedS or MSAC inlier model is
          * enabled. If true, estimated threshold was used to determine inliers,
          * if false only median of residuals was used.
          *
          * @return true if LMedS model is used, false if MSAC model is used.
          */
         boolean isLMedSInlierModelEnabled() {
             return lmedsInlierModelEnabled;
         }
 
         /**
          * Returns estimated threshold to determine whether samples are inliers
          * or not.
          *
          * @return estimated threshold to determine whether samples are inliers
          * or not.
          */
         public double getEstimatedThreshold() {
             return estimatedThreshold;
         }
 
         /**
          * Returns boolean indicating whether median residual computed in
          * current iteration has improved respect to previous iterations.
          *
          * @return true if median residual improved, false otherwise.
          */
         boolean isMedianResidualImproved() {
             return medianResidualImproved;
         }
 
         /**
          * Updates data contained in this instance.
          *
          * @param bestMedianResidual      best median of error found so far taking
          *                                into account all provided samples.
          * @param standardDeviation       standard deviation of error among all
          *                                provided samples respect to currently estimated result.
          * @param lmedsInlierModelEnabled indicates whether the LMedS or MSAC
          *                                inlier model is used.
          * @param residuals               residuals obtained for each sample of data.
          * @param numInliers              number of inliers found on current iteration.
          * @param medianResidual          median of error found on current iteration
          *                                among all provided samples.
          * @param estimatedThreshold      estimated threshold to determine whether
          *                                samples are inliers or not.
          * @param medianResidualImproved  indicates whether median residual
          *                                computed in current iteration has improved respect to previous
          *                                iteration.
          */
         protected void update(
                 final double bestMedianResidual, final double standardDeviation, final boolean lmedsInlierModelEnabled,
                 final double[] residuals, final int numInliers, final double medianResidual,
                 final double estimatedThreshold, final boolean medianResidualImproved) {
             this.bestMedianResidual = bestMedianResidual;
             this.standardDeviation = standardDeviation;
             this.lmedsInlierModelEnabled = lmedsInlierModelEnabled;
             this.residuals = residuals;
             this.numInliers = numInliers;
             this.medianResidual = medianResidual;
             this.estimatedThreshold = estimatedThreshold;
             this.medianResidualImproved = medianResidualImproved;
         }
     }
 
 }