/*
    * Copyright (C) 2013 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.geometry.estimators;
  
  import com.irurueta.algebra.Matrix;
  import com.irurueta.algebra.Utils;
  import com.irurueta.algebra.WrongSizeException;
  import com.irurueta.geometry.HomogeneousPoint2D;
  import com.irurueta.geometry.NotAvailableException;
  import com.irurueta.geometry.PinholeCamera;
  import com.irurueta.geometry.Point2D;
  import com.irurueta.geometry.Point3D;
  import com.irurueta.geometry.ProjectiveTransformation2D;
  import com.irurueta.geometry.ProjectiveTransformation3D;
  import com.irurueta.geometry.refiners.DecomposedPointCorrespondencePinholeCameraRefiner;
  
  import java.util.ArrayList;
  import java.util.BitSet;
  import java.util.Collections;
  import java.util.List;
  
  /**
   * This file contains abstract implementation for pinhole camera estimators
   * based on point correspondences.
   */
  @SuppressWarnings("DuplicatedCode")
  public abstract class PointCorrespondencePinholeCameraEstimator extends PinholeCameraEstimator {
  
      /**
       * Minimum number of required point correspondences to estimate a pinhole
       * camera.
       */
      public static final int MIN_NUMBER_OF_POINT_CORRESPONDENCES = 6;
  
      /**
       * Indicates if by default provided point correspondences are normalized to
       * increase the accuracy of the estimation.
       */
      public static final boolean DEFAULT_NORMALIZE_POINT_CORRESPONDENCES = true;
  
      /**
       * Defines tiny value considered as machine precision.
       */
      public static final double EPS = 1e-8;
  
      /**
       * List of corresponding 3D points.
       */
      protected List<Point3D> points3D;
  
      /**
       * List of corresponding 2D points.
       */
      protected List<Point2D> points2D;
  
      /**
       * Indicates if provided point correspondences are normalized to increase
       * the accuracy of the estimation.
       */
      private boolean normalizePointCorrespondences;
  
      /**
       * Constructor.
       */
      protected PointCorrespondencePinholeCameraEstimator() {
          super();
          normalizePointCorrespondences = DEFAULT_NORMALIZE_POINT_CORRESPONDENCES;
      }
  
      /**
       * Constructor.
       *
       * @param listener listener to be notified of events such as when estimation
       *                 starts, ends or estimation progress changes.
       */
      protected PointCorrespondencePinholeCameraEstimator(final PinholeCameraEstimatorListener listener) {
          super(listener);
          normalizePointCorrespondences = DEFAULT_NORMALIZE_POINT_CORRESPONDENCES;
      }
  
      /**
       * Constructor.
       *
       * @param points3D list of corresponding 3D points.
       * @param points2D list of corresponding 2D points.
       * @throws IllegalArgumentException if any of the lists are null.
      * @throws WrongListSizesException  if provided lists of points don't have
      *                                  the same size and enough points.
      */
     protected PointCorrespondencePinholeCameraEstimator(final List<Point3D> points3D, final List<Point2D> points2D)
             throws WrongListSizesException {
         super();
         internalSetLists(points3D, points2D);
         normalizePointCorrespondences = DEFAULT_NORMALIZE_POINT_CORRESPONDENCES;
     }
 
     /**
      * Constructor.
      *
      * @param points3D list of corresponding 3D points.
      * @param points2D list of corresponding 2D points.
      * @param listener listener to be notified of events such as when estimation
      *                 starts, ends or estimation progress changes.
      * @throws IllegalArgumentException if any of the lists are null.
      * @throws WrongListSizesException  if provided lists of points don't have
      *                                  the same size and enough points.
      */
     protected PointCorrespondencePinholeCameraEstimator(
             final List<Point3D> points3D, final List<Point2D> points2D, final PinholeCameraEstimatorListener listener)
             throws WrongListSizesException {
         super(listener);
         internalSetLists(points3D, points2D);
         normalizePointCorrespondences = DEFAULT_NORMALIZE_POINT_CORRESPONDENCES;
     }
 
     /**
      * Internal method to set list of corresponding points (it does not check
      * if estimator is locked).
      *
      * @param points3D list of corresponding 3D points.
      * @param points2D list of corresponding 2D points.
      * @throws IllegalArgumentException if any of the lists are null.
      * @throws WrongListSizesException  if provided lists of points don't have
      *                                  the same size and enough points.
      */
     private void internalSetLists(final List<Point3D> points3D, final List<Point2D> points2D)
             throws WrongListSizesException {
 
         if (points3D == null || points2D == null) {
             throw new IllegalArgumentException();
         }
 
         if (!areValidLists(points3D, points2D)) {
             throw new WrongListSizesException();
         }
 
         this.points3D = points3D;
         this.points2D = points2D;
     }
 
     /**
      * Sets list of corresponding points.
      *
      * @param points3D list of corresponding 3D points.
      * @param points2D list of corresponding 2D points.
      * @throws LockedException          if estimator is locked.
      * @throws IllegalArgumentException if any of the lists are null.
      * @throws WrongListSizesException  if provided lists of points don't have
      *                                  the same size and enough points.
      */
     public void setLists(final List<Point3D> points3D, final List<Point2D> points2D) throws LockedException,
             WrongListSizesException {
         if (isLocked()) {
             throw new LockedException();
         }
 
         internalSetLists(points3D, points2D);
     }
 
     /**
      * Returns list of corresponding 3D points.
      * Notice that this method returns an unmodifiable list of points to avoid
      * undesired modifications.
      *
      * @return list of corresponding 3D points.
      * @throws NotAvailableException if list of points is not yet available.
      */
     public List<Point3D> getPoints3D() throws NotAvailableException {
         if (points3D == null) {
             throw new NotAvailableException();
         }
 
         // to avoid undesired modifications
         return Collections.unmodifiableList(points3D);
     }
 
     /**
      * Returns list of corresponding 2D points.
      * Notice that this method returns an unmodifiable list of points to avoid
      * undesired modifications.
      *
      * @return list of corresponding 2D points.
      * @throws NotAvailableException if list of points is not yet available.
      */
     public List<Point2D> getPoints2D() throws NotAvailableException {
         if (points2D == null) {
             throw new NotAvailableException();
         }
 
         // to avoid undesired modifications
         return Collections.unmodifiableList(points2D);
     }
 
     /**
      * Indicates if lists of corresponding 2D/3D points are valid.
      * Lists are considered valid if they have the same number of points and
      * both have more than the required minimum of correspondences (which is 6).
      *
      * @param points3D list of corresponding 3D points.
      * @param points2D list of corresponding 2D points.
      * @return true if corresponding 2D/3D points are valid, false otherwise.
      */
     public static boolean areValidLists(final List<Point3D> points3D, final List<Point2D> points2D) {
         if (points3D == null || points2D == null) {
             return false;
         }
         return points3D.size() == points2D.size() && points3D.size() >= MIN_NUMBER_OF_POINT_CORRESPONDENCES;
     }
 
     /**
      * Indicates if lists have already been provided and are available for
      * retrieval.
      *
      * @return true if available, false otherwise.
      */
     public boolean areListsAvailable() {
         return points3D != null && points2D != null;
     }
 
     /**
      * Indicates if provided point correspondences are normalized to increase
      * the accuracy of the estimation.
      *
      * @return true if input point correspondences will be normalized, false
      * otherwise.
      */
     public boolean arePointCorrespondencesNormalized() {
         return normalizePointCorrespondences;
     }
 
     /**
      * Specifies whether provided point correspondences are normalized to
      * increase the accuracy of the estimation.
      *
      * @param normalize true if input point correspondences will be normalized,
      *                  false otherwise.
      * @throws LockedException if estimator is locked.
      */
     public void setPointCorrespondencesNormalized(final boolean normalize) throws LockedException {
 
         if (isLocked()) {
             throw new LockedException();
         }
         normalizePointCorrespondences = normalize;
     }
 
     /**
      * Transforms 2D points so that they have zero mean and unitary standard
      * deviation. Provided transformation will be updated containing the
      * transformation used for input points.
      *
      * @param list                  list of input 2D points.
      * @param inverseTransformation inverseTransformation used on input points.
      *                              This is an output variable.
      * @return transformed 2D points.
      * @throws PinholeCameraEstimatorException if transformation cannot be
      *                                         computed because point configuration might be degenerate.
      */
     private List<Point2D> transformPoints2D(
             final List<Point2D> list, final ProjectiveTransformation2D inverseTransformation)
             throws PinholeCameraEstimatorException {
 
         // compute image point coordinates limits
         var minX = Double.MAX_VALUE;
         var maxX = -Double.MAX_VALUE;
         var minY = Double.MAX_VALUE;
         var maxY = -Double.MAX_VALUE;
         for (final var point : list) {
             point.normalize();
             final var x = point.getInhomX();
             final var y = point.getInhomY();
 
             if (x < minX) {
                 minX = x;
             }
             if (x > maxX) {
                 maxX = x;
             }
             if (y < minY) {
                 minY = y;
             }
             if (y > maxY) {
                 maxY = y;
             }
         }
 
         // compute size of image plane (based on 2D coordinates limits), scale
         // and centroid
         final var width = maxX - minX;
         final var height = maxY - minY;
 
         final var norm = Math.sqrt(width * width + height * height);
         // points are too close to each other (norm is too small).
         // This is a degenerate configuration and results will be meaningless
         if (norm < EPS) {
             throw new PinholeCameraEstimatorException();
         }
 
         final var scale = 1.0 / norm;
         final var centroidX = (minX + maxX) / 2.0;
         final var centroidY = (minY + maxY) / 2.0;
 
         // set inverse transformation matrix
         try {
             final var inverseTransformationMatrix = new Matrix(Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH,
                     Point2D.POINT2D_HOMOGENEOUS_COORDINATES_LENGTH);
 
             inverseTransformationMatrix.setElementAt(0, 0, norm);
             // 1.0 / scale
             inverseTransformationMatrix.setElementAt(1, 1, norm);
             inverseTransformationMatrix.setElementAt(2, 2, 1.0);
             inverseTransformationMatrix.setElementAt(0, 2, centroidX);
             inverseTransformationMatrix.setElementAt(1, 2, centroidY);
 
             inverseTransformation.setT(inverseTransformationMatrix);
         } catch (final WrongSizeException ignore) {
             // never happens
         }
 
         inverseTransformation.normalize();
 
 
         // transform list of image points
         final var transformedPoints = new ArrayList<Point2D>(list.size());
         for (final var point : list) {
             point.normalize();
             final var homX = point.getHomX();
             final var homY = point.getHomY();
             final var homW = point.getHomW();
 
             final var homPoint = new HomogeneousPoint2D(
                     scale * (homX - centroidX * homW),
                     scale * (homY - centroidY * homW), homW);
             // normalize point to increase accuracy
             homPoint.normalize();
             transformedPoints.add(homPoint);
         }
 
         return transformedPoints;
     }
 
     /**
      * Transforms 3D points so that they have zero mean and unitary standard
      * deviation. Provided transformation will be updated containing the
      * transformation used for input points.
      *
      * @param list           list of input 3D points.
      * @param transformation transformation used on input points. This is an
      *                       output variable.
      * @return transformed 3D points.
      * @throws PinholeCameraEstimatorException if transformation cannot be
      *                                         computed because point configuration might be degenerate.
      */
     private List<Point3D> transformPoints3D(final List<Point3D> list, final ProjectiveTransformation3D transformation)
             throws PinholeCameraEstimatorException {
 
         // compute image point coordinates limits
         var minX = Double.MAX_VALUE;
         var maxX = -Double.MAX_VALUE;
         var minY = Double.MAX_VALUE;
         var maxY = -Double.MAX_VALUE;
         var minZ = Double.MAX_VALUE;
         var maxZ = -Double.MAX_VALUE;
         for (final var point : list) {
             point.normalize();
             final var x = point.getInhomX();
             final var y = point.getInhomY();
             final var z = point.getInhomZ();
 
             if (x < minX) {
                 minX = x;
             }
             if (x > maxX) {
                 maxX = x;
             }
             if (y < minY) {
                 minY = y;
             }
             if (y > maxY) {
                 maxY = y;
             }
             if (z < minZ) {
                 minZ = z;
             }
             if (z > maxZ) {
                 maxZ = z;
             }
         }
 
         // compute size of image plane (based on 2D coordinates limits), scale
         // and centroid
         final var width = maxX - minX;
         final var height = maxY - minY;
         final var depth = maxZ - minZ;
 
         final var norm = Math.sqrt(width * width + height * height + depth * depth);
         // points are too close to each other (norm is too small).
         // This is a degenerate configuration and results will be meaningless
         if (norm < EPS) {
             throw new PinholeCameraEstimatorException();
         }
 
         final var scale = 1.0 / norm;
         final var centroidX = (minX + maxX) / 2.0;
         final var centroidY = (minY + maxY) / 2.0;
         final var centroidZ = (minZ + maxZ) / 2.0;
 
         // set transformation matrix
         try {
             final var transformMatrix = new Matrix(Point3D.POINT3D_HOMOGENEOUS_COORDINATES_LENGTH,
                     Point3D.POINT3D_HOMOGENEOUS_COORDINATES_LENGTH);
             transformMatrix.setElementAt(0, 0, scale);
             transformMatrix.setElementAt(1, 1, scale);
             transformMatrix.setElementAt(2, 2, scale);
             transformMatrix.setElementAt(3, 3, 1.0);
             transformMatrix.setElementAt(0, 3, -scale * centroidX);
             transformMatrix.setElementAt(1, 3, -scale * centroidY);
             transformMatrix.setElementAt(2, 3, -scale * centroidZ);
 
             // normalize transformation to increase accuracy
             transformation.setT(transformMatrix);
         } catch (final WrongSizeException ignore) {
             // never happens
         }
         transformation.normalize();
 
         // transform list of world points
         final var transformedPoints = new ArrayList<Point3D>(list.size());
         for (final var point : list) {
             transformedPoints.add(transformation.transformAndReturnNew(point));
         }
 
         return transformedPoints;
     }
 
     /**
      * Estimates a pinhole camera.
      *
      * @return estimated pinhole camera.
      * @throws LockedException                 if estimator is locked.
      * @throws NotReadyException               if input has not yet been provided.
      * @throws PinholeCameraEstimatorException if an error occurs during
      *                                         estimation, usually because input data is not valid.
      */
     @Override
     public PinholeCamera estimate() throws LockedException, NotReadyException, PinholeCameraEstimatorException {
 
         if (isLocked()) {
             throw new LockedException();
         }
         if (!isReady()) {
             throw new NotReadyException();
         }
 
         try {
             locked = true;
             if (listener != null) {
                 listener.onEstimateStart(this);
             }
 
             final var inverseTransformation2D = new ProjectiveTransformation2D();
             final var transformation3D = new ProjectiveTransformation3D();
             final List<Point2D> inputPoints2D;
             final List<Point3D> inputPoints3D;
 
             if (normalizePointCorrespondences) {
                 // normalize 2D and 3D points and update transformations
                 inputPoints2D = transformPoints2D(points2D, inverseTransformation2D);
                 inputPoints3D = transformPoints3D(points3D, transformation3D);
             } else {
                 inputPoints2D = points2D;
                 inputPoints3D = points3D;
             }
 
             var pinholeCameraMatrix = internalEstimate(inputPoints3D, inputPoints2D);
 
             if (normalizePointCorrespondences) {
                 inverseTransformation2D.normalize();
                 transformation3D.normalize();
 
                 // denormalize pinhole camera
                 final var invTrans2DMatrix = inverseTransformation2D.asMatrix();
                 final var trans3DMatrix = transformation3D.asMatrix();
 
                 invTrans2DMatrix.multiply(pinholeCameraMatrix);
                 invTrans2DMatrix.multiply(trans3DMatrix);
 
                 pinholeCameraMatrix = invTrans2DMatrix;
 
                 // normalize by Frobenius norm to increase accuracy after point
                 // de-normalization
                 final var norm = Utils.normF(pinholeCameraMatrix);
                 pinholeCameraMatrix.multiplyByScalar(1.0 / norm);
             }
 
             final var camera = new PinholeCamera(pinholeCameraMatrix);
 
             if (listener != null) {
                 listener.onEstimateEnd(this);
             }
 
             return attemptRefine(camera);
 
         } catch (final PinholeCameraEstimatorException e) {
             throw e;
         } catch (final Exception e) {
             throw new PinholeCameraEstimatorException(e);
         } finally {
             locked = false;
         }
     }
 
     /**
      * Internal method that actually computes the normalized pinhole camera
      * internal matrix.
      * Returned matrix must have norm equal to one and might be estimated using
      * any convenient algorithm (i.e. DLT or weighted DLT).
      *
      * @param points3D list of 3D points. Points might or might not be
      *                 normalized.
      * @param points2D list of 2D points. Points might or might not be
      *                 normalized.
      * @return matrix of estimated pinhole camera.
      * @throws PinholeCameraEstimatorException if estimation fails for some
      *                                         reason (i.e. numerical instability or geometric degeneracy).
      */
     protected abstract Matrix internalEstimate(final List<Point3D> points3D, final List<Point2D> points2D)
             throws PinholeCameraEstimatorException;
 
     /**
      * Attempts to refine provided camera using requested suggestions.
      * If no suggestions are requested or if refinement fails, provided
      * camera is returned instead.
      *
      * @param pinholeCamera camera to be refined.
      * @return refined camera.
      */
     @Override
     protected PinholeCamera attemptRefine(final PinholeCamera pinholeCamera) {
         if (hasSuggestions()) {
             final var numPoints = points3D.size();
             final var inliers = new BitSet(numPoints);
             inliers.set(0, numPoints, true);
             final var residuals = new double[numPoints];
 
             final var refiner = new DecomposedPointCorrespondencePinholeCameraRefiner(pinholeCamera,
                     false, inliers, residuals, numPoints, points3D, points2D, 0.0);
             try {
                 refiner.setMinSuggestionWeight(minSuggestionWeight);
                 refiner.setMaxSuggestionWeight(maxSuggestionWeight);
                 refiner.setSuggestionWeightStep(suggestionWeightStep);
 
                 refiner.setSuggestSkewnessValueEnabled(suggestSkewnessValueEnabled);
                 refiner.setSuggestedSkewnessValue(suggestedSkewnessValue);
                 refiner.setSuggestHorizontalFocalLengthEnabled(suggestHorizontalFocalLengthEnabled);
                 refiner.setSuggestedHorizontalFocalLengthValue(suggestedHorizontalFocalLengthValue);
                 refiner.setSuggestVerticalFocalLengthEnabled(suggestVerticalFocalLengthEnabled);
                 refiner.setSuggestedVerticalFocalLengthValue(suggestedVerticalFocalLengthValue);
                 refiner.setSuggestAspectRatioEnabled(suggestAspectRatioEnabled);
                 refiner.setSuggestedAspectRatioValue(suggestedAspectRatioValue);
                 refiner.setSuggestPrincipalPointEnabled(suggestPrincipalPointEnabled);
                 refiner.setSuggestedPrincipalPointValue(suggestedPrincipalPointValue);
                 refiner.setSuggestRotationEnabled(suggestRotationEnabled);
                 refiner.setSuggestedRotationValue(suggestedRotationValue);
                 refiner.setSuggestCenterEnabled(suggestCenterEnabled);
                 refiner.setSuggestedCenterValue(suggestedCenterValue);
 
                 final var result = new PinholeCamera();
                 final var improved = refiner.refine(result);
 
                 return improved ? result : pinholeCamera;
 
             } catch (final Exception e) {
                 return pinholeCamera;
             }
         } else {
             return pinholeCamera;
         }
     }
 }