/* Copyright 2002-2025 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.measurements;
  
  import java.util.ArrayList;
  import java.util.Arrays;
  import java.util.IdentityHashMap;
  import java.util.List;
  import java.util.Map;
  import java.util.function.Function;
  
  import org.orekit.propagation.SpacecraftState;
  import org.orekit.time.AbsoluteDate;
  import org.orekit.utils.ParameterDriver;
  import org.orekit.utils.ParameterDriversList;
  import org.orekit.utils.TimeSpanMap;
  import org.orekit.utils.TimeStampedPVCoordinates;
  import org.orekit.utils.TimeSpanMap.Span;
  
  /** Class multiplexing several measurements as one.
   * <p>
   * Date comes from the first measurement, observed and estimated
   * values result from gathering all underlying measurements values.
   *
   * @author Luc Maisonobe
   * @since 10.1
   */
  public class MultiplexedMeasurement extends AbstractMeasurement<MultiplexedMeasurement> {
  
      /** Type of the measurement. */
      public static final String MEASUREMENT_TYPE = "MultiplexedMeasurement";
  
      /** Multiplexed measurements. */
      private final List<ObservedMeasurement<?>> observedMeasurements;
  
      /** Multiplexed measurements without derivatives.
       */
      private final List<EstimatedMeasurementBase<?>> estimatedMeasurementsWithoutDerivatives;
  
      /** Multiplexed measurements. */
      private final List<EstimatedMeasurement<?>> estimatedMeasurements;
  
      /** Multiplexed parameters drivers. */
      private final ParameterDriversList parametersDrivers;
  
      /** Total dimension. */
      private final int dimension;
  
      /** Total number of satellites involved. */
      private final int nbSat;
  
      /** States mapping. */
      private final int[][] multiplexedToUnderlying;
  
      /** States mapping. */
      private final int[][] underlyingToMultiplexed;
  
      /** Simple constructor.
       * @param measurements measurements to multiplex
       * @since 10.1
       */
      public MultiplexedMeasurement(final List<ObservedMeasurement<?>> measurements) {
          super(measurements.get(0).getDate(),
                multiplex(measurements, ComparableMeasurement::getObservedValue),
                multiplex(measurements, ObservedMeasurement::getTheoreticalStandardDeviation),
                multiplex(measurements, ObservedMeasurement::getBaseWeight),
                multiplex(measurements));
  
          this.observedMeasurements                    = measurements;
          this.estimatedMeasurementsWithoutDerivatives = new ArrayList<>();
          this.estimatedMeasurements                   = new ArrayList<>();
          this.parametersDrivers                       = new ParameterDriversList();
  
          // gather parameters drivers
          int dim = 0;
          for (final ObservedMeasurement<?> m : measurements) {
              for (final ParameterDriver driver : m.getParametersDrivers()) {
                  parametersDrivers.add(driver);
              }
              dim += m.getDimension();
          }
          parametersDrivers.sort();
          for (final ParameterDriver driver : parametersDrivers.getDrivers()) {
              addParameterDriver(driver);
          }
         this.dimension = dim;
 
         // set up states mappings for observed satellites
         final List<ObservableSatellite> deduplicated = getSatellites();
         this.nbSat   = deduplicated.size();
         this.multiplexedToUnderlying = new int[measurements.size()][];
         this.underlyingToMultiplexed = new int[measurements.size()][deduplicated.size()];
         for (int i = 0; i < multiplexedToUnderlying.length; ++i) {
             final List<ObservableSatellite> satellites = measurements.get(i).getSatellites();
             multiplexedToUnderlying[i] = new int[satellites.size()];
             for (int j = 0; j < multiplexedToUnderlying[i].length; ++j) {
                 final int index = satellites.get(j).getPropagatorIndex();
                 for (int k = 0; k < nbSat; ++k) {
                     if (deduplicated.get(k).getPropagatorIndex() == index) {
                         multiplexedToUnderlying[i][j] = k;
                         underlyingToMultiplexed[i][k] = j;
                         break;
                     }
                 }
             }
         }
 
     }
 
     /** Get the underlying measurements.
      * @return underlying measurements
      */
     public List<ObservedMeasurement<?>> getMeasurements() {
         return observedMeasurements;
     }
 
     /** Get the underlying estimated measurements without derivatives.
      * @return underlying estimated measurements without derivatives
      * @since 12.0
      */
     public List<EstimatedMeasurementBase<?>> getEstimatedMeasurementsWithoutDerivatives() {
         return estimatedMeasurementsWithoutDerivatives;
     }
 
     /** Get the underlying estimated measurements.
      * @return underlying estimated measurements
      */
     public List<EstimatedMeasurement<?>> getEstimatedMeasurements() {
         return estimatedMeasurements;
     }
 
     /** Get the spacecraft state index in the underlying measurement.
      * @param measurementIndex index of the underlying measurement
      * @param multiplexedStateIndex index of the spacecraft state in the multiplexed array
      * @return spacecraft state index in the underlying measurement
      * @since 13.0
      */
     public int getUnderlyingStateIndex(final int measurementIndex, final int multiplexedStateIndex) {
         return multiplexedToUnderlying[measurementIndex][multiplexedStateIndex];
     }
 
     /** Get the spacecraft state index in the multiplexed measurement.
      * @param measurementIndex index of the underlying measurement
      * @param underlyingStateIndex index of the spacecraft state in the underlying array
      * @return spacecraft state index in the multiplexed measurement
      * @since 13.0
      */
     public int getMultiplexedStateIndex(final int measurementIndex, final int underlyingStateIndex) {
         return underlyingToMultiplexed[measurementIndex][underlyingStateIndex];
     }
 
     /** {@inheritDoc} */
     @Override
     protected EstimatedMeasurementBase<MultiplexedMeasurement> theoreticalEvaluationWithoutDerivatives(final int iteration,
                                                                                                        final int evaluation,
                                                                                                        final SpacecraftState[] states) {
 
         final SpacecraftState[]              evaluationStates = new SpacecraftState[nbSat];
         final double[]                       value            = new double[dimension];
 
         // loop over all multiplexed measurements
         estimatedMeasurementsWithoutDerivatives.clear();
         int index = 0;
         for (int i = 0; i < observedMeasurements.size(); ++i) {
 
             // filter states involved in the current measurement
             final SpacecraftState[] filteredStates = new SpacecraftState[multiplexedToUnderlying[i].length];
             for (int j = 0; j < multiplexedToUnderlying[i].length; ++j) {
                 filteredStates[j] = states[getUnderlyingStateIndex(i, j)];
             }
 
             // perform evaluation
             final EstimatedMeasurementBase<?> eI = observedMeasurements.get(i).estimateWithoutDerivatives(iteration, evaluation, filteredStates);
             estimatedMeasurementsWithoutDerivatives.add(eI);
 
             // extract results
             final double[] valueI = eI.getEstimatedValue();
             System.arraycopy(valueI, 0, value, index, valueI.length);
             index += valueI.length;
 
             // extract states
             final SpacecraftState[] statesI = eI.getStates();
             for (int j = 0; j < multiplexedToUnderlying[i].length; ++j) {
                 evaluationStates[multiplexedToUnderlying[i][j]] = statesI[j];
             }
 
         }
 
         // create multiplexed estimation
         final EstimatedMeasurementBase<MultiplexedMeasurement> multiplexed =
                         new EstimatedMeasurementBase<>(this, iteration, evaluation,
                                                        evaluationStates,
                                                        new TimeStampedPVCoordinates[0]);
 
         // copy multiplexed value
         multiplexed.setEstimatedValue(value);
 
         return multiplexed;
 
     }
 
     /** {@inheritDoc} */
     @Override
     protected EstimatedMeasurement<MultiplexedMeasurement> theoreticalEvaluation(final int iteration, final int evaluation,
                                                                                  final SpacecraftState[] states) {
 
         final SpacecraftState[]              evaluationStates = new SpacecraftState[nbSat];
         final double[]                       value            = new double[dimension];
 
         // loop over all multiplexed measurements
         estimatedMeasurements.clear();
         int index = 0;
         for (int i = 0; i < observedMeasurements.size(); ++i) {
 
             // filter states involved in the current measurement
             final SpacecraftState[] filteredStates = new SpacecraftState[multiplexedToUnderlying[i].length];
             for (int j = 0; j < multiplexedToUnderlying[i].length; ++j) {
                 filteredStates[j] = states[multiplexedToUnderlying[i][j]];
             }
 
             // perform evaluation
             final EstimatedMeasurement<?> eI = observedMeasurements.get(i).estimate(iteration, evaluation, filteredStates);
             estimatedMeasurements.add(eI);
 
             // extract results
             final double[] valueI = eI.getEstimatedValue();
             System.arraycopy(valueI, 0, value, index, valueI.length);
             index += valueI.length;
 
             // extract states
             final SpacecraftState[] statesI = eI.getStates();
             for (int j = 0; j < multiplexedToUnderlying[i].length; ++j) {
                 evaluationStates[multiplexedToUnderlying[i][j]] = statesI[j];
             }
 
         }
 
         // create multiplexed estimation
         final EstimatedMeasurement<MultiplexedMeasurement> multiplexed =
                         new EstimatedMeasurement<>(this, iteration, evaluation,
                                                    evaluationStates,
                                                    new TimeStampedPVCoordinates[0]);
 
         // copy multiplexed value
         multiplexed.setEstimatedValue(value);
 
         // combine derivatives
         final int                            stateSize             = estimatedMeasurements.get(0).getStateSize();
         final double[]                       zeroDerivative        = new double[stateSize];
         final double[][][]                   stateDerivatives      = new double[nbSat][dimension][];
         for (final double[][] m : stateDerivatives) {
             Arrays.fill(m, zeroDerivative);
         }
 
         final Map<ParameterDriver, TimeSpanMap<double[]>> parametersDerivatives = new IdentityHashMap<>();
         index = 0;
         for (int i = 0; i < observedMeasurements.size(); ++i) {
 
             final EstimatedMeasurement<?> eI   = estimatedMeasurements.get(i);
             final int                     idx  = index;
             final int                     dimI = eI.getObservedMeasurement().getDimension();
 
             // state derivatives
             for (int j = 0; j < multiplexedToUnderlying[i].length; ++j) {
                 System.arraycopy(eI.getStateDerivatives(j), 0,
                                  stateDerivatives[multiplexedToUnderlying[i][j]], index,
                                  dimI);
             }
 
             // parameters derivatives
             eI.getDerivativesDrivers().forEach(driver -> {
                 final ParameterDriversList.DelegatingDriver delegating = parametersDrivers.findByName(driver.getName());
 
                 if (parametersDerivatives.get(delegating) == null) {
                     final TimeSpanMap<double[]> derivativeSpanMap = new TimeSpanMap<>(new double[dimension]);
                     parametersDerivatives.put(delegating, derivativeSpanMap);
                 }
 
                 final TimeSpanMap<Double> driverNameSpan = delegating.getValueSpanMap();
                 for (Span<Double> span = driverNameSpan.getSpan(driverNameSpan.getFirstSpan().getEnd()); span != null; span = span.next()) {
 
                     double[] derivatives = parametersDerivatives.get(delegating).get(span.getStart());
                     if (derivatives == null) {
                         derivatives = new double[dimension];
                     }
                     if (!parametersDerivatives.get(delegating).getSpan(span.getStart()).getStart().equals(span.getStart())) {
                         if ((span.getStart()).equals(AbsoluteDate.PAST_INFINITY)) {
                             parametersDerivatives.get(delegating).addValidBefore(derivatives, span.getEnd(), false);
                         } else {
                             parametersDerivatives.get(delegating).addValidAfter(derivatives, span.getStart(), false);
                         }
 
                     }
 
                     System.arraycopy(eI.getParameterDerivatives(driver, span.getStart()), 0, derivatives, idx, dimI);
 
                 }
 
             });
 
             index += dimI;
 
         }
 
         // set states derivatives
         for (int i = 0; i < nbSat; ++i) {
             multiplexed.setStateDerivatives(i, stateDerivatives[i]);
         }
 
         // set parameters derivatives
         parametersDerivatives.
             entrySet().
             forEach(e -> multiplexed.setParameterDerivatives(e.getKey(), e.getValue()));
 
         return multiplexed;
 
     }
 
     /** Multiplex measurements data.
      * @param measurements measurements to multiplex
      * @param extractor data extraction function
      * @return multiplexed data
      */
     private static double[] multiplex(final List<ObservedMeasurement<?>> measurements,
                                       final Function<ObservedMeasurement<?>, double[]> extractor) {
 
         // gather individual parts
         final List<double[]> parts = new ArrayList<> (measurements.size());
         int n = 0;
         for (final ObservedMeasurement<?> measurement : measurements) {
             final double[] p = extractor.apply(measurement);
             parts.add(p);
             n += p.length;
         }
 
         // create multiplexed data
         final double[] multiplexed = new double[n];
         int index = 0;
         for (final double[] p : parts) {
             System.arraycopy(p, 0, multiplexed, index, p.length);
             index += p.length;
         }
 
         return multiplexed;
 
     }
 
     /** Multiplex satellites data.
      * @param measurements measurements to multiplex
      * @return multiplexed satellites data
      */
     private static List<ObservableSatellite> multiplex(final List<ObservedMeasurement<?>> measurements) {
 
         final List<ObservableSatellite> satellites = new ArrayList<>();
 
         // gather all satellites, removing duplicates
         for (final ObservedMeasurement<?> measurement : measurements) {
             for (final ObservableSatellite satellite : measurement.getSatellites()) {
                 boolean searching = true;
                 for (int i = 0; i < satellites.size() && searching; ++i) {
                     // check if we already know this satellite
                     searching = satellite.getPropagatorIndex() != satellites.get(i).getPropagatorIndex();
                 }
                 if (searching) {
                     // this is a new satellite, add it to the global list
                     satellites.add(satellite);
                 }
             }
         }
 
         return satellites;
 
     }
 
 }