/*
    * 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.algebra.AlgebraException;
  import com.irurueta.ar.slam.BaseCalibrationData;
  import com.irurueta.ar.slam.BaseSlamEstimator;
  import com.irurueta.geometry.GeometryException;
  import com.irurueta.geometry.InhomogeneousPoint3D;
  import com.irurueta.geometry.MetricTransformation3D;
  import com.irurueta.geometry.PinholeCamera;
  import com.irurueta.geometry.PinholeCameraIntrinsicParameters;
  import com.irurueta.geometry.Quaternion;
  import com.irurueta.geometry.Rotation3D;
  
  import java.util.ArrayList;
  
  @SuppressWarnings("DuplicatedCode")
  public abstract class BaseSlamPairedViewsSparseReconstructor<
          D extends BaseCalibrationData,
          C extends BaseSlamPairedViewsSparseReconstructorConfiguration<D, C>,
          R extends BaseSlamPairedViewsSparseReconstructor<D, C, R, L, S>,
          L extends BaseSlamPairedViewsSparseReconstructorListener<R>,
          S extends BaseSlamEstimator<D>> extends BasePairedViewsSparseReconstructor<C, R, L> {
  
      /**
       * Slam estimator to estimate position, speed, orientation using
       * accelerometer and gyroscope data.
       */
      protected S slamEstimator;
  
      /**
       * Position estimated by means of SLAM. It is reused each time it is notified.
       */
      private final InhomogeneousPoint3D slamPosition = new InhomogeneousPoint3D();
  
      /**
       * Inverse Euclidean camera rotation. This is reused for memory efficiency.
       */
      private Rotation3D invEuclideanCameraRotation;
  
      /**
       * Camera estimated by means of SLAM. It is reused each time it is notified.
       */
      private final PinholeCamera slamCamera = new PinholeCamera();
  
      /**
       * Camera rotation estimated by means of SLAM. It is reused each time it is notified.
       */
      private final Quaternion slamRotation = new Quaternion();
  
      /**
       * Last SLAM timestamp.
       */
      private long lastTimestamp = -1;
  
      /**
       * Last view pair SLAM timestamp.
       */
      private long lastViewPairTimestamp = -1;
  
      /**
       * 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 BaseSlamPairedViewsSparseReconstructor(final C configuration, final L listener) {
          super(configuration, listener);
      }
  
      /**
       * Provides a new accelerometer sample to update SLAM estimation.
       * This method must be called whenever the accelerometer sensor receives new
       * data.
       * 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 accelerationX linear acceleration along x-axis expressed in meters
       *                      per squared second (m/s^2).
       * @param accelerationY linear acceleration along y-axis expressed in meters
       *                      per squared second (m/s^2).
       * @param accelerationZ linear acceleration along z-axis expressed in meters
      *                      per squared second (m/s^2).
      */
     public void updateAccelerometerSample(final long timestamp, final float accelerationX,
                                           final float accelerationY, final float accelerationZ) {
         if (lastViewPairTimestamp < 0) {
             lastViewPairTimestamp = timestamp;
         }
 
         if (slamEstimator != null) {
             slamEstimator.updateAccelerometerSample(timestamp - lastViewPairTimestamp,
                     accelerationX, accelerationY, accelerationZ);
         }
         lastTimestamp = timestamp;
     }
 
     /**
      * Provides a new accelerometer sample to update SLAM estimation.
      * This method must be called whenever the accelerometer sensor receives new
      * data.
      * 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 data      array containing x,y,z components of linear acceleration
      *                  expressed in meters per squared second (m/s^2).
      * @throws IllegalArgumentException if provided array does not have length
      *                                  3.
      */
     public void updateAccelerometerSample(final long timestamp, final float[] data) {
         if (lastViewPairTimestamp < 0) {
             lastViewPairTimestamp = timestamp;
         }
 
         if (slamEstimator != null) {
             slamEstimator.updateAccelerometerSample(timestamp - lastViewPairTimestamp, data);
         }
         lastTimestamp = timestamp;
     }
 
     /**
      * Provides a new gyroscope sample to update SLAM estimation.
      * If re-constructor is not running, calling this method has no effect.
      *
      * @param timestamp     timestamp of gyroscope sample since epoch time and
      *                      expressed in nanoseconds.
      * @param angularSpeedX angular speed of rotation along x-axis expressed in
      *                      radians per second (rad/s).
      * @param angularSpeedY angular speed of rotation along y-axis expressed in
      *                      radians per second (rad/s).
      * @param angularSpeedZ angular speed of rotation along z-axis expressed in
      *                      radians per second (rad/s).
      */
     public void updateGyroscopeSample(final long timestamp, final float angularSpeedX,
                                       final float angularSpeedY, final float angularSpeedZ) {
         if (lastViewPairTimestamp < 0) {
             lastViewPairTimestamp = timestamp;
         }
 
         if (slamEstimator != null) {
             slamEstimator.updateGyroscopeSample(timestamp - lastViewPairTimestamp,
                     angularSpeedX, angularSpeedY, angularSpeedZ);
         }
         lastTimestamp = timestamp;
     }
 
     /**
      * Provides a new gyroscope sample to update SLAM estimation.
      * If re-constructor is not running, calling this method has no effect.
      *
      * @param timestamp timestamp of gyroscope sample since epoch time and
      *                  expressed in nanoseconds.
      * @param data      angular speed of rotation along x,y,z axes expressed in
      *                  radians per second (rad/s).
      * @throws IllegalArgumentException if provided array does not have length
      *                                  3.
      */
     public void updateGyroscopeSample(final long timestamp, final float[] data) {
         if (lastViewPairTimestamp < 0) {
             lastViewPairTimestamp = timestamp;
         }
 
         if (slamEstimator != null) {
             slamEstimator.updateGyroscopeSample(timestamp - lastViewPairTimestamp, data);
         }
         lastTimestamp = timestamp;
     }
 
     /**
      * Resets this instance so that a reconstruction can be started from the beginning without
      * cancelling current one.
      */
     @Override
     public void reset() {
         super.reset();
         lastTimestamp = lastViewPairTimestamp = -1;
     }
 
     /**
      * Indicates whether implementations of a re-constructor uses absolute orientation or
      * not.
      *
      * @return true if absolute orientation is used, false, otherwise.
      */
     @Override
     protected boolean hasAbsoluteOrientation() {
         return false;
     }
 
 
     /**
      * Configures calibration data on SLAM estimator if available.
      */
     protected void setUpCalibrationData() {
         final var calibrationData = configuration.getCalibrationData();
         if (calibrationData != null) {
             slamEstimator.setCalibrationData(calibrationData);
         }
     }
 
     /**
      * Configures listener of SLAM estimator
      */
     protected void setUpSlamEstimatorListener() {
         slamEstimator.setListener(new BaseSlamEstimator.BaseSlamEstimatorListener<>() {
             @Override
             public void onFullSampleReceived(final BaseSlamEstimator<D> estimator) {
                 // not used
             }
 
             @Override
             public void onFullSampleProcessed(final BaseSlamEstimator<D> estimator) {
                 notifySlamStateAndCamera();
             }
 
             @Override
             public void onCorrectWithPositionMeasure(final BaseSlamEstimator<D> estimator) {
                 // not used
             }
 
             @Override
             public void onCorrectedWithPositionMeasure(final BaseSlamEstimator<D> estimator) {
                 notifySlamStateAndCamera();
             }
 
             private void notifySlamStateAndCamera() {
                 notifySlamStateIfNeeded();
                 notifySlamCameraIfNeeded();
             }
         });
     }
 
     /**
      * Transforms metric cameras on current pair of views so that they are referred to
      * last kept location and rotation and upgrades cameras from metric stratum to
      * Euclidean stratum.
      *
      * @param isInitialPairOfViews   true if initial pair of views is being processed, false otherwise.
      * @param hasAbsoluteOrientation true if absolute orientation is required, false otherwise.
      * @return true if cameras were successfully transformed.
      */
     @Override
     protected boolean transformPairOfCamerasAndPoints(final boolean isInitialPairOfViews,
                                                       final boolean hasAbsoluteOrientation) {
         final var previousMetricCamera = previousMetricEstimatedCamera.getCamera();
         final var currentMetricCamera = currentMetricEstimatedCamera.getCamera();
         if (previousMetricCamera == null || currentMetricCamera == null) {
             return false;
         }
 
         currentScale = estimateCurrentScale();
         final var sqrScale = currentScale * currentScale;
 
         referenceEuclideanTransformation = new MetricTransformation3D(currentScale);
         if (hasAbsoluteOrientation) {
             invEuclideanCameraRotation = lastEuclideanCameraRotation.inverseRotationAndReturnNew();
             if (isInitialPairOfViews) {
                 referenceEuclideanTransformation.setRotation(invEuclideanCameraRotation);
             }
         }
 
         if (!isInitialPairOfViews) {
             // additional pairs also need to translate and rotate
             if (invEuclideanCameraRotation == null) {
                 invEuclideanCameraRotation = lastEuclideanCameraRotation.inverseRotationAndReturnNew();
             } else {
                 lastEuclideanCameraRotation.inverseRotation(invEuclideanCameraRotation);
             }
             referenceEuclideanTransformation.setRotation(invEuclideanCameraRotation);
             referenceEuclideanTransformation.setTranslation(lastEuclideanCameraCenter);
         }
 
         try {
             // transform cameras
             final var previousEuclideanCamera = referenceEuclideanTransformation.transformAndReturnNew(
                     previousMetricCamera);
             final var currentEuclideanCamera = referenceEuclideanTransformation.transformAndReturnNew(
                     currentMetricCamera);
 
             previousEuclideanEstimatedCamera = new EstimatedCamera();
             previousEuclideanEstimatedCamera.setCamera(previousEuclideanCamera);
             previousEuclideanEstimatedCamera.setViewId(previousMetricEstimatedCamera.getViewId());
             previousEuclideanEstimatedCamera.setQualityScore(previousMetricEstimatedCamera.getQualityScore());
             if (previousMetricEstimatedCamera.getCovariance() != null) {
                 previousEuclideanEstimatedCamera.setCovariance(previousMetricEstimatedCamera.getCovariance()
                         .multiplyByScalarAndReturnNew(sqrScale));
             }
 
             currentEuclideanEstimatedCamera = new EstimatedCamera();
             currentEuclideanEstimatedCamera.setCamera(currentEuclideanCamera);
             currentEuclideanEstimatedCamera.setViewId(currentMetricEstimatedCamera.getViewId());
             currentEuclideanEstimatedCamera.setQualityScore(currentMetricEstimatedCamera.getQualityScore());
             if (currentMetricEstimatedCamera.getCovariance() != null) {
                 currentEuclideanEstimatedCamera.setCovariance(currentMetricEstimatedCamera.getCovariance()
                         .multiplyByScalarAndReturnNew(sqrScale));
             }
 
             // transform points
             euclideanReconstructedPoints = new ArrayList<>();
             ReconstructedPoint3D euclideanReconstructedPoint;
             for (final ReconstructedPoint3D metricReconstructedPoint : metricReconstructedPoints) {
                 final var metricPoint = metricReconstructedPoint.getPoint();
                 final var euclideanPoint = referenceEuclideanTransformation.transformAndReturnNew(metricPoint);
                 euclideanReconstructedPoint = new ReconstructedPoint3D();
                 euclideanReconstructedPoint.setPoint(euclideanPoint);
                 euclideanReconstructedPoint.setInlier(metricReconstructedPoint.isInlier());
                 euclideanReconstructedPoint.setId(metricReconstructedPoint.getId());
                 euclideanReconstructedPoint.setColorData(metricReconstructedPoint.getColorData());
                 if (metricReconstructedPoint.getCovariance() != null) {
                     euclideanReconstructedPoint.setCovariance(metricReconstructedPoint.getCovariance()
                             .multiplyByScalarAndReturnNew(sqrScale));
                 }
                 euclideanReconstructedPoint.setQualityScore(metricReconstructedPoint.getQualityScore());
                 euclideanReconstructedPoints.add(euclideanReconstructedPoint);
             }
 
         } catch (final AlgebraException e) {
             return false;
         }
 
         return super.transformPairOfCamerasAndPoints(isInitialPairOfViews, hasAbsoluteOrientation);
     }
 
     /**
      * Estimates current scale using SLAM data.
      *
      * @return estimated scale.
      */
     private double estimateCurrentScale() {
         try {
             final var metricCamera1 = previousMetricEstimatedCamera.getCamera();
             final var metricCamera2 = currentMetricEstimatedCamera.getCamera();
 
             if (!metricCamera1.isCameraCenterAvailable()) {
                 metricCamera1.decompose(false, true);
             }
             if (!metricCamera2.isCameraCenterAvailable()) {
                 metricCamera2.decompose(false, true);
             }
 
             final var metricCenter1 = metricCamera1.getCameraCenter();
             final var metricCenter2 = metricCamera2.getCameraCenter();
 
             // obtain baseline (camera separation from slam estimator data)
             final var slamPosX = slamEstimator.getStatePositionX();
             final var slamPosY = slamEstimator.getStatePositionY();
             final var slamPosZ = slamEstimator.getStatePositionZ();
 
             slamPosition.setInhomogeneousCoordinates(slamPosX, slamPosY, slamPosZ);
 
             // we assume that Euclidean center of 1st camera is at origin because by the time
             // this method is called, metric cameras have not yet been orientation and position
             // transformed
             final var euclideanBaseline = Math.sqrt(slamPosX * slamPosX + slamPosY * slamPosY + slamPosZ * slamPosZ);
             final var metricBaseline = metricCenter1.distanceTo(metricCenter2);
 
             // because when we call reset in SLAM estimator, timestamp is lost, we keep
             // track of last timestamp to be subtracted on subsequent update calls
             lastViewPairTimestamp = lastTimestamp;
 
             // reset linear velocity and orientation and keep other slam state parameters
             slamEstimator.resetPositionAndVelocity();
 
             return euclideanBaseline / metricBaseline;
         } catch (final Exception e) {
             failed = true;
             //noinspection unchecked
             listener.onFail((R) this);
             return DEFAULT_SCALE;
         }
     }
 
     /**
      * Notifies SLAM state if notification is enabled at configuration time.
      */
     private void notifySlamStateIfNeeded() {
         if (!configuration.isNotifyAvailableSlamDataEnabled()) {
             return;
         }
 
         final var lastPosX = lastEuclideanCameraCenter != null ? lastEuclideanCameraCenter.getInhomX() : 0.0;
         final var lastPosY = lastEuclideanCameraCenter != null ? lastEuclideanCameraCenter.getInhomY() : 0.0;
         final var lastPosZ = lastEuclideanCameraCenter != null ? lastEuclideanCameraCenter.getInhomZ() : 0.0;
 
         final var positionX = lastPosX + slamEstimator.getStatePositionX();
         final var positionY = lastPosY + slamEstimator.getStatePositionY();
         final var positionZ = lastPosZ + slamEstimator.getStatePositionZ();
 
         final var velocityX = slamEstimator.getStateVelocityX();
         final var velocityY = slamEstimator.getStateVelocityY();
         final var velocityZ = slamEstimator.getStateVelocityZ();
 
         final var accelerationX = slamEstimator.getStateAccelerationX();
         final var accelerationY = slamEstimator.getStateAccelerationY();
         final var accelerationZ = slamEstimator.getStateAccelerationZ();
 
         final var quaternionA = slamEstimator.getStateQuaternionA();
         final var quaternionB = slamEstimator.getStateQuaternionB();
         final var quaternionC = slamEstimator.getStateQuaternionC();
         final var quaternionD = slamEstimator.getStateQuaternionD();
 
         final var angularSpeedX = slamEstimator.getStateAngularSpeedX();
         final var angularSpeedY = slamEstimator.getStateAngularSpeedY();
         final var angularSpeedZ = slamEstimator.getStateAngularSpeedZ();
 
         //noinspection unchecked
         listener.onSlamDataAvailable((R) this, positionX, positionY, positionZ, velocityX, velocityY, velocityZ,
                 accelerationX, accelerationY, accelerationZ, quaternionA, quaternionB, quaternionC, quaternionD,
                 angularSpeedX, angularSpeedY, angularSpeedZ, slamEstimator.getStateCovariance());
     }
 
     /**
      * Notifies estimated camera by means of SLAM if notification is enabled at
      * configuration time and intrinsics are already available.
      */
     private void notifySlamCameraIfNeeded() {
         if (!configuration.isNotifyEstimatedSlamCameraEnabled()) {
             return;
         }
 
         // try with current camera
         var camera = currentEuclideanEstimatedCamera != null ? currentEuclideanEstimatedCamera.getCamera() : null;
         if (camera == null) {
             // if not available try with previous camera
             camera = previousEuclideanEstimatedCamera != null ? previousEuclideanEstimatedCamera.getCamera() : null;
         }
 
         try {
             PinholeCameraIntrinsicParameters intrinsicParameters = null;
             if (camera != null) {
                 if (!camera.areIntrinsicParametersAvailable()) {
                     // decompose camera to obtain intrinsic parameters
                     camera.decompose();
                 }
 
                 intrinsicParameters = camera.getIntrinsicParameters();
             } else if (configuration.areIntrinsicParametersKnown()) {
                 //noinspection unchecked
                 intrinsicParameters = listener.onIntrinsicParametersRequested((R) this, currentViewId);
             }
 
             if (intrinsicParameters == null) {
                 return;
             }
 
             final var lastPosX = lastEuclideanCameraCenter != null ? lastEuclideanCameraCenter.getInhomX() : 0.0;
             final var lastPosY = lastEuclideanCameraCenter != null ? lastEuclideanCameraCenter.getInhomY() : 0.0;
             final var lastPosZ = lastEuclideanCameraCenter != null ? lastEuclideanCameraCenter.getInhomZ() : 0.0;
 
             final var positionX = lastPosX + slamEstimator.getStatePositionX();
             final var positionY = lastPosY + slamEstimator.getStatePositionY();
             final var positionZ = lastPosZ + slamEstimator.getStatePositionZ();
             slamPosition.setInhomogeneousCoordinates(positionX, positionY, positionZ);
 
             final var quaternionA = slamEstimator.getStateQuaternionA();
             final var quaternionB = slamEstimator.getStateQuaternionB();
             final var quaternionC = slamEstimator.getStateQuaternionC();
             final var quaternionD = slamEstimator.getStateQuaternionD();
             slamRotation.setA(quaternionA);
             slamRotation.setB(quaternionB);
             slamRotation.setC(quaternionC);
             slamRotation.setD(quaternionD);
 
             slamCamera.setIntrinsicAndExtrinsicParameters(intrinsicParameters, slamRotation, slamPosition);
 
             //noinspection unchecked
             listener.onSlamCameraEstimated((R) this, slamCamera);
 
         } catch (final GeometryException ignore) {
             // do nothing
         }
     }
 }