/* Copyright 2002-2021 CS GROUP
    * Licensed to CS GROUP (CS) under one or more
    * contributor license agreements.  See the NOTICE file distributed with
    * this work for additional information regarding copyright ownership.
    * CS licenses this file to You 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 org.orekit.estimation.sequential;
  
  import java.util.List;
  
  import org.hipparchus.exception.MathRuntimeException;
  import org.hipparchus.filtering.kalman.ProcessEstimate;
  import org.hipparchus.filtering.kalman.extended.ExtendedKalmanFilter;
  import org.hipparchus.linear.MatrixDecomposer;
  import org.hipparchus.linear.MatrixUtils;
  import org.hipparchus.linear.RealMatrix;
  import org.hipparchus.linear.RealVector;
  import org.orekit.errors.OrekitException;
  import org.orekit.estimation.measurements.ObservedMeasurement;
  import org.orekit.estimation.measurements.PV;
  import org.orekit.estimation.measurements.Position;
  import org.orekit.propagation.Propagator;
  import org.orekit.propagation.conversion.OrbitDeterminationPropagatorBuilder;
  import org.orekit.propagation.conversion.PropagatorBuilder;
  import org.orekit.propagation.numerical.NumericalPropagator;
  import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
  import org.orekit.time.AbsoluteDate;
  import org.orekit.utils.ParameterDriver;
  import org.orekit.utils.ParameterDriversList;
  import org.orekit.utils.ParameterDriversList.DelegatingDriver;
  
  
  /**
   * Implementation of a Kalman filter to perform orbit determination.
   * <p>
   * The filter uses a {@link OrbitDeterminationPropagatorBuilder} to initialize its reference trajectory {@link NumericalPropagator}
   * or {@link DSSTPropagator} .
   * </p>
   * <p>
   * The estimated parameters are driven by {@link ParameterDriver} objects. They are of 3 different types:<ol>
   *   <li><b>Orbital parameters</b>:The position and velocity of the spacecraft, or, more generally, its orbit.<br>
   *       These parameters are retrieved from the reference trajectory propagator builder when the filter is initialized.</li>
   *   <li><b>Propagation parameters</b>: Some parameters modelling physical processes (SRP or drag coefficients etc...).<br>
   *       They are also retrieved from the propagator builder during the initialization phase.</li>
   *   <li><b>Measurements parameters</b>: Parameters related to measurements (station biases, positions etc...).<br>
   *       They are passed down to the filter in its constructor.</li>
   * </ol>
   * <p>
   * The total number of estimated parameters is m, the size of the state vector.
   * </p>
   * <p>
   * The Kalman filter implementation used is provided by the underlying mathematical library Hipparchus.
   * All the variables seen by Hipparchus (states, covariances, measurement matrices...) are normalized
   * using a specific scale for each estimated parameters or standard deviation noise for each measurement components.
   * </p>
   *
   * <p>A {@link KalmanEstimator} object is built using the {@link KalmanEstimatorBuilder#build() build}
   * method of a {@link KalmanEstimatorBuilder}.</p>
   *
   * @author Romain Gerbaud
   * @author Maxime Journot
   * @author Luc Maisonobe
   * @since 9.2
   */
  public class KalmanEstimator {
  
      /** Builders for orbit propagators. */
      private List<OrbitDeterminationPropagatorBuilder> propagatorBuilders;
  
      /** Reference date. */
      private final AbsoluteDate referenceDate;
  
      /** Kalman filter process model. */
      private final AbstractKalmanModel processModel;
  
      /** Filter. */
      private final ExtendedKalmanFilter<MeasurementDecorator> filter;
  
      /** Observer to retrieve current estimation info. */
      private KalmanObserver observer;
  
      /** Kalman filter estimator constructor (package private).
       * @param decomposer decomposer to use for the correction phase
       * @param propagatorBuilders propagators builders used to evaluate the orbit.
       * @param processNoiseMatricesProviders providers for process noise matrices
       * @param estimatedMeasurementParameters measurement parameters to estimate
       * @param measurementProcessNoiseMatrix provider for measurement process noise matrix
       * @since 10.3
       */
     KalmanEstimator(final MatrixDecomposer decomposer,
                     final List<OrbitDeterminationPropagatorBuilder> propagatorBuilders,
                     final List<CovarianceMatrixProvider> processNoiseMatricesProviders,
                     final ParameterDriversList estimatedMeasurementParameters,
                     final CovarianceMatrixProvider measurementProcessNoiseMatrix) {
 
         this.propagatorBuilders = propagatorBuilders;
         this.referenceDate      = propagatorBuilders.get(0).getInitialOrbitDate();
         this.observer           = null;
 
         // Build the process model and measurement model
         this.processModel = propagatorBuilders.get(0).buildKalmanModel(propagatorBuilders,
                                                                        processNoiseMatricesProviders,
                                                                        estimatedMeasurementParameters,
                                                                        measurementProcessNoiseMatrix);
 
         this.filter = new ExtendedKalmanFilter<>(decomposer, processModel, processModel.getEstimate());
 
     }
 
     /** Set the observer.
      * @param observer the observer
      */
     public void setObserver(final KalmanObserver observer) {
         this.observer = observer;
     }
 
     /** Get the current measurement number.
      * @return current measurement number
      */
     public int getCurrentMeasurementNumber() {
         return processModel.getCurrentMeasurementNumber();
     }
 
     /** Get the current date.
      * @return current date
      */
     public AbsoluteDate getCurrentDate() {
         return processModel.getCurrentDate();
     }
 
     /** Get the "physical" estimated state (i.e. not normalized)
      * @return the "physical" estimated state
      */
     public RealVector getPhysicalEstimatedState() {
         return processModel.getPhysicalEstimatedState();
     }
 
     /** Get the "physical" estimated covariance matrix (i.e. not normalized)
      * @return the "physical" estimated covariance matrix
      */
     public RealMatrix getPhysicalEstimatedCovarianceMatrix() {
         return processModel.getPhysicalEstimatedCovarianceMatrix();
     }
 
     /** Get the orbital parameters supported by this estimator.
      * <p>
      * If there are more than one propagator builder, then the names
      * of the drivers have an index marker in square brackets appended
      * to them in order to distinguish the various orbits. So for example
      * with one builder generating Keplerian orbits the names would be
      * simply "a", "e", "i"... but if there are several builders the
      * names would be "a[0]", "e[0]", "i[0]"..."a[1]", "e[1]", "i[1]"...
      * </p>
      * @param estimatedOnly if true, only estimated parameters are returned
      * @return orbital parameters supported by this estimator
      */
     public ParameterDriversList getOrbitalParametersDrivers(final boolean estimatedOnly) {
 
         final ParameterDriversList estimated = new ParameterDriversList();
         for (int i = 0; i < propagatorBuilders.size(); ++i) {
             final String suffix = propagatorBuilders.size() > 1 ? "[" + i + "]" : null;
             for (final ParameterDriver driver : propagatorBuilders.get(i).getOrbitalParametersDrivers().getDrivers()) {
                 if (driver.isSelected() || !estimatedOnly) {
                     if (suffix != null && !driver.getName().endsWith(suffix)) {
                         // we add suffix only conditionally because the method may already have been called
                         // and suffixes may have already been appended
                         driver.setName(driver.getName() + suffix);
                     }
                     estimated.add(driver);
                 }
             }
         }
         return estimated;
     }
 
     /** Get the propagator parameters supported by this estimator.
      * @param estimatedOnly if true, only estimated parameters are returned
      * @return propagator parameters supported by this estimator
      */
     public ParameterDriversList getPropagationParametersDrivers(final boolean estimatedOnly) {
 
         final ParameterDriversList estimated = new ParameterDriversList();
         for (PropagatorBuilder builder : propagatorBuilders) {
             for (final DelegatingDriver delegating : builder.getPropagationParametersDrivers().getDrivers()) {
                 if (delegating.isSelected() || !estimatedOnly) {
                     for (final ParameterDriver driver : delegating.getRawDrivers()) {
                         estimated.add(driver);
                     }
                 }
             }
         }
         return estimated;
     }
 
     /** Get the list of estimated measurements parameters.
      * @return the list of estimated measurements parameters
      */
     public ParameterDriversList getEstimatedMeasurementsParameters() {
         return processModel.getEstimatedMeasurementsParameters();
     }
 
     /** Process a single measurement.
      * <p>
      * Update the filter with the new measurement by calling the estimate method.
      * </p>
      * @param observedMeasurement the measurement to process
      * @return estimated propagators
      */
     public Propagator[] estimationStep(final ObservedMeasurement<?> observedMeasurement) {
         try {
             final ProcessEstimate estimate = filter.estimationStep(decorate(observedMeasurement));
             processModel.finalizeEstimation(observedMeasurement, estimate);
             if (observer != null) {
                 observer.evaluationPerformed(processModel);
             }
             return processModel.getEstimatedPropagators();
         } catch (MathRuntimeException mrte) {
             throw new OrekitException(mrte);
         }
     }
 
     /** Process several measurements.
      * @param observedMeasurements the measurements to process in <em>chronologically sorted</em> order
      * @return estimated propagators
      */
     public Propagator[] processMeasurements(final Iterable<ObservedMeasurement<?>> observedMeasurements) {
         Propagator[] propagators = null;
         for (ObservedMeasurement<?> observedMeasurement : observedMeasurements) {
             propagators = estimationStep(observedMeasurement);
         }
         return propagators;
     }
 
     /** Decorate an observed measurement.
      * <p>
      * The "physical" measurement noise matrix is the covariance matrix of the measurement.
      * Normalizing it consists in applying the following equation: Rn[i,j] =  R[i,j]/σ[i]/σ[j]
      * Thus the normalized measurement noise matrix is the matrix of the correlation coefficients
      * between the different components of the measurement.
      * </p>
      * @param observedMeasurement the measurement
      * @return decorated measurement
      */
     private MeasurementDecorator decorate(final ObservedMeasurement<?> observedMeasurement) {
 
         // Normalized measurement noise matrix contains 1 on its diagonal and correlation coefficients
         // of the measurement on its non-diagonal elements.
         // Indeed, the "physical" measurement noise matrix is the covariance matrix of the measurement
         // Normalizing it leaves us with the matrix of the correlation coefficients
         final RealMatrix covariance;
         if (observedMeasurement instanceof PV) {
             // For PV measurements we do have a covariance matrix and thus a correlation coefficients matrix
             final PV pv = (PV) observedMeasurement;
             covariance = MatrixUtils.createRealMatrix(pv.getCorrelationCoefficientsMatrix());
         } else if (observedMeasurement instanceof Position) {
             // For Position measurements we do have a covariance matrix and thus a correlation coefficients matrix
             final Position position = (Position) observedMeasurement;
             covariance = MatrixUtils.createRealMatrix(position.getCorrelationCoefficientsMatrix());
         } else {
             // For other measurements we do not have a covariance matrix.
             // Thus the correlation coefficients matrix is an identity matrix.
             covariance = MatrixUtils.createRealIdentityMatrix(observedMeasurement.getDimension());
         }
 
         return new MeasurementDecorator(observedMeasurement, covariance, referenceDate);
 
     }
 
 }