/*
    * 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.ar.epipolar;
  
  import com.irurueta.geometry.CoordinatesType;
  import com.irurueta.geometry.Line2D;
  import com.irurueta.geometry.Point2D;
  import com.irurueta.geometry.estimators.LockedException;
  import com.irurueta.geometry.estimators.NotReadyException;
  
  import java.util.ArrayList;
  import java.util.List;
  
  /**
   * Fixes matched pairs of points so that they perfectly follow a given epipolar
   * geometry.
   * When matching points typically the matching precision is about 1 pixel,
   * however this makes that matched points under a given epipolar geometry (i.e.
   * fundamental or essential matrix), do not lie perfectly on the corresponding
   * epipolar plane or epipolar lines.
   * The consequence is that triangularization of these matches will fail or
   * produce inaccurate results.
   * By fixing matched points using a corrector following a given epipolar
   * geometry, this effect is alleviated.
   * This corrector uses the Sampson approximation which is capable to remove
   * small errors when matches are close to their real epipolar lines. This method
   * is faster than the Gold Standard method, but can only correct small errors
   * (1 or 2 pixels).
   */
  public class SampsonCorrector extends Corrector {
  
      /**
       * Constructor.
       */
      public SampsonCorrector() {
          super();
      }
  
      /**
       * Constructor.
       *
       * @param fundamentalMatrix fundamental matrix to be set.
       */
      public SampsonCorrector(final FundamentalMatrix fundamentalMatrix) {
          super(fundamentalMatrix);
      }
  
      /**
       * Constructor.
       *
       * @param leftPoints  points to be corrected on left view.
       * @param rightPoints points to be corrected on right view.
       * @throws IllegalArgumentException if provided lists of points don't have
       *                                  the same size.
       */
      public SampsonCorrector(final List<Point2D> leftPoints, final List<Point2D> rightPoints) {
          super(leftPoints, rightPoints);
      }
  
      /**
       * Constructor.
       *
       * @param leftPoints        points to be corrected on left view.
       * @param rightPoints       points to be corrected on right view.
       * @param fundamentalMatrix fundamental matrix to be set.
       * @throws IllegalArgumentException if provided lists of points don't have
       *                                  the same size.
       */
      public SampsonCorrector(final List<Point2D> leftPoints, final List<Point2D> rightPoints,
                              final FundamentalMatrix fundamentalMatrix) {
          super(leftPoints, rightPoints, fundamentalMatrix);
      }
  
      /**
       * Constructor.
       *
       * @param listener listener to handle events generated by this class.
       */
      public SampsonCorrector(final CorrectorListener listener) {
          super(listener);
      }
  
      /**
       * Constructor.
       *
       * @param fundamentalMatrix fundamental matrix to be set.
      * @param listener          listener to handle events generated by this class.
      */
     public SampsonCorrector(final FundamentalMatrix fundamentalMatrix, final CorrectorListener listener) {
         super(fundamentalMatrix, listener);
     }
 
     /**
      * Constructor.
      *
      * @param leftPoints  points to be corrected on left view.
      * @param rightPoints points to be corrected on right view.
      * @param listener    listener to handle events generated by this class.
      * @throws IllegalArgumentException if provided lists of points don't have
      *                                  the same size.
      */
     public SampsonCorrector(final List<Point2D> leftPoints, final List<Point2D> rightPoints,
                             final CorrectorListener listener) {
         super(leftPoints, rightPoints, listener);
     }
 
     /**
      * Constructor.
      *
      * @param leftPoints        points to be corrected on left view.
      * @param rightPoints       points to be corrected on right view.
      * @param fundamentalMatrix fundamental matrix to be set.
      * @param listener          listener to handle events generated by this class.
      * @throws IllegalArgumentException if provided lists of points don't have
      *                                  the same size.
      */
     public SampsonCorrector(final List<Point2D> leftPoints, final List<Point2D> rightPoints,
                             final FundamentalMatrix fundamentalMatrix, final CorrectorListener listener) {
         super(leftPoints, rightPoints, fundamentalMatrix, listener);
     }
 
     /**
      * Corrects the lists of provided matched points to be corrected.
      *
      * @throws NotReadyException if this instance is not ready (either points or
      *                           fundamental matrix has not been provided yet).
      * @throws LockedException   if this instance is locked doing computations.
      */
     @SuppressWarnings("DuplicatedCode")
     @Override
     public void correct() throws NotReadyException, LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         if (!isReady()) {
             throw new NotReadyException();
         }
 
         locked = true;
 
         if (listener != null) {
             listener.onCorrectStart(this);
         }
 
         // epipolar lines to be reused for memory efficiency
         final var leftEpipolarLine = new Line2D();
         final var rightEpipolarLine = new Line2D();
 
         leftCorrectedPoints = new ArrayList<>();
         rightCorrectedPoints = new ArrayList<>();
 
         final var size = leftPoints.size();
         float progress;
         var previousProgress = 0.0f;
         for (var i = 0; i < size; i++) {
             final var leftPoint = leftPoints.get(i);
             final var rightPoint = rightPoints.get(i);
 
             final var leftCorrectedPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
             final var rightCorrectedPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
 
             // correct single pair
             SampsonSingleCorrector.correct(leftPoint, rightPoint, fundamentalMatrix, leftCorrectedPoint,
                     rightCorrectedPoint, leftEpipolarLine, rightEpipolarLine);
 
             leftCorrectedPoints.add(leftCorrectedPoint);
             rightCorrectedPoints.add(rightCorrectedPoint);
 
             if (listener != null) {
                 progress = i / (float) size;
                 if (progress - previousProgress > progressDelta) {
                     // progress has changed significantly
                     previousProgress = progress;
                     listener.onCorrectProgressChange(this, progress);
                 }
             }
         }
 
         if (listener != null) {
             listener.onCorrectEnd(this);
         }
 
         locked = false;
     }
 
     /**
      * Gets type of correction being used.
      *
      * @return type of correction.
      */
     @Override
     public CorrectorType getType() {
         return CorrectorType.SAMPSON_CORRECTOR;
     }
 }