/*
    * Copyright (C) 2016 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 java.util.ArrayList;
  import java.util.BitSet;
  
  /**
   * This class implements LMedS (Least Median of Squares) algorithm to robustly
   * estimate a data model.
   * LMedS is based on the idea that a given proportion of outliers exists in the
   * total amount of samples provided. This algorithm tries to iteratively find
   * the beast subset of samples picking the ones with the least median of error.
   * To determine whether a sample is an outlier or not, and the estimated error
   * for each sample, provided listener must be used.
   * Contrary to RANSAC, this algorithm does not require a fixed threshold to be
   * set to determine whether samples are inliers or not. Instead, threshold is
   * computed dynamically. Because of that LMedS typically produces results with
   * larger error than RANSAC having a similar computational cost, because samples
   * usually contain a large error. Hence, if threshold is known in advance for a
   * given estimation, RANSAC should be preferred rather than LMedS.
   * On the contrary, if it can be ensured that samples are very accurate except
   * for some outliers, then LMedS becomes much more accurate than RANSAC because
   * it typically converges to a solution with a very small threshold. However,
   * typically inlier samples tend to have certain error, and in practice LMedS
   * produces results with a similar accuracy and computational cost than RANSAC.
   *
   * @param <T> type of object to be estimated.
   */
  @SuppressWarnings("DuplicatedCode")
  public class LMedSRobustEstimator<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;
  
      /**
       * Default value to be used for stop threshold. Stop threshold can be used
       * to keep the algorithm iterating in case that best threshold is not small
       * enough. Once a better solution is found yielding a threshold smaller than
       * this value, the algorithm will stop.
       */
      public static final double DEFAULT_STOP_THRESHOLD = 0.0;
  
      /**
       * Minimum allowed stop threshold value.
       */
      public static final double MIN_STOP_THRESHOLD = 0.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;
 
     /**
      * Constant to estimate standard deviation of residuals based on their
      * median.
      */
     public static final double STD_CONSTANT = 1.4826;
 
     /**
      * 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;
 
     /**
      * 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 LMedSInliersData bestInliersData;
 
     /**
      * Threshold to be used to keep the algorithm iterating in case that
      * best threshold is not small enough. Once a better solution is found
      * yielding a threshold smaller than this value, the algorithm will stop.
      */
     private double stopThreshold;
 
     /**
      * 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;
 
 
     /**
      * Constructor.
      */
     public LMedSRobustEstimator() {
         super();
         confidence = DEFAULT_CONFIDENCE;
         maxIterations = DEFAULT_MAX_ITERATIONS;
         iters = maxIterations;
         bestResult = null;
         bestInliersData = null;
         stopThreshold = DEFAULT_STOP_THRESHOLD;
         inlierFactor = DEFAULT_INLIER_FACTOR;
     }
 
     /**
      * 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 LMedSRobustEstimator(final LMedSRobustEstimatorListener<T> listener) {
         super(listener);
         confidence = DEFAULT_CONFIDENCE;
         maxIterations = DEFAULT_MAX_ITERATIONS;
         iters = maxIterations;
         bestResult = null;
         bestInliersData = null;
         stopThreshold = DEFAULT_STOP_THRESHOLD;
         inlierFactor = DEFAULT_INLIER_FACTOR;
     }
 
     /**
      * 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 threshold to be used to keep the algorithm iterating in case that
      * best threshold is not small enough. Once a better solution is found
      * yielding a threshold smaller than this value, the algorithm will stop.
      *
      * @return threshold to be used to keep the algorithm iterating in case that
      * best threshold is not small enough.
      */
     public double getStopThreshold() {
         return stopThreshold;
     }
 
     /**
      * Sets threshold to be used to keep the algorithm iterating in case that
      * best threshold is not small enough. Once a better solution is found
      * yielding a threshold smaller than this vlaue, the algorithm will stop.
      *
      * @param stopThreshold threshold to be used to keep the algorithm iterating
      *                      in case that best threshold is not small enough.
      * @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 setStopThreshold(final double stopThreshold) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         if (stopThreshold < MIN_STOP_THRESHOLD) {
             throw new IllegalArgumentException();
         }
 
         this.stopThreshold = stopThreshold;
     }
 
     /**
      * 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 number of iterations to be done to obtain required confidence.
      *
      * @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.
      */
     public LMedSInliersData 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 LMedSRobustEstimatorListener);
     }
 
     /**
      * 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 {
             final var listener = (LMedSRobustEstimatorListener<T>) this.listener;
 
             locked = true;
 
             listener.onEstimateStart(this);
 
             final var totalSamples = listener.getTotalSamples();
             final var subsetSize = listener.getSubsetSize();
             int bestNumInliers;
             var threshold = Double.MAX_VALUE;
             iters = Integer.MAX_VALUE;
             int newNIters;
             var currentIter = 0;
             // reusable list that will contain preliminary solutions on each
             // iteration
             final var iterResults = new ArrayList<T>();
             bestResult = null; // best result found so far
             // progress and previous progress to determine when progress
             // notification must occur
             var previousProgress = 0.0f;
             float progress;
             // indices of subset picked in one iteration
             final var subsetIndices = new int[subsetSize];
             final var residualsTemp = new double[totalSamples];
             // indicates if result improved
             boolean improved;
             // indicates whether algorithm must continue iterating
             var continueIteration = true;
 
             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 LMedSInliersData(totalSamples);
             // sorter to compute medians
             final var sorter = Sorter.<Double>create();
 
             while (continueIteration) {
                 // generate a random subset of samples
                 subsetSelector.computeRandomSubsets(subsetSize, subsetIndices);
 
                 // clear list of preliminary solutions before calling listener
                 iterResults.clear();
                 // compute solution for current iteration
                 listener.estimatePreliminarSolutions(subsetIndices, iterResults);
 
                 // iterate over all solutions that have been found
                 improved = false;
                 for (final var iterResult : iterResults) {
                     // compute inliers
                     computeInliers(iterResult, subsetSize, inlierFactor, residualsTemp, listener, sorter, inliersData);
 
                     // save solution that produces the best residual
                     if (inliersData.isMedianResidualImproved()) {
                         improved = true;
 
                         // keep current solution
                         bestResult = iterResult;
 
                         // keep the best inliers data corresponding to best solution,
                         // in case it can be useful along with the result
                         bestInliersData = inliersData;
 
                         // recompute number of times the algorithm needs to be
                         // executed depending on current number of inliers to
                         // achieve with probability mConfidence that we have
                         // inliers and probability 1 - mConfidence that we have
                         // outliers
                         bestNumInliers = inliersData.getNumInliers();
                         final var probInlier = ((double) bestNumInliers) / ((double) totalSamples);
 
                         final var probSubsetAllInliers = Math.pow(probInlier, subsetSize);
 
                         if (Math.abs(probSubsetAllInliers) < Double.MIN_VALUE || Double.isNaN(probSubsetAllInliers)) {
                             newNIters = Integer.MAX_VALUE;
                         } else {
                             final var logProbSomeOutliers = Math.log(1.0 - probSubsetAllInliers);
                             if (Math.abs(logProbSomeOutliers) < Double.MIN_VALUE || Double.isNaN(logProbSomeOutliers)) {
                                 newNIters = Integer.MAX_VALUE;
                             } else {
                                 newNIters = (int) Math.ceil(Math.abs(Math.log(1.0 - confidence) / logProbSomeOutliers));
                             }
                         }
 
                         if (newNIters < iters) {
                             iters = newNIters;
                         }
 
                         threshold = inliersData.getEstimatedThreshold();
 
                         // create new inliers data instance until a new best
                         // solution is found
                         final var bestMedianResidual = inliersData.getBestMedianResidual();
                         inliersData = new LMedSInliersData(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.getInliers(), inliersData.getResiduals(), inliersData.getNumInliers(),
                                 inliersData.getEstimatedThreshold(), false);
                     }
                 }
 
                 if (iters > 0) {
                     progress = Math.min((float) currentIter / (float) iters, 1.0f);
                 } else {
                     progress = 1.0f;
                 }
                 if (progress - previousProgress > progressDelta) {
                     previousProgress = progress;
                     listener.onEstimateProgressChange(this, progress);
                 }
                 currentIter++;
                 continueIteration = (currentIter < maxIterations) && (threshold > stopThreshold);
                 if (!improved) {
                     continueIteration &= (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 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.LMEDS;
     }
 
     /**
      * 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 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 double[] residualsTemp,
             final LMedSRobustEstimatorListener<T> listener, final Sorter<Double> sorter, LMedSInliersData inliersData) {
 
         final var residuals = inliersData.getResiduals();
         final var inliers = inliersData.getInliers();
         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;
         for (var i = 0; i < totalSamples; i++) {
             if (residuals[i] <= normEstimatedThreshold) {
                 numInliers++;
                 inliers.set(i);
             } else {
                 inliers.clear(i);
             }
         }
 
         // store values in inliers data, only if residuals improve
         if (medianResidualImproved) {
             inliersData.update(bestMedianResidual, standardDeviation, inliers, residuals, numInliers,
                     normEstimatedThreshold, true);
         }
     }
 
     /**
      * Contains data related to inliers estimated in one iteration.
      */
     public static class LMedSInliersData 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;
 
         /**
          * Efficiently stores which samples are considered inliers and which
          * ones aren't.
          */
         private BitSet inliers;
 
         /**
          * 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.
          */
         protected LMedSInliersData(final int totalSamples) {
             bestMedianResidual = Double.MAX_VALUE;
             standardDeviation = Double.MAX_VALUE;
             estimatedThreshold = Double.MAX_VALUE;
             inliers = new BitSet(totalSamples);
             residuals = new double[totalSamples];
             numInliers = 0;
             medianResidualImproved = false;
         }
 
         /**
          * 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.
          */
         public 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.
          */
         public double getStandardDeviation() {
             return standardDeviation;
         }
 
         /**
          * 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 inliers;
         }
 
         /**
          * 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.
          */
         public 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 inliers                efficiently stores which samples are considered
          *                               inliers and which ones aren't.
          * @param residuals              residuals obtained for each sample of data.
          * @param numInliers             number of inliers found on current iteration.
          * @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 BitSet inliers, final double[] residuals, final int numInliers,
                               final double estimatedThreshold, final boolean medianResidualImproved) {
             this.bestMedianResidual = bestMedianResidual;
             this.standardDeviation = standardDeviation;
             this.inliers = inliers;
             this.residuals = residuals;
             this.numInliers = numInliers;
             this.estimatedThreshold = estimatedThreshold;
             this.medianResidualImproved = medianResidualImproved;
         }
     }
 }