/*
    * 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.slam.AbsoluteOrientationBaseSlamEstimator;
  import com.irurueta.ar.slam.BaseCalibrationData;
  import com.irurueta.geometry.MetricTransformation3D;
  import com.irurueta.geometry.Point3D;
  import com.irurueta.geometry.Rotation3D;
  
  import java.util.ArrayList;
  
  /**
   * Base class in charge of estimating cameras and 3D reconstructed points from
   * sparse image point correspondences in two views and also in charge of
   * estimating overall scene scale and absolute orientation by means of SLAM
   * (Simultaneous Location And Mapping) using data obtained from sensors like
   * accelerometers or gyroscopes.
   * NOTE: absolute orientation slam estimators are not very accurate during
   * estimation of the orientation state, for that reason we take into account
   * the initial orientation.
   *
   * @param <D> type of calibration data.
   * @param <C> type of configuration.
   * @param <R> type of re-constructor.
   * @param <L> type of listener.
   * @param <S> type of SLAM estimator.
   */
  public abstract class BaseAbsoluteOrientationSlamTwoViewsSparseReconstructor<
          D extends BaseCalibrationData,
          C extends BaseSlamTwoViewsSparseReconstructorConfiguration<D, C>,
          R extends BaseSlamTwoViewsSparseReconstructor<D, C, R, L, S>,
          L extends BaseSlamTwoViewsSparseReconstructorListener<R>,
          S extends AbsoluteOrientationBaseSlamEstimator<D>> extends
          BaseSlamTwoViewsSparseReconstructor<D, C, R, L, S> {
  
      /**
       * First sample of orientation received.
       */
      protected Rotation3D firstOrientation;
  
      /**
       * Constructor.
       *
       * @param configuration configuration for this re-constructor.
       * @param listener      listener in charge of handling events.
       * @throws NullPointerException if listener or configuration is not
       *                              provided.
       */
      protected BaseAbsoluteOrientationSlamTwoViewsSparseReconstructor(
              final C configuration, final L listener) {
          super(configuration, listener);
      }
  
      /**
       * Provides a new orientation sample to update SLAM estimator.
       * If re-constructor is not running, calling this method has no effect.
       *
       * @param timestamp   timestamp of accelerometer sample since epoch time and
       *                    expressed in nanoseconds.
       * @param orientation new orientation.
       */
      public void updateOrientationSample(final long timestamp,
                                          final Rotation3D orientation) {
          if (slamEstimator != null) {
              slamEstimator.updateOrientationSample(timestamp, orientation);
          }
          if (firstOrientation == null) {
              //make a copy of orientation
              firstOrientation = orientation.toQuaternion();
          }
      }
  
      /**
       * Updates scene scale and orientation using SLAM data.
       *
       * @return true if scale was successfully updated, false otherwise.
       */
      @SuppressWarnings("DuplicatedCode")
      protected boolean updateScaleAndOrientation() {
  
          // obtain baseline (camera separation from slam estimator data
          final var posX = slamEstimator.getStatePositionX();
          final var posY = slamEstimator.getStatePositionY();
          final var posZ = slamEstimator.getStatePositionZ();
  
         // to estimate baseline, we assume that first camera is placed at
         // world origin
         final var baseline = Math.sqrt(posX * posX + posY * posY + posZ * posZ);
 
         try {
             final var camera1 = estimatedCamera1.getCamera();
             final var camera2 = estimatedCamera2.getCamera();
 
             camera1.decompose();
             camera2.decompose();
 
             final var center1 = camera1.getCameraCenter();
             final var center2 = camera2.getCameraCenter();
 
             // R1' = R1*Rdiff
             // Rdiff = R1^T*R1'
 
             // where R1' is the desired orientation (obtained by sampling a
             // sensor)
             // and R1 is always the identity for the 1st camera.
             // Hence R1' = Rdiff
 
             // t1' is the desired translation which is zero for the 1st
             // camera.
 
             // We want: P1' = K*[R1' t1'] = K*[R1' 0]
             // And we have P1 = K[I 0]
 
             // We need a transformation T so that:
             // P1' = P1*T^-1 = K[I 0][R1' 0]
             //                      [0   1]
 
             // Hence: T^-1 = [R1' 0]
             //              [0   1]
 
             // or T = [R1'^T 0]
             //       [0     1]
 
             // because we are also applying a transformation of scale s,
             // the combination of both transformations is
             // T = [s*R1'^T 0]
             //     [0       1]
 
             final var r = firstOrientation.inverseRotationAndReturnNew();
 
             final var estimatedBaseline = center1.distanceTo(center2);
 
             final var scale = baseline / estimatedBaseline;
 
             final var scaleAndOrientationTransformation = new MetricTransformation3D(scale);
             scaleAndOrientationTransformation.setRotation(r);
 
             // update scale of cameras
             scaleAndOrientationTransformation.transform(camera1);
             scaleAndOrientationTransformation.transform(camera2);
 
             estimatedCamera1.setCamera(camera1);
             estimatedCamera2.setCamera(camera2);
 
             // update scale of reconstructed points
             final var numPoints = reconstructedPoints.size();
             final var reconstructedPoints3D = new ArrayList<Point3D>();
             for (final var reconstructedPoint : reconstructedPoints) {
                 reconstructedPoints3D.add(reconstructedPoint.getPoint());
             }
 
             scaleAndOrientationTransformation.transformAndOverwritePoints(reconstructedPoints3D);
 
             // set scaled points into result
             for (var i = 0; i < numPoints; i++) {
                 reconstructedPoints.get(i).setPoint(reconstructedPoints3D.get(i));
             }
 
             return true;
         } catch (final Exception e) {
             failed = true;
             //noinspection unchecked
             listener.onFail((R) this);
 
             return false;
         }
     }
 }