/*
    * Copyright (C) 2021 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.navigation.inertial.calibration;
  
  import com.irurueta.navigation.NotReadyException;
  import com.irurueta.navigation.inertial.INSLooselyCoupledKalmanConfig;
  import com.irurueta.navigation.inertial.calibration.generators.AccelerometerAndGyroscopeMeasurementsGenerator;
  import com.irurueta.navigation.inertial.calibration.generators.AccelerometerGyroscopeAndMagnetometerMeasurementsGenerator;
  import com.irurueta.navigation.inertial.calibration.intervals.thresholdfactor.AccelerometerAndGyroscopeIntervalDetectorThresholdFactorOptimizer;
  import com.irurueta.navigation.inertial.calibration.intervals.thresholdfactor.AccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer;
  
  /**
   * Utility class to create {@link INSLooselyCoupledKalmanConfig} by combining
   * different sources of estimated data.
   * Sources of data can be any measurement generator, static interval detector or
   * noise estimator implementing {@link AccelerometerNoiseRootPsdSource}
   * or {@link GyroscopeNoiseRootPsdSource}.
   */
  public class INSLooselyCoupledKalmanConfigCreator {
  
      /**
       * A source of estimated accelerometer noise root PSD.
       */
      private AccelerometerNoiseRootPsdSource accelerometerNoiseRootPsdSource;
  
      /**
       * A source of estimated gyroscope noise root PSD.
       */
      private GyroscopeNoiseRootPsdSource gyroscopeNoiseRootPsdSource;
  
      /**
       * A source of estimated accelerometer bias random walk PSD.
       */
      private AccelerometerBiasRandomWalkSource accelerometerBiasRandomWalkSource;
  
      /**
       * A source of estimated gyroscope bias random walk PSD.
       */
      private GyroscopeBiasRandomWalkSource gyroscopeBiasRandomWalkSource;
  
      /**
       * A source of position noise standard deviation.
       */
      private PositionNoiseStandardDeviationSource positionNoiseStandardDeviationSource;
  
      /**
       * A source of velocity noise standard deviation.
       */
      private VelocityNoiseStandardDeviationSource velocityNoiseStandardDeviationSource;
  
      /**
       * Constructor.
       */
      public INSLooselyCoupledKalmanConfigCreator() {
      }
  
      /**
       * Constructor.
       *
       * @param accelerometerNoiseRootPsdSource      a source of estimated accelerometer noise root PSD.
       * @param gyroscopeNoiseRootPsdSource          a source of estimated gyroscope noise root PSD.
       * @param accelerometerBiasRandomWalkSource    a source of estimated accelerometer bias random walk PSD.
       * @param gyroscopeBiasRandomWalkSource        a source of estimated gyroscope bias random walk PSD.
       * @param positionNoiseStandardDeviationSource a source of position noise standard deviation.
       * @param velocityNoiseStandardDeviationSource a source of velocity noise standard deviation.
       */
      public INSLooselyCoupledKalmanConfigCreator(
              final AccelerometerNoiseRootPsdSource accelerometerNoiseRootPsdSource,
              final GyroscopeNoiseRootPsdSource gyroscopeNoiseRootPsdSource,
              final AccelerometerBiasRandomWalkSource accelerometerBiasRandomWalkSource,
              final GyroscopeBiasRandomWalkSource gyroscopeBiasRandomWalkSource,
              final PositionNoiseStandardDeviationSource positionNoiseStandardDeviationSource,
              final VelocityNoiseStandardDeviationSource velocityNoiseStandardDeviationSource) {
          this.accelerometerNoiseRootPsdSource = accelerometerNoiseRootPsdSource;
          this.gyroscopeNoiseRootPsdSource = gyroscopeNoiseRootPsdSource;
          this.accelerometerBiasRandomWalkSource = accelerometerBiasRandomWalkSource;
          this.gyroscopeBiasRandomWalkSource = gyroscopeBiasRandomWalkSource;
          this.positionNoiseStandardDeviationSource = positionNoiseStandardDeviationSource;
          this.velocityNoiseStandardDeviationSource = velocityNoiseStandardDeviationSource;
      }
  
      /**
       * Constructor.
       *
       * @param generator           an accelerometer + gyroscope measurement
       *                            generator.
      * @param randomWalkEstimator a random walk estimator.
      */
     public INSLooselyCoupledKalmanConfigCreator(
             final AccelerometerAndGyroscopeMeasurementsGenerator generator,
             final RandomWalkEstimator randomWalkEstimator) {
         this(generator, generator, randomWalkEstimator, randomWalkEstimator, randomWalkEstimator, randomWalkEstimator);
     }
 
     /**
      * Constructor.
      *
      * @param generator           an accelerometer + gyroscope + magnetometer
      *                            measurement generator.
      * @param randomWalkEstimator a random walk estimator.
      */
     public INSLooselyCoupledKalmanConfigCreator(
             final AccelerometerGyroscopeAndMagnetometerMeasurementsGenerator generator,
             final RandomWalkEstimator randomWalkEstimator) {
         this(generator, generator, randomWalkEstimator, randomWalkEstimator, randomWalkEstimator, randomWalkEstimator);
     }
 
     /**
      * Constructor.
      *
      * @param optimizer           an accelerometer and gyroscope threshold factor
      *                            optimizer.
      * @param randomWalkEstimator a random walk estimator.
      */
     public INSLooselyCoupledKalmanConfigCreator(
             final AccelerometerAndGyroscopeIntervalDetectorThresholdFactorOptimizer optimizer,
             final RandomWalkEstimator randomWalkEstimator) {
         this(optimizer, optimizer, randomWalkEstimator, randomWalkEstimator, randomWalkEstimator, randomWalkEstimator);
     }
 
     /**
      * Constructor.
      *
      * @param optimizer           an accelerometer + gyroscope + magnetometer
      *                            threshold factor optimizer.
      * @param randomWalkEstimator a random walk estimator.
      */
     public INSLooselyCoupledKalmanConfigCreator(
             final AccelerometerGyroscopeAndMagnetometerIntervalDetectorThresholdFactorOptimizer optimizer,
             final RandomWalkEstimator randomWalkEstimator) {
         this(optimizer, optimizer, randomWalkEstimator, randomWalkEstimator, randomWalkEstimator, randomWalkEstimator);
     }
 
     /**
      * Gets the source of estimated accelerometer noise root PSD.
      *
      * @return source of estimated accelerometer noise root PSD.
      */
     public AccelerometerNoiseRootPsdSource getAccelerometerNoiseRootPsdSource() {
         return accelerometerNoiseRootPsdSource;
     }
 
     /**
      * Sets source of estimated accelerometer noise root PSD.
      *
      * @param accelerometerNoiseRootPsdSource source of estimated accelerometer
      *                                        noise root PSD.
      */
     public void setAccelerometerNoiseRootPsdSource(
             final AccelerometerNoiseRootPsdSource accelerometerNoiseRootPsdSource) {
         this.accelerometerNoiseRootPsdSource = accelerometerNoiseRootPsdSource;
     }
 
     /**
      * Gets the source of estimated gyroscope noise root PSD.
      *
      * @return source of estimated gyroscope noise root PSD.
      */
     public GyroscopeNoiseRootPsdSource getGyroscopeNoiseRootPsdSource() {
         return gyroscopeNoiseRootPsdSource;
     }
 
     /**
      * Sets source of estimated gyroscope noise root PSD.
      *
      * @param gyroscopeNoiseRootPsdSource source of estimated gyroscope noise
      *                                    root PSD.
      */
     public void sstGyroscopeNoiseRootPsdSource(final GyroscopeNoiseRootPsdSource gyroscopeNoiseRootPsdSource) {
         this.gyroscopeNoiseRootPsdSource = gyroscopeNoiseRootPsdSource;
     }
 
     /**
      * Gets the source of estimated accelerometer bias random walk PSD.
      *
      * @return source of estimated accelerometer bias random walk PSD.
      */
     public AccelerometerBiasRandomWalkSource getAccelerometerBiasRandomWalkSource() {
         return accelerometerBiasRandomWalkSource;
     }
 
     /**
      * Sets source of estimated accelerometer bias random walk PSD.
      *
      * @param accelerometerBiasRandomWalkSource source of estimated accelerometer
      *                                          bias random walk PSD.
      */
     public void setAccelerometerBiasRandomWalkSource(
             final AccelerometerBiasRandomWalkSource accelerometerBiasRandomWalkSource) {
         this.accelerometerBiasRandomWalkSource = accelerometerBiasRandomWalkSource;
     }
 
     /**
      * Gets the source of estimated gyroscope bias random walk PSD.
      *
      * @return source of estimated gyroscope bias random walk PSD.
      */
     public GyroscopeBiasRandomWalkSource getGyroscopeBiasRandomWalkSource() {
         return gyroscopeBiasRandomWalkSource;
     }
 
     /**
      * Sets source of estimated gyroscope bias random walk PSD.
      *
      * @param gyroscopeBiasRandomWalkSource source of estimated gyroscope bias
      *                                      random walk PSD.
      */
     public void setGyroscopeBiasRandomWalkSource(final GyroscopeBiasRandomWalkSource gyroscopeBiasRandomWalkSource) {
         this.gyroscopeBiasRandomWalkSource = gyroscopeBiasRandomWalkSource;
     }
 
     /**
      * Gets the source of position noise standard deviation.
      *
      * @return source of position noise standard deviation.
      */
     public PositionNoiseStandardDeviationSource getPositionNoiseStandardDeviationSource() {
         return positionNoiseStandardDeviationSource;
     }
 
     /**
      * Sets source of position noise standard deviation.
      *
      * @param positionUncertaintySource source of position noise standard
      *                                  deviation.
      */
     public void setPositionNoiseStandardDeviationSource(
             final PositionNoiseStandardDeviationSource positionUncertaintySource) {
         positionNoiseStandardDeviationSource = positionUncertaintySource;
     }
 
     /**
      * Gets the source of velocity noise standard deviation.
      *
      * @return source of velocity noise standard deviation.
      */
     public VelocityNoiseStandardDeviationSource getVelocityNoiseStandardDeviationSource() {
         return velocityNoiseStandardDeviationSource;
     }
 
     /**
      * Sets source of velocity noise standard deviation.
      *
      * @param velocityUncertaintySource source of velocity noise standard
      *                                  deviation.
      */
     public void setVelocityNoiseStandardDeviationSource(
             final VelocityNoiseStandardDeviationSource velocityUncertaintySource) {
         velocityNoiseStandardDeviationSource = velocityUncertaintySource;
     }
 
     /**
      * Indicates whether all sources have been provided to be able to
      * create a {@link INSLooselyCoupledKalmanConfig} instance.
      *
      * @return true if the creator is ready, false otherwise.
      */
     public boolean isReady() {
         return accelerometerNoiseRootPsdSource != null
                 && gyroscopeNoiseRootPsdSource != null
                 && accelerometerBiasRandomWalkSource != null
                 && gyroscopeBiasRandomWalkSource != null
                 && positionNoiseStandardDeviationSource != null
                 && velocityNoiseStandardDeviationSource != null;
     }
 
     /**
      * Creates a {@link INSLooselyCoupledKalmanConfig} instance containing estimated
      * parameters during calibration.
      *
      * @return instance containing configuration data.
      * @throws NotReadyException if the creator is not ready.
      */
     public INSLooselyCoupledKalmanConfig create() throws NotReadyException {
         if (!isReady()) {
             throw new NotReadyException();
         }
 
         final var gyroNoisePsd = Math.pow(gyroscopeNoiseRootPsdSource.getGyroscopeBaseNoiseLevelRootPsd(), 2.0);
         final var accelNoisePsd = Math.pow(accelerometerNoiseRootPsdSource.getAccelerometerBaseNoiseLevelRootPsd(),
                 2.0);
         final var accelRandomWalkBiasPsd = accelerometerBiasRandomWalkSource.getAccelerometerBiasPSD();
         final var gyroRandomWalkBiasPsd = gyroscopeBiasRandomWalkSource.getGyroBiasPSD();
         final var positionNoiseSd = positionNoiseStandardDeviationSource.getPositionNoiseStandardDeviation();
         final var velocityNoiseSd = velocityNoiseStandardDeviationSource.getVelocityNoiseStandardDeviation();
         return new INSLooselyCoupledKalmanConfig(gyroNoisePsd, accelNoisePsd, accelRandomWalkBiasPsd,
                 gyroRandomWalkBiasPsd, positionNoiseSd, velocityNoiseSd);
     }
 }