/*
    * 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.navigation.inertial.calibration.Triad;
  import com.irurueta.units.Measurement;
  import com.irurueta.units.Time;
  import com.irurueta.units.TimeConverter;
  import com.irurueta.units.TimeUnit;
  
  import java.util.LinkedList;
  
  /**
   * Base class to estimate measurement noise variances and PSD's (Power Spectral Densities)
   * along with average values for a windowed amount of samples.
   * Implementations of this estimator must be used when the body where the measurement device
   * is attached to remains static on the same position with zero velocity while capturing data.
   * 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}.
   * This estimator does NOT require the knowledge of current location and body
   * orientation.
   * Because body location and orientation is not known, estimated average values
   * cannot be used to determine biases. Only norm of noise estimations
   * (variance or standard deviation) can be safely used.
   * Notice that if there are less than {@link #getWindowSize()} processed
   * samples in the window, this estimator will assume that the remaining ones
   * until the window is completed have zero values.
   *
   * @param <U> a measurement unit type.
   * @param <M> a measurement type.
   * @param <T> a triad type.
   * @param <E> an estimator type.
   * @param <L> a listener type.
   */
  @SuppressWarnings("DuplicatedCode")
  public abstract class WindowedTriadNoiseEstimator<U extends Enum<?>,
          M extends Measurement<U>, T extends Triad<U, M, T>,
          E extends WindowedTriadNoiseEstimator<U, M, T, E, L>,
          L extends WindowedTriadNoiseEstimatorListener<U, M, T, E>> {
  
      /**
       * Number of samples to keep within the window by default.
       * For an accelerometer generating 100 samples/second, this is equivalent to
       * 1 second.
       * For an accelerometer generating 50 samples/second, this is equivalent to
       * 2 seconds.
       */
      public static final int DEFAULT_WINDOW_SIZE = 101;
  
      /**
       * Minimum allowed window size.
       */
      public static final int MIN_WINDOW_SIZE = 3;
  
      /**
       * Default time interval between accelerometer samples expressed in seconds
       * (s).
       */
      public static final double DEFAULT_TIME_INTERVAL_SECONDS = 0.02;
  
      /**
       * Length of number of samples to keep within the window being processed.
       * Window size must always be larger than allowed minimum value and must have
       * an odd value.
       */
      private int windowSize = DEFAULT_WINDOW_SIZE;
  
      /**
       * Time interval expressed in seconds (s) between consecutive triad
       * samples.
       */
      private double timeInterval = DEFAULT_TIME_INTERVAL_SECONDS;
  
      /**
       * Keeps the list of triad samples that remain within the window.
       */
      private final LinkedList<T> windowedSamples = new LinkedList<>();
  
      /**
       * Listener to handle events raised by this estimator.
       */
      private L listener;
 
     /**
      * Contains estimated average of x coordinate 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 avgX;
 
     /**
      * Contains estimated average of y coordinate 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 avgY;
 
     /**
      * Contains estimated average of z coordinate 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 avgZ;
 
     /**
      * Contains estimated variance of x coordinate 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 varianceX;
 
     /**
      * Contains estimated variance of y coordinate 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 varianceY;
 
     /**
      * Contains estimated variance of x coordinate 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 varianceZ;
 
     /**
      * Number of processed acceleration triad samples.
      */
     private int numberOfProcessedSamples;
 
     /**
      * Indicates whether estimator is running or not.
      */
     private boolean running;
 
     /**
      * Constructor.
      */
     protected WindowedTriadNoiseEstimator() {
     }
 
     /**
      * Constructor.
      *
      * @param listener listener to handle events raised by this estimator.
      */
     protected WindowedTriadNoiseEstimator(final L listener) {
         this.listener = listener;
     }
 
     /**
      * Gets length of number of samples to keep within the window being processed.
      * Window size must always be larger than allowed minimum value and must
      * have an odd value.
      *
      * @return length of number of samples to keep within the window.
      */
     public int getWindowSize() {
         return windowSize;
     }
 
     /**
      * Sets length of number of samples to keep within the window being processed.
      * Window size must always be larger than allowed minimum value and must have
      * an odd value.
      * When window size is modified, instance state is reset.
      *
      * @param windowSize length of number of samples to keep within the window.
      * @throws IllegalArgumentException if provided value is not valid.
      * @throws LockedException          if estimator is currently running.
      */
     public void setWindowSize(final int windowSize) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         // check that window is larger than minimum allowed value
         if (windowSize < MIN_WINDOW_SIZE) {
             throw new IllegalArgumentException();
         }
 
         // check that window size is not even
         if (windowSize % 2 == 0) {
             throw new IllegalArgumentException();
         }
 
         this.windowSize = windowSize;
         reset();
     }
 
     /**
      * Gets time interval between triad samples expressed in
      * seconds (s).
      *
      * @return time interval between triad samples.
      */
     public double getTimeInterval() {
         return timeInterval;
     }
 
     /**
      * Sets time interval between triad samples expressed in
      * seconds (s).
      *
      * @param timeInterval time interval between 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 triad samples.
      *
      * @return time interval between triad samples.
      */
     public Time getTimeIntervalAsTime() {
         return new Time(timeInterval, TimeUnit.SECOND);
     }
 
     /**
      * Gets time interval between 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 triad samples.
      *
      * @param timeInterval time interval between 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 first provided measurement triad within the window.
      *
      * @return first provided measurement triad within the window or null if not
      * available.
      */
     public T getFirstWindowedTriad() {
         return windowedSamples.isEmpty() ? null : windowedSamples.getFirst();
     }
 
     /**
      * Gets first provided measurement triad within the window.
      *
      * @param result instance where first provided measurement triad will be stored.
      * @return true if result instance was updated, false otherwise.
      */
     public boolean getFirstWindowedTriad(final T result) {
         if (windowedSamples.isEmpty()) {
             return false;
         } else {
             result.copyFrom(windowedSamples.getFirst());
             return true;
         }
     }
 
     /**
      * Gets last provided measurement triad within the window.
      *
      * @return last provided measurement triad within the window or null if not
      * available.
      */
     public T getLastWindowedTriad() {
         return windowedSamples.isEmpty() ? null : windowedSamples.getLast();
     }
 
     /**
      * Gets last provided measurement triad within the window.
      *
      * @param result instance where last provided measurement triad will be stored.
      * @return true if result instance was updated, false otherwise.
      */
     public boolean getLastWindowedTriad(final T result) {
         if (windowedSamples.isEmpty()) {
             return false;
         } else {
             result.copyFrom(windowedSamples.getLast());
             return true;
         }
     }
 
     /**
      * Gets estimated average of x coordinate of measurement expressed in its default
      * unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of x coordinate of measurement in current window.
      */
     public double getAvgX() {
         return avgX;
     }
 
     /**
      * Gets estimated average of x coordinate of measurement within current window.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of x coordinate of measurement in current window.
      */
     public M getAvgXAsMeasurement() {
         return createMeasurement(avgX, getDefaultUnit());
     }
 
     /**
      * Gets estimated average of x coordinate of measurement within current window.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @param result instance where average of x coordinate of measurement will be stored.
      */
     public void getAvgXAsMeasurement(final M result) {
         result.setValue(avgX);
         result.setUnit(getDefaultUnit());
     }
 
     /**
      * Gets estimated average of y coordinate of measurement expressed in its default
      * unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of y coordinate of measurement in current window.
      */
     public double getAvgY() {
         return avgY;
     }
 
     /**
      * Gets estimated average of y coordinate of measurement within current window.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of y coordinate of measurement in current window.
      */
     public M getAvgYAsMeasurement() {
         return createMeasurement(avgY, getDefaultUnit());
     }
 
     /**
      * Gets estimated average of y coordinate of measurement within current window.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @param result instance where average of y coordinate of measurement will be stored.
      */
     public void getAvgYAsMeasurement(final M result) {
         result.setValue(avgY);
         result.setUnit(getDefaultUnit());
     }
 
     /**
      * Gets estimated average of z coordinate of measurement expressed in its default
      * unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of z coordinate of measurement in current window.
      */
     public double getAvgZ() {
         return avgZ;
     }
 
     /**
      * Gets estimated average of z coordinate of measurement within current window.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @return average of z coordinate of measurement in current window.
      */
     public M getAvgZAsMeasurement() {
         return createMeasurement(avgZ, getDefaultUnit());
     }
 
     /**
      * Gets estimated average of z coordinate of measurement within current window.
      * This value will depend of body location and orientation, hence it should never
      * be used as a calibration bias.
      *
      * @param result instance where average of z coordinate of measurement will be stored.
      */
     public void getAvgZAsMeasurement(final M result) {
         result.setValue(avgZ);
         result.setUnit(getDefaultUnit());
     }
 
     /**
      * Gets estimated average as a measurement triad.
      *
      * @return average measurement triad.
      */
     public T getAvgTriad() {
         return createTriad(avgX, avgY, avgZ, getDefaultUnit());
     }
 
     /**
      * Gets estimated average as a measurement triad.
      *
      * @param result instance where average values and unit will be stored.
      */
     public void getAvgTriad(final T result) {
         result.setValueCoordinatesAndUnit(avgX, avgY, avgZ, getDefaultUnit());
     }
 
     /**
      * Gets norm of estimated average measurement within current window expressed in its default
      * unit (m/s^2 for acceleration, rad/s for angular speed or T for magnetic flux density).
      * This value is independent of body orientation.
      *
      * @return norm of estimated average specific force.
      */
     public double getAvgNorm() {
         return Math.sqrt(avgX * avgX + avgY * avgY + avgZ * avgZ);
     }
 
     /**
      * Gets norm of estimated average measurement within current window.
      *
      * @return norm of estimated average measurement.
      */
     public M getAvgNormAsMeasurement() {
         return createMeasurement(getAvgNorm(), getDefaultUnit());
     }
 
     /**
      * Gets norm of estimated average measurement within current window.
      *
      * @param result instance where norm of estimated average measurement will be stored.
      */
     public void getAvgNormAsMeasurement(final M result) {
         result.setValue(getAvgNorm());
         result.setUnit(getDefaultUnit());
     }
 
     /**
      * Gets estimated variance of x coordinate 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 x coordinate of measurement within current
      * window.
      */
     public double getVarianceX() {
         return varianceX;
     }
 
     /**
      * Gets estimated variance of y coordinate 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 y coordinate of measurement within current
      * window.
      */
     public double getVarianceY() {
         return varianceY;
     }
 
     /**
      * Gets estimated variance of z coordinate 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 z coordinate of measurement within current
      * window.
      */
     public double getVarianceZ() {
         return varianceZ;
     }
 
     /**
      * Gets estimated standard deviation of x coordinate 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 deviation of x coordinate of measurement within
      * current window.
      */
     public double getStandardDeviationX() {
         return Math.sqrt(varianceX);
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of measurement within current
      * window.
      *
      * @return estimated standard deviation of x coordinate of measurement.
      */
     public M getStandardDeviationXAsMeasurement() {
         return createMeasurement(getStandardDeviationX(), getDefaultUnit());
     }
 
     /**
      * Gets estimated standard deviation of x coordinate of measurement within current
      * window.
      *
      * @param result instance where estimated standard deviation of x coordinate of
      *               measurement will be stored.
      */
     public void getStandardDeviationXAsMeasurement(final M result) {
         result.setValue(getStandardDeviationX());
         result.setUnit(getDefaultUnit());
     }
 
     /**
      * Gets estimated standard deviation of y coordinate 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 deviation of y coordinate of measurement within
      * current window.
      */
     public double getStandardDeviationY() {
         return Math.sqrt(varianceY);
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of measurement within current
      * window.
      *
      * @return estimated standard deviation of y coordinate of measurement.
      */
     public M getStandardDeviationYAsMeasurement() {
         return createMeasurement(getStandardDeviationY(), getDefaultUnit());
     }
 
     /**
      * Gets estimated standard deviation of y coordinate of measurement within current
      * window.
      *
      * @param result instance where estimated standard deviation of y coordinate of
      *               measurement will be stored.
      */
     public void getStandardDeviationYAsMeasurement(final M result) {
         result.setValue(getStandardDeviationY());
         result.setUnit(getDefaultUnit());
     }
 
     /**
      * Gets estimated standard deviation of z coordinate 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 deviation of z coordinate of measurement within
      * current window.
      */
     public double getStandardDeviationZ() {
         return Math.sqrt(varianceZ);
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of measurement within current
      * window.
      *
      * @return estimated standard deviation of z coordinate of measurement.
      */
     public M getStandardDeviationZAsMeasurement() {
         return createMeasurement(getStandardDeviationZ(), getDefaultUnit());
     }
 
     /**
      * Gets estimated standard deviation of z coordinate of measurement within current
      * window.
      *
      * @param result instance where estimated standard deviation of z coordinate of
      *               measurement will be stored.
      */
     public void getStandardDeviationZAsMeasurement(final M result) {
         result.setValue(getStandardDeviationZ());
         result.setUnit(getDefaultUnit());
     }
 
     /**
      * Gets estimated standard deviation of measurement within current window.
      *
      * @return estimated standard deviation triad of measurement.
      */
     public T getStandardDeviationTriad() {
         return createTriad(getStandardDeviationX(), getStandardDeviationY(), getStandardDeviationZ(), getDefaultUnit());
     }
 
     /**
      * Gets estimated standard deviation of measurement within current window.
      *
      * @param result instance where estimated standard deviation triad of
      *               measurement will be stored.
      */
     public void getStandardDeviationTriad(final T result) {
         result.setValueCoordinatesAndUnit(getStandardDeviationX(), getStandardDeviationY(), getStandardDeviationZ(),
                 getDefaultUnit());
     }
 
     /**
      * Gets norm of estimated standard deviation of measurement within current
      * window expressed in its default unit (m/s^2 for acceleration, rad/s for
      * angular speed or T for magnetic flux density).
      *
      * @return norm of estimated standard deviation of measurement.
      */
     public double getStandardDeviationNorm() {
         final var fx = getStandardDeviationX();
         final var fy = getStandardDeviationY();
         final var fz = getStandardDeviationZ();
         return Math.sqrt(fx * fx + fy * fy + fz * fz);
     }
 
     /**
      * Gets norm of estimated standard deviation of measurement within current window.
      *
      * @return norm of estimated standard deviation of measurement.
      */
     public M getStandardDeviationNormAsMeasurement() {
         return createMeasurement(getStandardDeviationNorm(), getDefaultUnit());
     }
 
     /**
      * Gets norm of estimated standard deviation of measurement within current window.
      *
      * @param result instance where norm of estimated standard deviation will be stored.
      */
     public void getStandardDeviationNormAsMeasurement(final M result) {
         result.setValue(getStandardDeviationNorm());
         result.setUnit(getDefaultUnit());
     }
 
     /**
      * Gets average of estimated standard deviation coordinates of measurement within
      * current window expressed in its default unit (m/s^2 for acceleration, rad/s for
      * angular speed or T for magnetic flux density).
      *
      * @return average of estimated standard deviation coordinates.
      */
     public double getAverageStandardDeviation() {
         final var fx = getStandardDeviationX();
         final var fy = getStandardDeviationY();
         final var fz = getStandardDeviationZ();
         return (fx + fy + fz) / 3.0;
     }
 
     /**
      * Gets average of estimated standard deviation coordinates of measurement within
      * current window.
      *
      * @return average of estimated standard deviation coordinates.
      */
     public M getAverageStandardDeviationAsMeasurement() {
         return createMeasurement(getAverageStandardDeviation(), getDefaultUnit());
     }
 
     /**
      * Gets average of estimated standard deviation coordinates of measurement within
      * current window.
      *
      * @param result instance where average of estimated standard deviation coordinates
      *               will be stored.
      */
     public void getAverageStandardDeviationAsMeasurement(final M result) {
         result.setValue(getAverageStandardDeviation());
         result.setUnit(getDefaultUnit());
     }
 
     /**
      * Gets measurement noise PSD (Power Spectral Density) on x axis expressed
      * in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
      * magnetometer.
      *
      * @return measurement noise PSD on x axis.
      */
     public double getPsdX() {
         return varianceX * timeInterval;
     }
 
     /**
      * Gets measurement noise PSD (Power Spectral Density) on y axis expressed
      * in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
      * magnetometer.
      *
      * @return measurement noise PSD on y axis.
      */
     public double getPsdY() {
         return varianceY * timeInterval;
     }
 
     /**
      * Gets measurement noise PSD (Power Spectral Density) on z axis expressed
      * in (m^2 * s^-3) for accelerometer, (rad^2/s) for gyroscope or (T^2 * s) for
      * magnetometer.
      *
      * @return measurement noise PSD on z axis.
      */
     public double getPsdZ() {
         return varianceZ * timeInterval;
     }
 
     /**
      * Gets measurement noise root PSD (Power Spectral Density) on x axis 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 on x axis.
      */
     public double getRootPsdX() {
         return Math.sqrt(getPsdX());
     }
 
     /**
      * Gets measurement noise root PSD (Power Spectral Density) on y axis 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 on y axis.
      */
     public double getRootPsdY() {
         return Math.sqrt(getPsdY());
     }
 
     /**
      * Gets measurement noise root PSD (Power Spectral Density) on z axis 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 on z axis.
      */
     public double getRootPsdZ() {
         return Math.sqrt(getPsdZ());
     }
 
     /**
      * Gets average measurement noise PSD (Power Spectral Density) among
      * x,y,z components expressed as (m^2 * s^-3) for accelerometer,
      * (rad^2/s) for gyroscope or (T^2 * s) for magnetometer.
      *
      * @return average measurement noise PSD.
      */
     public double getAvgNoisePsd() {
         return (getPsdX() + getPsdY() + getPsdZ()) / 3.0;
     }
 
     /**
      * Gets norm of noise root PSD (Power Spectral Density) among x,y,z
      * components expressed as (m * s^-1.5) for accelerometer,
      * (rad * s^-0.5) for gyroscope or (T * s^0.5) for magnetometer.
      *
      * @return norm of measurement noise root PSD.
      */
     public double getNoiseRootPsdNorm() {
         return Math.sqrt(getPsdX() + getPsdY() + getPsdZ());
     }
 
     /**
      * 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;
     }
 
     /**
      * Gets number of currently windowed samples.
      *
      * @return number of samples within the window.
      */
     public int getNumberOfSamplesInWindow() {
         return windowedSamples.size();
     }
 
     /**
      * Indicates whether estimator is currently running or not.
      *
      * @return true if estimator is running, false otherwise.
      */
     public boolean isRunning() {
         return running;
     }
 
     /**
      * Indicates whether window of samples is filled or not.
      *
      * @return true if window is filled, false otherwise.
      */
     public boolean isWindowFilled() {
         return getNumberOfSamplesInWindow() == windowSize;
     }
 
     /**
      * Adds a triad of measurement samples and processes current window.
      * Notice that if there are less than {@link #getWindowSize()} processed
      * samples in the window, the remaining ones are considered to be zero
      * when average values and standard deviation is estimated.
      *
      * @param triad measurement triad to be added and processed.
      * @throws LockedException if estimator is currently running.
      */
     public void addTriadAndProcess(final T triad) throws LockedException {
         internalAdd(triad, true);
     }
 
     /**
      * Adds a triad of measurement samples and processes current window.
      * Values are expressed in measurement default unit (m/s^2 for acceleration, rad/s for
      * angular speed or T for magnetic flux density).
      * Notice that if there are less than {@link #getWindowSize()} processed
      * samples in the window, the remaining ones are considered to be zero
      * when average values and standard deviation is estimated.
      *
      * @param valueX x coordinate of measurement to be added and processed.
      * @param valueY y coordinate of measurement to be added and processed.
      * @param valueZ z coordinate of measurement to be added and processed.
      * @throws LockedException if estimator is currently running.
      */
     public void addTriadAndProcess(final double valueX, final double valueY, final double valueZ)
             throws LockedException {
         addTriadAndProcess(createTriad(valueX, valueY, valueZ, getDefaultUnit()));
     }
 
     /**
      * Adds a triad of measurement samples and processes current window.
      * Notice that if there are less than {@link #getWindowSize()} processed
      * samples in the window, the remaining ones are considered to be zero
      * when average values and standard deviation is estimated.
      *
      * @param valueX x coordinate of measurement to be added and processed.
      * @param valueY y coordinate of measurement to be added and processed.
      * @param valueZ z coordinate of measurement to be added and processed.
      * @throws LockedException if estimator is currently running.
      */
     public void addTriadAndProcess(final M valueX, final M valueY, final M valueZ) throws LockedException {
         addTriadAndProcess(createTriad(valueX, valueY, valueZ));
     }
 
     /**
      * Adds a triad of measurement samples without processing current window or updating
      * result values.
      * Notice that if there are less than {@link #getWindowSize()} processed
      * samples in the window, the remaining ones are considered to be zero
      * when average values and standard deviation is estimated.
      *
      * @param triad measurement triad to be added.
      * @throws LockedException if estimator is currently running.
      */
     public void addTriad(final T triad) throws LockedException {
         internalAdd(triad, false);
     }
 
     /**
      * Adds a triad of measurement samples without processing current window or updating
      * result values.
      * Values are expressed in measurement default unit (m/s^2 for acceleration, rad/s for
      * angular speed or T for magnetic flux density).
      * Notice that if there are less than {@link #getWindowSize()} processed
      * samples in the window, the remaining ones are considered to be zero
      * when average values and standard deviation is estimated.
      *
      * @param valueX x coordinate of measurement to be added.
      * @param valueY y coordinate of measurement to be added.
      * @param valueZ z coordinate of measurement to be added.
      * @throws LockedException if estimator is currently running.
      */
     public void addTriad(final double valueX, final double valueY, final double valueZ) throws LockedException {
         addTriad(createTriad(valueX, valueY, valueZ, getDefaultUnit()));
     }
 
     /**
      * Adds a triad of measurement samples without processing current window or updating
      * result values.
      * Notice that if there are less than {@link #getWindowSize()} processed
      * samples in the window, the remaining ones are considered to be zero
      * when average values and standard deviation is estimated.
      *
      * @param valueX x coordinate of measurement to be added.
      * @param valueY y coordinate of measurement to be added.
      * @param valueZ z coordinate of measurement to be added.
      * @throws LockedException if estimator is currently running.
      */
     public void addTriad(final M valueX, final M valueY, final M valueZ) throws LockedException {
         addTriad(createTriad(valueX, valueY, valueZ));
     }
 
     /**
      * 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;
         }
 
         windowedSamples.clear();
         avgX = 0.0;
         avgY = 0.0;
         avgZ = 0.0;
         varianceX = 0.0;
         varianceY = 0.0;
         varianceZ = 0.0;
         numberOfProcessedSamples = 0;
 
         if (listener != null) {
             //noinspection unchecked
             listener.onReset((E) this);
         }
 
         return true;
     }
 
     /**
      * Creates a copy of a triad.
      *
      * @param input triad to be copied.
      * @return copy of a triad.
      */
     protected abstract T copyTriad(final T input);
 
     /**
      * Creates a triad with provided values and unit.
      *
      * @param valueX x coordinate value.
      * @param valueY y coordinate value.
      * @param valueZ z coordinate value.
      * @param unit   unit.
      * @return created triad.
      */
     protected abstract T createTriad(final double valueX, final double valueY, final double valueZ, final U unit);
 
     /**
      * Creates a triad with provided values.
      *
      * @param valueX x coordinate value.
      * @param valueY y coordinate value.
      * @param valueZ z coordinate value.
      * @return created triad.
      */
     protected abstract T createTriad(final M valueX, final M valueY, final M valueZ);
 
     /**
      * 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);
 
     /**
      * Internally adds a triad of measurement samples and processes current window if indicated.
      *
      * @param triad   measurement triad to be added.
      * @param process true if window of samples must also be processed, false otherwise.
      * @throws LockedException if estimator is currently running.
      */
     private void internalAdd(final T triad, final boolean process) throws LockedException {
         if (running) {
             throw new LockedException();
         }
 
         running = true;
 
         if (windowedSamples.isEmpty() && listener != null) {
             //noinspection unchecked
             listener.onStart((E) this);
         }
 
         final var wasFilled = isWindowFilled();
         if (wasFilled) {
             // remove first sample
             windowedSamples.removeFirst();
         }
 
         windowedSamples.addLast(copyTriad(triad));
 
         // process window
         if (process) {
             processWindow();
         }
 
         running = false;
 
         if (listener != null) {
             //noinspection unchecked
             listener.onTriadAdded((E) this);
 
             if (!wasFilled && isWindowFilled()) {
                 //noinspection unchecked
                 listener.onWindowFilled((E) this);
             }
         }
     }
 
     /**
      * Processes current windowed samples.
      */
     private void processWindow() {
 
         numberOfProcessedSamples++;
 
         // compute averages
         var averageX = 0.0;
         var averageY = 0.0;
         var averageZ = 0.0;
         for (final var triad : windowedSamples) {
             final var valueX = triad.getValueX();
             final var valueY = triad.getValueY();
             final var valueZ = triad.getValueZ();
 
             averageX += valueX;
             averageY += valueY;
             averageZ += valueZ;
         }
 
         averageX /= windowSize;
         averageY /= windowSize;
         averageZ /= windowSize;
 
         // compute variances
         var varX = 0.0;
         var varY = 0.0;
         var varZ = 0.0;
         for (final var triad : windowedSamples) {
             final var fx = triad.getValueX();
             final var fy = triad.getValueY();
             final var fz = triad.getValueZ();
 
             final var diffX = fx - averageX;
             final var diffY = fy - averageY;
             final var diffZ = fz - averageZ;
 
             final var diffX2 = diffX * diffX;
             final var diffY2 = diffY * diffY;
             final var diffZ2 = diffZ * diffZ;
 
             varX += diffX2;
             varY += diffY2;
             varZ += diffZ2;
         }
 
         final var m = windowSize - 1;
 
         varX /= m;
         varY /= m;
         varZ /= m;
 
         this.avgX = averageX;
         this.avgY = averageY;
         this.avgZ = averageZ;
 
         varianceX = varX;
         varianceY = varY;
         varianceZ = varZ;
     }
 }