/*
    * 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.noise;
  
  import com.irurueta.navigation.LockedException;
  import com.irurueta.navigation.inertial.calibration.TimeIntervalEstimator;
  import com.irurueta.units.Measurement;
  import com.irurueta.units.Time;
  import com.irurueta.units.TimeConverter;
  import com.irurueta.units.TimeUnit;
  
  /**
   * Base class to estimate measurement noise variances and PSD's (Power Spectral Densities)
   * along with their average values.
   * Implementations of this estimator may use norms of measurement triads to estimate noise
   * levels.
   * To compute PSD's, this estimator assumes that measurement samples are obtained
   * at a constant provided rate equal to {@link #getTimeInterval()} seconds.
   * If not available, accelerometer sampling rate average can be estimated using
   * {@link TimeIntervalEstimator}.
   *
   * @param <U> a measurement unit type.
   * @param <M> a measurement type.
   * @param <E> an estimator type.
   * @param <L> a listener type.
   */
  @SuppressWarnings("DuplicatedCode")
  public abstract class AccumulatedMeasurementNoiseEstimator<U extends Enum<?>,
          M extends Measurement<U>,
          E extends AccumulatedMeasurementNoiseEstimator<U, M, E, L>,
          L extends AccumulatedMeasurementNoiseEstimatorListener<U, M, E>> {
  
      /**
       * Default time interval between accelerometer samples expressed in seconds
       * (s).
       */
      public static final double DEFAULT_TIME_INTERVAL_SECONDS = 0.02;
  
      /**
       * Time interval expressed in seconds (s) between consecutive accelerometer
       * samples.
       */
      private double timeInterval = DEFAULT_TIME_INTERVAL_SECONDS;
  
      /**
       * Listener to handle events raised by this estimator.
       */
      private L listener;
  
      /**
       * Last provided measurement.
       */
      private M lastMeasurement;
  
      /**
       * Contains estimated average of measurement expressed in its default unit
       * (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
       */
      private double avg;
  
      /**
       * Contains estimated variance of measurement expressed in its default squared unit
       * (m^2/s^4 for acceleration, rad^2/s^2 for angular speed or T^2 for magnetic
       * flux density).
       */
      private double variance;
  
      /**
       * Number of processed body kinematics samples.
       */
      private int numberOfProcessedSamples;
  
      /**
       * Number of processed timestamp samples plus one.
       */
      private int numberOfProcessedSamplesPlusOne = 1;
  
      /**
       * Indicates that estimator is running.
       */
      private boolean running;
  
      /**
       * Constructor.
       */
      protected AccumulatedMeasurementNoiseEstimator() {
     }
 
     /**
      * Constructor.
      *
      * @param listener listener to handle events raised by this estimator.
      */
     protected AccumulatedMeasurementNoiseEstimator(final L listener) {
         this.listener = listener;
     }
 
     /**
      * Gets time interval between accelerometer triad samples expressed in
      * seconds (s).
      *
      * @return time interval between accelerometer triad samples.
      */
     public double getTimeInterval() {
         return timeInterval;
     }
 
     /**
      * Sets time interval between accelerometer triad samples expressed in
      * seconds (s).
      *
      * @param timeInterval time interval between accelerometer triad samples.
      * @throws IllegalArgumentException if provided value is negative.
      * @throws LockedException          if estimator is currently running.
      */
     public void setTimeInterval(final double timeInterval) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         if (timeInterval < 0.0) {
             throw new IllegalArgumentException();
         }
 
         this.timeInterval = timeInterval;
     }
 
     /**
      * Gets time interval between accelerometer triad samples.
      *
      * @return time interval between accelerometer triad samples.
      */
     public Time getTimeIntervalAsTime() {
         return new Time(timeInterval, TimeUnit.SECOND);
     }
 
     /**
      * Gets time interval between accelerometer triad samples.
      *
      * @param result instance where time interval will be stored.
      */
     public void getTimeIntervalAsTime(final Time result) {
         result.setValue(timeInterval);
         result.setUnit(TimeUnit.SECOND);
     }
 
     /**
      * Sets time interval between accelerometer triad samples.
      *
      * @param timeInterval time interval between accelerometer triad samples.
      * @throws LockedException if estimator is currently running.
      */
     public void setTimeInterval(final Time timeInterval) throws LockedException {
         setTimeInterval(TimeConverter.convert(timeInterval.getValue().doubleValue(),
                 timeInterval.getUnit(), TimeUnit.SECOND));
     }
 
     /**
      * Gets listener to handle events raised by this estimator.
      *
      * @return listener to handle events raised by this estimator.
      */
     public L getListener() {
         return listener;
     }
 
     /**
      * Sets listener to handle events raised by this estimator.
      *
      * @param listener listener to handle events raised by this estimator.
      * @throws LockedException if this estimator is running.
      */
     public void setListener(final L listener) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         this.listener = listener;
     }
 
     /**
      * Gets last provided measurement or null if not available.
      *
      * @return last provided measurement or null.
      */
     public M getLastMeasurement() {
         return lastMeasurement;
     }
 
     /**
      * Gets last provided measurement.
      *
      * @param result instance where last provided measurement will be stored.
      * @return true if result instance was updated, false otherwise.
      */
     public boolean getLastMeasurement(final M result) {
         if (lastMeasurement != null) {
             result.setValue(lastMeasurement.getValue());
             result.setUnit(lastMeasurement.getUnit());
             return true;
         } else {
             return false;
         }
     }
 
     /**
      * Gets estimated average of measurement expressed in its default unit
      * (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
      *
      * @return average of measurement in current window.
      */
     public double getAvg() {
         return avg;
     }
 
     /**
      * Gets estimated average of measurement within current window.
      *
      * @return average of measurement in current window
      */
     public M getAvgAsMeasurement() {
         return createMeasurement(avg, getDefaultUnit());
     }
 
     /**
      * Gets estimated average of measurement within current window.
      *
      * @param result instance where average of measurement will be stored.
      */
     public void getAvgAsMeasurement(final M result) {
         result.setValue(avg);
         result.setUnit(getDefaultUnit());
     }
 
     /**
      * Gets estimated variance of measurement within current window
      * expressed in its default squared unit (m^2/s^4 for acceleration,
      * rad^2/s^2 for angular speed or T^2 for magnetic flux density).
      *
      * @return estimated variance of measurement within current window.
      */
     public double getVariance() {
         return variance;
     }
 
     /**
      * Gets estimated standard deviation of measurement within current window
      * and expressed in its default unit (m/s^2 for acceleration, rad/s for
      * angular speed or T for magnetic flux density).
      *
      * @return estimated standard of measurement.
      */
     public double getStandardDeviation() {
         return Math.sqrt(variance);
     }
 
     /**
      * Gets estimated standard deviation of measurement within current window.
      *
      * @return estimated standard deviation of measurement.
      */
     public M getStandardDeviationAsMeasurement() {
         return createMeasurement(getStandardDeviation(), getDefaultUnit());
     }
 
     /**
      * Gets estimated standard deviation of measurement within current window.
      *
      * @param result instance where estimated standard deviation of measurement
      *               will be stored.
      */
     public void getStandardDeviationAsMeasurement(final M result) {
         result.setValue(getStandardDeviation());
         result.setUnit(getDefaultUnit());
     }
 
     /**
      * Gets measurement noise PSD (Power Spectral Density) expressed
      * in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
      * magnetometer.
      *
      * @return measurement noise PSD.
      */
     public double getPsd() {
         return variance * timeInterval;
     }
 
     /**
      * Gets measurement noise root PSD (Power Spectral Density) expressed in
      * (m * s^-1.5) for accelerometer, (rad * s^-0.5) for gyroscope or (T * s^0.5) for
      * magnetometer.
      *
      * @return measurement noise root PSD.
      */
     public double getRootPsd() {
         return Math.sqrt(getPsd());
     }
 
     /**
      * Gets number of samples that have been processed so far.
      *
      * @return number of samples that have been processed so far.
      */
     public int getNumberOfProcessedSamples() {
         return numberOfProcessedSamples;
     }
 
     /**
      * Indicates whether estimator is currently running or not.
      *
      * @return true if estimator is running, false otherwise.
      */
     public boolean isRunning() {
         return running;
     }
 
     /**
      * Adds a measurement value expressed in its default unit (m/s^2 for acceleration, rad/s for
      * angular speed or T for magnetic flux density).
      *
      * @param value value to be added.
      * @throws LockedException if estimator is currently running.
      */
     public void addMeasurement(final double value) throws LockedException {
 
         if (running) {
             throw new LockedException();
         }
 
         running = true;
 
         if (lastMeasurement == null && listener != null) {
             //noinspection unchecked
             listener.onStart((E) this);
         }
 
         // compute average
         final var tmp = (double) numberOfProcessedSamples / (double) numberOfProcessedSamplesPlusOne;
         avg = avg * tmp + value / numberOfProcessedSamplesPlusOne;
 
         // compute variance
         final var diff = value - avg;
         final var diff2 = diff * diff;
 
         variance = variance * tmp + diff2 / numberOfProcessedSamplesPlusOne;
 
         if (lastMeasurement == null) {
             lastMeasurement = createMeasurement(value, getDefaultUnit());
         } else {
             lastMeasurement.setValue(value);
             lastMeasurement.setUnit(getDefaultUnit());
         }
 
         numberOfProcessedSamples++;
         numberOfProcessedSamplesPlusOne++;
 
         if (listener != null) {
             //noinspection unchecked
             listener.onMeasurementAdded((E) this);
         }
 
         running = false;
     }
 
     /**
      * Adds a measurement value.
      *
      * @param measurement measurement to be added.
      * @throws LockedException if estimator is currently running.
      */
     public void addMeasurement(final M measurement) throws LockedException {
         addMeasurement(convertToDefaultUnit(measurement));
     }
 
     /**
      * Resets current estimator.
      *
      * @return true if estimator was successfully reset, false if no reset was needed.
      * @throws LockedException if estimator is currently running.
      */
     public boolean reset() throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         if (numberOfProcessedSamples == 0) {
             return false;
         }
 
         running = true;
         lastMeasurement = null;
         avg = 0.0;
         variance = 0.0;
         numberOfProcessedSamples = 0;
         numberOfProcessedSamplesPlusOne = 1;
 
         if (listener != null) {
             //noinspection unchecked
             listener.onReset((E) this);
         }
 
         running = false;
 
         return true;
     }
 
     /**
      * Gets default unit for a measurement.
      *
      * @return default unit for a measurement.
      */
     protected abstract U getDefaultUnit();
 
     /**
      * Creates a measurement with provided value and unit.
      *
      * @param value value to be set.
      * @param unit  unit to be set.
      * @return created measurement.
      */
     protected abstract M createMeasurement(final double value, final U unit);
 
     /**
      * Converts provided measurement into default unit.
      *
      * @param value measurement to be converted.
      * @return converted value.
      */
     protected abstract double convertToDefaultUnit(M value);
 }