TDOAModifierUtil

/* Copyright 2002-2024 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.modifiers;

import java.util.Arrays;

import org.hipparchus.analysis.differentiation.Gradient;
import org.orekit.estimation.measurements.EstimatedMeasurement;
import org.orekit.estimation.measurements.EstimatedMeasurementBase;
import org.orekit.estimation.measurements.EstimationModifier;
import org.orekit.estimation.measurements.GroundStation;
import org.orekit.estimation.measurements.ObservedMeasurement;
import org.orekit.propagation.FieldSpacecraftState;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.integration.AbstractGradientConverter;
import org.orekit.utils.Differentiation;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.ParameterDriversProvider;
import org.orekit.utils.TimeSpanMap.Span;

/** Utility class for TDOA measurements.
 * @author Pascal Parraud
 * @since 11.2
 */
class TDOAModifierUtil {

    /** Private constructor for utility class.*/
    private TDOAModifierUtil() {
        // not used
    }

    /** Apply a modifier to an estimated measurement.
     * @param <T> type of the measurement
     * @param estimated estimated measurement to modify
     * @param primeStation prime station
     * @param secondStation second station
     * @param modelEffect model effect
     * @deprecated as of 12.1, replaced by {@link #modifyWithoutDerivatives(EstimatedMeasurementBase,
     * GroundStation, GroundStation, ParametricModelEffect, EstimationModifier)}
     */
    @Deprecated
    public static <T extends ObservedMeasurement<T>> void modifyWithoutDerivatives(final EstimatedMeasurementBase<T> estimated,
                                                                                   final GroundStation primeStation,
                                                                                   final GroundStation secondStation,
                                                                                   final ParametricModelEffect modelEffect) {
        modifyWithoutDerivatives(estimated, primeStation, secondStation, modelEffect, null);
    }

    /** Apply a modifier to an estimated measurement.
     * @param <T> type of the measurement
     * @param estimated estimated measurement to modify
     * @param primeStation prime station
     * @param secondStation second station
     * @param modelEffect model effect
     * @param modifier applied modifier
     * @since 12.1
     */
    public static <T extends ObservedMeasurement<T>> void modifyWithoutDerivatives(final EstimatedMeasurementBase<T> estimated,
                                                                                   final GroundStation primeStation,
                                                                                   final GroundStation secondStation,
                                                                                   final ParametricModelEffect modelEffect,
                                                                                   final EstimationModifier<T> modifier) {

        final SpacecraftState state       = estimated.getStates()[0];
        final double[]        oldValue    = estimated.getEstimatedValue();
        final double          primeDelay  = modelEffect.evaluate(primeStation, state);
        final double          secondDelay = modelEffect.evaluate(secondStation, state);

        // Update estimated value taking into account the ionospheric delay for each downlink.
        // The ionospheric time delay is directly applied to the TDOA.
        final double[] newValue = oldValue.clone();
        newValue[0] += primeDelay;
        newValue[0] -= secondDelay;
        estimated.modifyEstimatedValue(modifier, newValue);
    }

    /** Apply a modifier to an estimated measurement.
     * @param <T> type of the measurement
     * @param estimated estimated measurement to modify
     * @param primeStation prime station
     * @param secondStation second station
     * @param converter gradient converter
     * @param parametricModel parametric modifier model
     * @param modelEffect model effect
     * @param modelEffectGradient model effect gradient
     * @deprecated as of 12.1, replaced by {@link #modify(EstimatedMeasurement,
     * ParameterDriversProvider, AbstractGradientConverter, GroundStation, GroundStation,
     * ParametricModelEffect, ParametricModelEffectGradient, EstimationModifier)}
     */
    @Deprecated
    public static <T extends ObservedMeasurement<T>> void modify(final EstimatedMeasurement<T> estimated,
                                                                 final ParameterDriversProvider parametricModel,
                                                                 final AbstractGradientConverter converter,
                                                                 final GroundStation primeStation, final GroundStation secondStation,
                                                                 final ParametricModelEffect modelEffect,
                                                                 final ParametricModelEffectGradient modelEffectGradient) {
        modify(estimated, parametricModel, converter, primeStation, secondStation,
               modelEffect, modelEffectGradient, null);
    }

