/*
    * Copyright (C) 2017 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.sfm;
  
  import com.irurueta.ar.calibration.estimators.DualAbsoluteQuadricEstimator;
  import com.irurueta.ar.calibration.estimators.LMSEDualAbsoluteQuadricEstimator;
  import com.irurueta.ar.epipolar.FundamentalMatrix;
  import com.irurueta.geometry.PinholeCamera;
  import com.irurueta.geometry.estimators.LockedException;
  import com.irurueta.geometry.estimators.NotReadyException;
  
  import java.util.ArrayList;
  
  /**
   * Estimates an initial pair of cameras in the metric stratum (up to an
   * arbitrary scale) using a given fundamental matrix and assuming zero skewness
   * and principal point at the origin for the intrinsic parameters of estimated
   * cameras.
   * Aspect ratio can be configured but by default it is assumed to be 1.0.
   */
  public class DualAbsoluteQuadricInitialCamerasEstimator extends InitialCamerasEstimator {
  
      /**
       * Aspect ratio of intrinsic parameters of cameras.
       * Typically, this value is 1.0 if vertical coordinates increase upwards,
       * or -1.0 if it is the opposite.
       */
      private double aspectRatio = DualAbsoluteQuadricEstimator.DEFAULT_FOCAL_DISTANCE_ASPECT_RATIO;
  
      /**
       * Constructor.
       */
      public DualAbsoluteQuadricInitialCamerasEstimator() {
          super();
      }
  
      /**
       * Constructor.
       *
       * @param fundamentalMatrix fundamental matrix relating two views.
       */
      public DualAbsoluteQuadricInitialCamerasEstimator(final FundamentalMatrix fundamentalMatrix) {
          super(fundamentalMatrix);
      }
  
      /**
       * Constructor.
       *
       * @param listener listener to handle events raised by this instance.
       */
      public DualAbsoluteQuadricInitialCamerasEstimator(final InitialCamerasEstimatorListener listener) {
          super(listener);
      }
  
      /**
       * Constructor.
       *
       * @param fundamentalMatrix fundamental matrix relating two views.
       * @param listener          listener to handle events raised by this instance.
       */
      public DualAbsoluteQuadricInitialCamerasEstimator(
              final FundamentalMatrix fundamentalMatrix, final InitialCamerasEstimatorListener listener) {
          super(fundamentalMatrix, listener);
      }
  
      /**
       * Returns method used by this estimator.
       *
       * @return method used by this estimator.
       */
      @Override
      public InitialCamerasEstimatorMethod getMethod() {
          return InitialCamerasEstimatorMethod.DUAL_ABSOLUTE_QUADRIC;
      }
  
      /**
       * Indicates if estimator is ready.
       *
       * @return true if estimator is ready, false otherwise.
       */
      @Override
      public boolean isReady() {
          return fundamentalMatrix != null;
      }
  
      /**
      * Estimates cameras.
      *
      * @throws LockedException                         if estimator is locked.
      * @throws NotReadyException                       if estimator is not ready.
      * @throws InitialCamerasEstimationFailedException if estimation of cameras
      *                                                 fails for some reason, typically due to numerical
      *                                                 instabilities.
      */
     @Override
     public void estimate() throws LockedException, NotReadyException, InitialCamerasEstimationFailedException {
         if (isLocked()) {
             throw new LockedException();
         }
 
         if (!isReady()) {
             throw new NotReadyException();
         }
 
         try {
             locked = true;
 
             if (listener != null) {
                 listener.onStart(this);
             }
 
             if (estimatedLeftCamera == null) {
                 estimatedLeftCamera = new PinholeCamera();
             }
             if (estimatedRightCamera == null) {
                 estimatedRightCamera = new PinholeCamera();
             }
 
             generateInitialMetricCamerasUsingDAQ(fundamentalMatrix, aspectRatio, estimatedLeftCamera,
                     estimatedRightCamera);
 
             if (listener != null) {
                 listener.onFinish(this, estimatedLeftCamera, estimatedRightCamera);
             }
         } catch (final InitialCamerasEstimationFailedException e) {
             if (listener != null) {
                 listener.onFail(this, e);
             }
             throw e;
         } finally {
             locked = false;
         }
     }
 
     /**
      * Gets aspect ratio of intrinsic parameters of cameras.
      * Typically, this value is 1.0 if vertical coordinates increase upwards,
      * or -1.0 if it is the opposite.
      *
      * @return aspect ratio of intrinsic parameters of cameras.
      */
     public double getAspectRatio() {
         return aspectRatio;
     }
 
     /**
      * Sets aspect ratio of intrinsic parameters of cameras.
      * Typically, this value is 1.0 if vertical coordinates increase upwards,
      * or -1.0 if it is the opposite.
      *
      * @param aspectRatio aspect ratio of intrinsic parameters of cameras.
      * @throws LockedException if estimator is locked.
      */
     public void setAspectRatio(final double aspectRatio) throws LockedException {
         if (isLocked()) {
             throw new LockedException();
         }
         this.aspectRatio = aspectRatio;
     }
 
     /**
      * Generates initial cameras in metric stratum (with arbitrary scale) using
      * provided fundamental matrix by means of estimation of the Dual Absolute
      * Quadric and the required projective to metric transformation.
      *
      * @param fundamentalMatrix input fundamental matrix to estimate cameras.
      * @param leftCamera        instance where left camera will be stored.
      * @param rightCamera       instance where right camera will be stored.
      * @throws InitialCamerasEstimationFailedException if estimation fails.
      */
     public static void generateInitialMetricCamerasUsingDAQ(
             final FundamentalMatrix fundamentalMatrix,
             final PinholeCamera leftCamera,
             final PinholeCamera rightCamera)
             throws InitialCamerasEstimationFailedException {
         generateInitialMetricCamerasUsingDAQ(fundamentalMatrix,
                 DualAbsoluteQuadricEstimator.DEFAULT_FOCAL_DISTANCE_ASPECT_RATIO, leftCamera, rightCamera);
     }
 
     /**
      * Generates initial cameras in metric stratum (with arbitrary scale) using
      * provided fundamental matrix by means of estimation of the Dual Absolute
      * Quadric and the required projective to metric transformation.
      *
      * @param fundamentalMatrix input fundamental matrix to estimate cameras.
      * @param aspectRatio       aspect ratio of intrinsic parameters of cameras.
      *                          Typically, this value is 1.0 if vertical coordinates increase upwards or
      *                          -1.0 if it is the opposite.
      * @param leftCamera        instance where left camera will be stored.
      * @param rightCamera       instance where right camera will be stored.
      * @throws InitialCamerasEstimationFailedException if estimation fails.
      */
     public static void generateInitialMetricCamerasUsingDAQ(
             final FundamentalMatrix fundamentalMatrix,
             final double aspectRatio,
             final PinholeCamera leftCamera,
             final PinholeCamera rightCamera)
             throws InitialCamerasEstimationFailedException {
 
         try {
             // generate arbitrary projective cameras
             fundamentalMatrix.generateCamerasInArbitraryProjectiveSpace(leftCamera, rightCamera);
 
             final var cameras = new ArrayList<PinholeCamera>();
             cameras.add(leftCamera);
             cameras.add(rightCamera);
 
             // estimate dual absolute quadric
             final var daqEstimator = new LMSEDualAbsoluteQuadricEstimator(cameras);
             daqEstimator.setLMSESolutionAllowed(false);
             daqEstimator.setZeroSkewness(true);
             daqEstimator.setPrincipalPointAtOrigin(true);
             daqEstimator.setFocalDistanceAspectRatioKnown(true);
             daqEstimator.setFocalDistanceAspectRatio(aspectRatio);
             daqEstimator.setSingularityEnforced(true);
 
             final var daq = daqEstimator.estimate();
             final var transformation = daq.getMetricToProjectiveTransformation();
             // inverse transformation to upgrade cameras from projective to
             // metric stratum
             transformation.inverse();
 
             // transform cameras
             transformation.transform(leftCamera);
             transformation.transform(rightCamera);
         } catch (final Exception e) {
             throw new InitialCamerasEstimationFailedException(e);
         }
     }
 }