/*
    * Copyright (C) 2020 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.generators;
  
  import com.irurueta.navigation.LockedException;
  import com.irurueta.navigation.inertial.BodyKinematics;
  import com.irurueta.navigation.inertial.calibration.AngularSpeedTriad;
  import com.irurueta.navigation.inertial.calibration.BodyKinematicsSequence;
  import com.irurueta.navigation.inertial.calibration.GyroscopeNoiseRootPsdSource;
  import com.irurueta.navigation.inertial.calibration.StandardDeviationTimedBodyKinematics;
  import com.irurueta.navigation.inertial.calibration.TimedBodyKinematics;
  import com.irurueta.navigation.inertial.calibration.intervals.TriadStaticIntervalDetector;
  import com.irurueta.navigation.inertial.calibration.noise.AccumulatedAngularSpeedTriadNoiseEstimator;
  import com.irurueta.units.AngularSpeed;
  import com.irurueta.units.AngularSpeedUnit;
  
  import java.util.ArrayList;
  import java.util.List;
  
  /**
   * Generates measurements for the calibration of gyroscopes by alternating
   * static and dynamic intervals where device is kept static or moved.
   * Generated measurements must be used with easy gyroscope calibrators.
   * Notice that accuracy of the gyroscope calibration is very sensitive to the
   * accuracy of detected dynamic intervals respect the average specific forces
   * during static intervals.
   * In order to increase the accuracy, calibration should be repeated trying different
   * threshold factors {@link #getThresholdFactor()}.
   * Such calibrators are the following ones:
   * - {@link com.irurueta.navigation.inertial.calibration.gyroscope.EasyGyroscopeCalibrator}
   * - {@link com.irurueta.navigation.inertial.calibration.gyroscope.KnownBiasEasyGyroscopeCalibrator}
   * - {@link com.irurueta.navigation.inertial.calibration.gyroscope.RobustEasyGyroscopeCalibrator} and all its
   * implementations.
   * - {@link com.irurueta.navigation.inertial.calibration.gyroscope.RobustKnownBiasEasyGyroscopeCalibrator} and all its
   * implementations.
   */
  public class GyroscopeMeasurementsGenerator extends
          MeasurementsGenerator<BodyKinematicsSequence<StandardDeviationTimedBodyKinematics>,
                  GyroscopeMeasurementsGenerator, GyroscopeMeasurementsGeneratorListener, TimedBodyKinematics>
          implements GyroscopeNoiseRootPsdSource {
  
      /**
       * An angular speed triad.
       * This is reused for memory efficiency.
       */
      protected final AngularSpeedTriad angularSpeedTriad = new AngularSpeedTriad();
  
      /**
       * Items to be added to a generated sequence when next static period occurs.
       */
      private List<StandardDeviationTimedBodyKinematics> currentSequenceItems;
  
      /**
       * Accumulated noise estimator for angular speed measurements.
       */
      private final AccumulatedAngularSpeedTriadNoiseEstimator accumulatedEstimator =
              new AccumulatedAngularSpeedTriadNoiseEstimator();
  
      /**
       * Estimated acceleration standard deviation during initialization expressed
       * in meters per squared second (m/s^2).
       */
      private double accelerationStandardDeviation;
  
      /**
       * Estimated angular speed standard deviation during initialization expressed
       * in radians per second (rad/s).
       */
      private double angularSpeedStandardDeviation;
  
      /**
       * Estimated norm of gyroscope noise root PSD (Power Spectral Density)
       * expressed as (rad * s^-0.5).
       */
      private double angularSpeedNoiseRootPsd;
  
      /**
       * Previous average x-coordinate of measurements expressed in meters
       * per squared second (m/s^2).
       */
      private Double previousAvgX;
  
      /**
       * Previous average y-coordinate of measurements expressed in meters
       * per squared second (m/s^2).
       */
     private Double previousAvgY;
 
     /**
      * Previous average z-coordinate of measurements expressed in meters
      * per squared second (m/s^2).
      */
     private Double previousAvgZ;
 
     /**
      * Current average x-coordinate of measurements expressed in meters
      * per squared second (m/s^2).
      */
     private Double currentAvgX;
 
     /**
      * Current average y-coordinate of measurements expressed in meters
      * per squared second (m/s^2).
      */
     private Double currentAvgY;
 
     /**
      * Current average z-coordinate of measurements expressed in meters
      * per squared second (m/s^2).
      */
     private Double currentAvgZ;
 
     /**
      * Contains previous status while processing samples.
      */
     private TriadStaticIntervalDetector.Status previousStatus;
 
     /**
      * Constructor.
      */
     public GyroscopeMeasurementsGenerator() {
         super();
     }
 
     /**
      * Constructor.
      *
      * @param listener listener to handle events raised by this generator.
      */
     public GyroscopeMeasurementsGenerator(final GyroscopeMeasurementsGeneratorListener listener) {
         super(listener);
     }
 
     /**
      * Sets time interval between input samples expressed in seconds (s).
      *
      * @param timeInterval time interval between input samples.
      * @throws IllegalArgumentException if provided value is negative.
      * @throws LockedException          if generator is currently running.
      */
     @Override
     public void setTimeInterval(final double timeInterval) throws LockedException {
         super.setTimeInterval(timeInterval);
         accumulatedEstimator.setTimeInterval(timeInterval);
     }
 
     /**
      * Resets this generator.
      *
      * @throws LockedException if generator is busy.
      */
     @Override
     public void reset() throws LockedException {
         super.reset();
 
         currentSequenceItems = null;
 
         accelerationStandardDeviation = 0.0;
         angularSpeedStandardDeviation = 0.0;
 
         previousAvgX = null;
         previousAvgY = null;
         previousAvgZ = null;
 
         currentAvgX = null;
         currentAvgY = null;
         currentAvgZ = null;
 
         accumulatedEstimator.reset();
 
         previousStatus = null;
     }
 
     /**
      * Gets estimated average angular rate during initialization phase.
      *
      * @return estimated average angular rate during initialization phase.
      */
     public AngularSpeedTriad getInitialAvgAngularSpeedTriad() {
         return accumulatedEstimator.getAvgTriad();
     }
 
     /**
      * Gets estimated average angular rate during initialization phase.
      *
      * @param result instance where result will be stored.
      */
     public void getInitialAvgAngularSpeedTriad(final AngularSpeedTriad result) {
         accumulatedEstimator.getAvgTriad(result);
     }
 
     /**
      * Gets estimated standard deviation of angular rate during initialization phase.
      *
      * @return estimated standard deviation of angular rate during initialization phase.
      */
     public AngularSpeedTriad getInitialAngularSpeedTriadStandardDeviation() {
         return accumulatedEstimator.getStandardDeviationTriad();
     }
 
     /**
      * Gets estimated standard deviation of angular rate during initialization phase.
      *
      * @param result instance where result will be stored.
      */
     public void getInitialAngularSpeedTriadStandardDeviation(final AngularSpeedTriad result) {
         accumulatedEstimator.getStandardDeviationTriad(result);
     }
 
     /**
      * Gets gyroscope base noise level that has been detected during
      * initialization expressed in radians per second (rad/s).
      * This is equal to the standard deviation of the gyroscope measurements
      * during initialization phase.
      *
      * @return gyroscope base noise level.
      */
     public double getGyroscopeBaseNoiseLevel() {
         return angularSpeedStandardDeviation;
     }
 
     /**
      * Gets gyroscope base noise level that has been detected during
      * initialization.
      * This is equal to the standard deviation of the gyroscope measurements
      * during initialization phase.
      *
      * @return gyroscope base noise level.
      */
     public AngularSpeed getGyroscopeBaseNoiseLevelAsMeasurement() {
         return new AngularSpeed(angularSpeedStandardDeviation, AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets gyroscope base noise level that has been detected during
      * initialization.
      * This is equal to the standard deviation of the gyroscope measurements
      * during initialization phase.
      *
      * @param result instance where result will be stored.
      */
     public void getGyroscopeBaseNoiseLevelAsMeasurement(final AngularSpeed result) {
         result.setValue(angularSpeedStandardDeviation);
         result.setUnit(AngularSpeedUnit.RADIANS_PER_SECOND);
     }
 
     /**
      * Gets gyroscope base noise level PSD (Power Spectral Density)
      * expressed in (rad^2/s).
      *
      * @return gyroscope base noise level PSD.
      */
     public double getGyroscopeBaseNoiseLevelPsd() {
         return angularSpeedNoiseRootPsd * angularSpeedNoiseRootPsd;
     }
 
     /**
      * Gets gyroscope base noise level root PSD (Power Spectral Density)
      * expressed in (rad * s^-0.5)
      *
      * @return gyroscope base noise level root PSD.
      */
     @Override
     public double getGyroscopeBaseNoiseLevelRootPsd() {
         return angularSpeedNoiseRootPsd;
     }
 
     /**
      * Post process provided input sample.
      *
      * @param sample an input sample.
      * @throws LockedException if generator is busy.
      */
     @Override
     protected void postProcess(final TimedBodyKinematics sample) throws LockedException {
         final var status = staticIntervalDetector.getStatus();
 
         if (status == TriadStaticIntervalDetector.Status.INITIALIZING) {
             sample.getKinematics().getAngularRateTriad(angularSpeedTriad);
             accumulatedEstimator.addTriad(angularSpeedTriad);
         }
 
         // while we are in a dynamic interval, we must record all timed kinematics
         // along with accelerometer and gyroscope standard deviations
         if (status == TriadStaticIntervalDetector.Status.DYNAMIC_INTERVAL) {
             if (isDynamicIntervalSkipped()) {
                 // dynamic interval has been skipped because there were too many
                 // items in the sequence.
                 currentSequenceItems = null;
             } else {
                 if (previousStatus == TriadStaticIntervalDetector.Status.STATIC_INTERVAL) {
                     previousAvgX = staticIntervalDetector.getAccumulatedAvgX();
                     previousAvgY = staticIntervalDetector.getAccumulatedAvgY();
                     previousAvgZ = staticIntervalDetector.getAccumulatedAvgZ();
                 }
 
                 addSequenceItem(sample);
             }
         } else if (status == TriadStaticIntervalDetector.Status.STATIC_INTERVAL
                 && previousStatus == TriadStaticIntervalDetector.Status.DYNAMIC_INTERVAL
                 && currentSequenceItems != null && !currentSequenceItems.isEmpty()) {
 
             currentAvgX = staticIntervalDetector.getInstantaneousAvgX();
             currentAvgY = staticIntervalDetector.getInstantaneousAvgY();
             currentAvgZ = staticIntervalDetector.getInstantaneousAvgZ();
 
             // we have all required data to generate a sequence
             BodyKinematicsSequence<StandardDeviationTimedBodyKinematics> sequence = null;
             if (listener != null) {
                 sequence = new BodyKinematicsSequence<>();
                 sequence.setBeforeMeanSpecificForceCoordinates(previousAvgX, previousAvgY, previousAvgZ);
                 sequence.setItems(currentSequenceItems);
                 sequence.setAfterMeanSpecificForceCoordinates(currentAvgX, currentAvgY, currentAvgZ);
             }
 
             currentSequenceItems = null;
 
             if (listener != null) {
                 listener.onGeneratedMeasurement(this, sequence);
             }
         }
     }
 
     /**
      * Gets corresponding acceleration triad from provided input sample.
      * This method must store the result into {@link #triad}.
      *
      * @param sample input sample.
      */
     @Override
     protected void getAccelerationTriadFromInputSample(final TimedBodyKinematics sample) {
         sample.getKinematics().getSpecificForceTriad(triad);
     }
 
     /**
      * Handles a static-to-dynamic interval change.
      *
      * @param accumulatedAvgX average x-coordinate of measurements during last
      *                        static period expressed in meters per squared
      *                        second (m/s^2).
      * @param accumulatedAvgY average y-coordinate of specific force during last
      *                        static period expressed in meters per squared
      *                        second (m/s^2).
      * @param accumulatedAvgZ average z-coordinate of specific force during last
      *                        static period expressed in meters per squared
      *                        second (m/s^2).
      * @param accumulatedStdX standard deviation of x-coordinate of measurements
      *                        during last static period expressed in meters per
      *                        squared second (m/s^2).
      * @param accumulatedStdY standard deviation of y-coordinate of measurements
      *                        during last static period expressed in meters per
      *                        squared second (m/s^2).
      * @param accumulatedStdZ standard deviation of z-coordinate of measurements
      *                        during last static period expressed in meters per
      *                        squared second (m/s^2).
      */
     @Override
     protected void handleStaticToDynamicChange(
             final double accumulatedAvgX, final double accumulatedAvgY, final double accumulatedAvgZ,
             final double accumulatedStdX, final double accumulatedStdY, final double accumulatedStdZ) {
         previousStatus = TriadStaticIntervalDetector.Status.STATIC_INTERVAL;
     }
 
     /**
      * Handles a dynamic-to-static interval change.
      */
     @Override
     protected void handleDynamicToStaticChange() {
         previousStatus = TriadStaticIntervalDetector.Status.DYNAMIC_INTERVAL;
     }
 
     /**
      * Handles an initialization completion.
      */
     @Override
     protected void handleInitializationCompleted() {
         accelerationStandardDeviation = staticIntervalDetector.getBaseNoiseLevel();
         angularSpeedStandardDeviation = accumulatedEstimator.getStandardDeviationNorm();
         angularSpeedNoiseRootPsd = accumulatedEstimator.getNoiseRootPsdNorm();
 
         previousStatus = staticIntervalDetector.getStatus();
     }
 
     /**
      * Handles an error during initialization.
      */
     @Override
     protected void handleInitializationFailed() {
         previousStatus = null;
 
         try {
             accumulatedEstimator.reset();
         } catch (final LockedException ignore) {
             // no action needed
         }
     }
 
     /**
      * Adds an item to current sequence items.
      *
      * @param sample sample to generate a sequence item from.
      */
     private void addSequenceItem(final TimedBodyKinematics sample) {
         if (currentSequenceItems == null) {
             currentSequenceItems = new ArrayList<>();
         }
 
         final var kinematics = new BodyKinematics(sample.getKinematics());
         final var timestampSeconds = sample.getTimestampSeconds();
         final var stdTimedKinematics = new StandardDeviationTimedBodyKinematics(kinematics, timestampSeconds,
                 accelerationStandardDeviation, angularSpeedStandardDeviation);
         currentSequenceItems.add(stdTimedKinematics);
     }
 }