    /** Apply a modifier to an estimated measurement.
     * @param <T> type of the measurement
     * @param estimated estimated measurement to modify
     * @param primeStation prime station
     * @param secondStation second station
     * @param converter gradient converter
     * @param parametricModel parametric modifier model
     * @param modelEffect model effect
     * @param modelEffectGradient model effect gradient
     * @param modifier applied modifier
     * @since 12.1
     */
    public static <T extends ObservedMeasurement<T>> void modify(final EstimatedMeasurement<T> estimated,
                                                                 final ParameterDriversProvider parametricModel,
                                                                 final AbstractGradientConverter converter,
                                                                 final GroundStation primeStation, final GroundStation secondStation,
                                                                 final ParametricModelEffect modelEffect,
                                                                 final ParametricModelEffectGradient modelEffectGradient,
                                                                 final EstimationModifier<T> modifier) {

        final SpacecraftState state    = estimated.getStates()[0];
        final double[]        oldValue = estimated.getEstimatedValue();

        // Update estimated derivatives with Jacobian of the measure wrt state
        final FieldSpacecraftState<Gradient> gState = converter.getState(parametricModel);
        final Gradient[] gParameters       = converter.getParameters(gState, parametricModel);
        final Gradient   primeGDelay       = modelEffectGradient.evaluate(primeStation, gState, gParameters);
        final Gradient   secondGDelay      = modelEffectGradient.evaluate(secondStation, gState, gParameters);
        final double[]   primeDerivatives  = primeGDelay.getGradient();
        final double[]   secondDerivatives = secondGDelay.getGradient();

        final double[][] stateDerivatives  = estimated.getStateDerivatives(0);
        for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
            stateDerivatives[0][jcol] += primeDerivatives[jcol];
            stateDerivatives[0][jcol] -= secondDerivatives[jcol];
        }
        estimated.setStateDerivatives(0, stateDerivatives);

        int index = 0;
        for (final ParameterDriver driver : parametricModel.getParametersDrivers()) {
            if (driver.isSelected()) {
                for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

                    // update estimated derivatives with derivative of the modification wrt ionospheric parameters
                    double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
                    parameterDerivative += primeDerivatives[index + converter.getFreeStateParameters()];
                    parameterDerivative -= secondDerivatives[index + converter.getFreeStateParameters()];
                    estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
                    index += 1;
                }
            }

        }

        // Update derivatives with respect to primary station position
        for (final ParameterDriver driver : Arrays.asList(primeStation.getClockOffsetDriver(),
                                                          primeStation.getEastOffsetDriver(),
                                                          primeStation.getNorthOffsetDriver(),
                                                          primeStation.getZenithOffsetDriver())) {
            if (driver.isSelected()) {
                for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

                    double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
                    parameterDerivative += Differentiation.differentiate((d, t) -> modelEffect.evaluate(primeStation, state),
                                                                     3, 10.0 * driver.getScale()).value(driver, state.getDate());
                    estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
                }
            }
        }

        // Update derivatives with respect to secondary station position
        for (final ParameterDriver driver : Arrays.asList(secondStation.getClockOffsetDriver(),
                                                          secondStation.getEastOffsetDriver(),
                                                          secondStation.getNorthOffsetDriver(),
                                                          secondStation.getZenithOffsetDriver())) {
            if (driver.isSelected()) {
                for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {

                    double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
                    parameterDerivative -= Differentiation.differentiate((d, t) -> modelEffect.evaluate(secondStation, state),
                                                                     3, 10.0 * driver.getScale()).value(driver, state.getDate());
                    estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
                }
            }
        }

        // Update estimated value taking into account the ionospheric delay for each downlink.
        // The ionospheric time delay is directly applied to the TDOA.
        final double[] newValue = oldValue.clone();
        newValue[0] += primeGDelay.getReal();
        newValue[0] -= secondGDelay.getReal();
        estimated.modifyEstimatedValue(modifier, newValue);
    }

}