/*
    * 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 multiple 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 defining 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 BaseAbsoluteOrientationSlamSparseReconstructor<
          D extends BaseCalibrationData,
          C extends BaseSlamSparseReconstructorConfiguration<D, C>,
          R extends BaseSlamSparseReconstructor<D, C, R, L, S>,
          L extends BaseSlamSparseReconstructorListener<R>,
          S extends AbsoluteOrientationBaseSlamEstimator<D>> extends
          BaseSlamSparseReconstructor<D, C, R, L, S> {
  
      /**
       * First sample of orientation received.
       */
      private Rotation3D firstOrientation;
  
      /**
       * Inverse of first orientation.
       */
      private Rotation3D invFirstOrientation;
  
      /**
       * 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 BaseAbsoluteOrientationSlamSparseReconstructor(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();
              invFirstOrientation = firstOrientation.inverseRotationAndReturnNew();
          }
      }
  
      /**
       * Updates scene scale and orientation using SLAM data.
       *
       * @param isInitialPairOfViews true if initial pair of views is being processed, false otherwise.
       * @return true if scale was successfully updated, false otherwise.
       */
      @SuppressWarnings("DuplicatedCode")
      protected boolean updateScaleAndOrientation(final boolean isInitialPairOfViews) {
  
          try {
             final var metricCamera1 = previousMetricEstimatedCamera.getCamera();
             final var metricCamera2 = currentMetricEstimatedCamera.getCamera();
 
             double slamPosX;
             double slamPosY;
             double slamPosZ;
             double scale;
             if (isInitialPairOfViews) {
                 // obtain baseline (camera separation from slam estimator data
                 slamPosX = slamEstimator.getStatePositionX();
                 slamPosY = slamEstimator.getStatePositionY();
                 slamPosZ = slamEstimator.getStatePositionZ();
 
                 slamPosition.setInhomogeneousCoordinates(slamPosX, slamPosY, slamPosZ);
 
                 metricCamera1.decompose(false, true);
                 metricCamera2.decompose(false, true);
 
                 final var center1 = metricCamera1.getCameraCenter();
                 final var center2 = metricCamera2.getCameraCenter();
 
                 final var baseline = center1.distanceTo(slamPosition);
                 final var estimatedBaseline = center1.distanceTo(center2);
 
                 scale = currentScale = baseline / estimatedBaseline;
             } else {
                 scale = currentScale;
             }
 
             // 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 scaleAndOrientationTransformation = new MetricTransformation3D(scale);
             scaleAndOrientationTransformation.setRotation(invFirstOrientation);
 
             // update scale of cameras
             final var euclideanCamera1 = scaleAndOrientationTransformation.transformAndReturnNew(metricCamera1);
             final var euclideanCamera2 = scaleAndOrientationTransformation.transformAndReturnNew(metricCamera2);
 
             euclideanCamera2.decompose(false, true);
             slamEstimator.correctWithPositionMeasure(euclideanCamera2.getCameraCenter(),
                     configuration.getCameraPositionCovariance());
 
             if (!isInitialPairOfViews) {
                 slamPosX = slamEstimator.getStatePositionX();
                 slamPosY = slamEstimator.getStatePositionY();
                 slamPosZ = slamEstimator.getStatePositionZ();
                 slamPosition.setInhomogeneousCoordinates(slamPosX, slamPosY, slamPosZ);
 
                 // adjust scale of current camera
                 final var euclideanCenter2 = euclideanCamera2.getCameraCenter();
 
                 final var euclideanPosX = euclideanCenter2.getInhomX();
                 final var euclideanPosY = euclideanCenter2.getInhomY();
                 final var euclideanPosZ = euclideanCenter2.getInhomZ();
 
                 final var scaleVariationX = euclideanPosX / slamPosX;
                 final var scaleVariationY = euclideanPosY / slamPosY;
                 final var scaleVariationZ = euclideanPosZ / slamPosZ;
 
                 final var scaleVariation = (scaleVariationX + scaleVariationY + scaleVariationZ) / 3.0;
                 scale *= scaleVariation;
                 currentScale = scale;
                 scaleAndOrientationTransformation.setScale(currentScale);
 
                 // update camera
                 scaleAndOrientationTransformation.transform(metricCamera2, euclideanCamera2);
             }
             final var sqrScale = scale * scale;
 
             previousEuclideanEstimatedCamera = new EstimatedCamera();
             previousEuclideanEstimatedCamera.setCamera(euclideanCamera1);
             previousEuclideanEstimatedCamera.setViewId(previousMetricEstimatedCamera.getViewId());
             previousEuclideanEstimatedCamera.setQualityScore(previousMetricEstimatedCamera.getQualityScore());
             if (previousMetricEstimatedCamera.getCovariance() != null) {
                 previousEuclideanEstimatedCamera.setCovariance(previousMetricEstimatedCamera.getCovariance()
                         .multiplyByScalarAndReturnNew(sqrScale));
             }
 
             currentEuclideanEstimatedCamera = new EstimatedCamera();
             currentEuclideanEstimatedCamera.setCamera(euclideanCamera2);
             currentEuclideanEstimatedCamera.setViewId(currentMetricEstimatedCamera.getViewId());
             currentEuclideanEstimatedCamera.setQualityScore(currentMetricEstimatedCamera.getQualityScore());
             if (currentMetricEstimatedCamera.getCovariance() != null) {
                 currentEuclideanEstimatedCamera.setCovariance(currentMetricEstimatedCamera.getCovariance()
                         .multiplyByScalarAndReturnNew(sqrScale));
             }
 
             // update scale of reconstructed points
             final var numPoints = activeMetricReconstructedPoints.size();
             final var metricReconstructedPoints3D = new ArrayList<Point3D>();
             for (final var reconstructedPoint : activeMetricReconstructedPoints) {
                 metricReconstructedPoints3D.add(reconstructedPoint.getPoint());
             }
 
             final var euclideanReconstructedPoints3D = scaleAndOrientationTransformation.transformPointsAndReturnNew(
                     metricReconstructedPoints3D);
 
             // set scaled points into result
             activeEuclideanReconstructedPoints = new ArrayList<>();
             ReconstructedPoint3D euclideanPoint;
             ReconstructedPoint3D metricPoint;
             for (var i = 0; i < numPoints; i++) {
                 metricPoint = activeMetricReconstructedPoints.get(i);
 
                 euclideanPoint = new ReconstructedPoint3D();
                 euclideanPoint.setId(metricPoint.getId());
                 euclideanPoint.setPoint(euclideanReconstructedPoints3D.get(i));
                 euclideanPoint.setInlier(metricPoint.isInlier());
                 euclideanPoint.setQualityScore(metricPoint.getQualityScore());
                 if (metricPoint.getCovariance() != null) {
                     euclideanPoint.setCovariance(metricPoint.getCovariance().multiplyByScalarAndReturnNew(sqrScale));
                 }
                 euclideanPoint.setColorData(metricPoint.getColorData());
 
                 activeEuclideanReconstructedPoints.add(euclideanPoint);
             }
 
             return true;
 
         } catch (final Exception e) {
             failed = true;
             //noinspection unchecked
             listener.onFail((R) this);
 
             return false;
         }
     }
 }