AmbiguitySolver

/* 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.gnss;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.stream.Collectors;

import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.QRDecomposer;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.linear.RealVector;
import org.hipparchus.util.FastMath;
import org.orekit.errors.OrekitIllegalArgumentException;
import org.orekit.errors.OrekitMessages;
import org.orekit.utils.ParameterDriver;
import org.orekit.utils.TimeSpanMap.Span;

/** Class for solving integer ambiguity problems.
 * @see LambdaMethod
 * @author Luc Maisonobe
 * @since 10.0
 */
public class AmbiguitySolver {

    /** Drivers for ambiguity drivers. */
    private final List<ParameterDriver> ambiguityDrivers;

    /** Solver for the underlying Integer Least Square problem. */
    private final IntegerLeastSquareSolver solver;

    /** Acceptance test to use. */
    private final AmbiguityAcceptance acceptance;

    /** Simple constructor.
     * @param ambiguityDrivers drivers for ambiguity parameters
     * @param solver solver for the underlying Integer Least Square problem
     * @param acceptance acceptance test to use
     * @see LambdaMethod
     */
    public AmbiguitySolver(final List<ParameterDriver> ambiguityDrivers,
                           final IntegerLeastSquareSolver solver,
                           final AmbiguityAcceptance acceptance) {
        this.ambiguityDrivers = ambiguityDrivers;
        this.solver           = solver;
        this.acceptance       = acceptance;
    }

    /** Get all the ambiguity parameters drivers.
     * @return all ambiguity parameters drivers
     */
    public List<ParameterDriver> getAllAmbiguityDrivers() {
        return Collections.unmodifiableList(ambiguityDrivers);
    }

    /** Get the ambiguity parameters drivers that have not been fixed yet.
     * @return ambiguity parameters drivers that have not been fixed yet
     */
    protected List<ParameterDriver> getFreeAmbiguityDrivers() {
        return ambiguityDrivers.
                        stream().
                        filter(d -> {
                            if (d.isSelected()) {
                                // in order to make the code generic and compatible with pDriver having
                                // 1 or several values driven getValue is called with a "random date"
                                // it should be OK as we take the near number
                                final double near   = FastMath.rint(d.getValue());
                                final double gapMin = near - d.getMinValue();
                                final double gapMax = d.getMaxValue() - near;
                                return FastMath.max(FastMath.abs(gapMin), FastMath.abs(gapMax)) > 1.0e-15;
                            } else {
                                return false;
                            }
                        }).
                        collect(Collectors.toList());
    }

    /** Get ambiguity indirection array for ambiguity parameters drivers that have not been fixed yet.
     * @param startIndex start index for measurements parameters in global covariance matrix
     * @param measurementsParametersDrivers measurements parameters drivers in global covariance matrix order
     * @return indirection array between full covariance matrix and ambiguity covariance matrix
     */
    protected int[] getFreeAmbiguityIndirection(final int startIndex,
                                                final List<ParameterDriver> measurementsParametersDrivers) {

        // set up indirection array
        final List<ParameterDriver> freeDrivers = getFreeAmbiguityDrivers();
        final List<String> measurementsPDriversNames = new ArrayList<String>();
        int totalValuesToEstimate = 0;
        for (ParameterDriver driver : freeDrivers) {
            totalValuesToEstimate += driver.getNbOfValues();
        }
        for (ParameterDriver measDriver : measurementsParametersDrivers) {
            for (Span<String> spanMeasurementsParametersDrivers = measDriver.getNamesSpanMap().getFirstSpan();
                    spanMeasurementsParametersDrivers != null; spanMeasurementsParametersDrivers = spanMeasurementsParametersDrivers.next()) {
                measurementsPDriversNames.add(spanMeasurementsParametersDrivers.getData());
            }

        }

        final int n = freeDrivers.size();
        final int[] indirection = new int[totalValuesToEstimate];
        int nb = 0;
        for (int i = 0; i < n; ++i) {

            for (Span<String> spanFreeDriver = freeDrivers.get(i).getNamesSpanMap().getFirstSpan(); spanFreeDriver != null; spanFreeDriver = spanFreeDriver.next()) {
                indirection[nb] = -1;

                for (int k = 0; k < measurementsPDriversNames.size(); ++k) {
                    if (spanFreeDriver.getData().equals(measurementsPDriversNames.get(k))) {
                        indirection[nb] = startIndex + k;
                        break;
                    }
                }

                if (indirection[nb] < 0) {
                    // the parameter was not found
                    final StringBuilder builder = new StringBuilder();
                    for (final String driverName : measurementsPDriversNames) {
                        if (builder.length() > 0) {
                            builder.append(", ");
                        }
                        builder.append(driverName);
                    }
                    throw new OrekitIllegalArgumentException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
                                                             spanFreeDriver.getData(), builder.toString());
                }
                nb++;
            }
        }

        return indirection;

    }

    /** Un-fix an integer ambiguity (typically after a phase cycle slip).
     * @param ambiguityDriver driver for the ambiguity to un-fix
     */
    public void unFixAmbiguity(final ParameterDriver ambiguityDriver) {
        ambiguityDriver.setMinValue(Double.NEGATIVE_INFINITY);
        ambiguityDriver.setMaxValue(Double.POSITIVE_INFINITY);
    }

    /** Fix integer ambiguities.
     * @param startIndex start index for measurements parameters in global covariance matrix
     * @param measurementsParametersDrivers measurements parameters drivers in global covariance matrix order
     * @param covariance global covariance matrix
     * @return list of newly fixed ambiguities (ambiguities already fixed before the call are not counted)
     */
    public List<ParameterDriver> fixIntegerAmbiguities(final int startIndex,
                                                       final List<ParameterDriver> measurementsParametersDrivers,
                                                       final RealMatrix covariance) {

        // set up Integer Least Square problem
        final List<ParameterDriver> ambiguities      = getAllAmbiguityDrivers();

        // construct floatambiguities array
        int nbPDriver = 0;
        for (ParameterDriver pDriver : ambiguities) {
            nbPDriver += pDriver.getNbOfValues();
        }
        final double[]              floatAmbiguities = new double[nbPDriver];
        int floatAmbRank = 0;
        for (ParameterDriver pDriver : ambiguities) {
            for (Span<Double> span = pDriver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
                floatAmbiguities[floatAmbRank++] = span.getData();
            }
        }

        final int[]                 indirection      = getFreeAmbiguityIndirection(startIndex, measurementsParametersDrivers);
        // solve the ILS problem
        final IntegerLeastSquareSolution[] candidates =
                        solver.solveILS(acceptance.numberOfCandidates(), floatAmbiguities, indirection, covariance);

        // FIXME A cleaner way is:
        //     1°/ Add a getName() method in IntegerLeastSquareSolver interface
        //     2°/ Add static name attribute to IntegerBootstrapping, LAMBDA and ModifiedLAMBDA classes
        if (solver instanceof IntegerBootstrapping && candidates.length == 0) {
            return Collections.emptyList();
        }

        // check number of candidates
        if (candidates.length < acceptance.numberOfCandidates()) {
            return Collections.emptyList();
        }

        // check acceptance
        final IntegerLeastSquareSolution bestCandidate = acceptance.accept(candidates);
        if (bestCandidate == null) {
            return Collections.emptyList();
        }

        // fix the ambiguities
        final long[] fixedAmbiguities = bestCandidate.getSolution();
        final List<ParameterDriver> fixedDrivers = new ArrayList<>(indirection.length);
        int nb = 0;
        for (int i = 0; i < measurementsParametersDrivers.size(); ++i) {
            final ParameterDriver driver = measurementsParametersDrivers.get(indirection[nb] - startIndex);
            driver.setMinValue(fixedAmbiguities[i]);
            driver.setMaxValue(fixedAmbiguities[i]);
            fixedDrivers.add(driver);
            nb += driver.getNbOfValues();
        }

        // Update the others parameter drivers accordingly to the fixed integer ambiguity
        // Covariance matrix between integer ambiguity and the other parameter driver
        final RealMatrix Qab = getCovMatrix(covariance, indirection);

        final RealVector X = new QRDecomposer(1.0e-10).decompose(getAmbiguityMatrix(covariance, indirection)).solve(MatrixUtils.createRealVector(floatAmbiguities).
                                                                                                           subtract(MatrixUtils.createRealVector(toDoubleArray(fixedAmbiguities.length, fixedAmbiguities))));
        final RealVector Y =  Qab.preMultiply(X);

        int entry = 0;
        for (int i = startIndex + 1; i < covariance.getColumnDimension(); i++) {
            if (!belongTo(indirection, i)) {
                final ParameterDriver driver = measurementsParametersDrivers.get(i - startIndex);
                for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {

                    driver.setValue(driver.getValue(span.getStart()) - Y.getEntry(entry++ - startIndex), span.getStart());
                }
            }
        }

        return fixedDrivers;

    }

   /** Get the covariance matrix between the integer ambiguities and the other parameter driver.
    * @param cov global covariance matrix
    * @param indirection array of the position of integer ambiguity parameter driver
    * @return covariance matrix.
    */
    private RealMatrix getCovMatrix(final RealMatrix cov, final int[] indirection) {
        final RealMatrix Qab = MatrixUtils.createRealMatrix(indirection.length, cov.getColumnDimension());
        int index = 0;
        int iter  = 0;
        while (iter < indirection.length) {
            // Loop on column dimension
            for (int j = 0; j < cov.getColumnDimension(); j++) {
                if (!belongTo(indirection, j)) {
                    Qab.setEntry(index, 0, cov.getEntry(index, 0));
                }
            }
            index++;
            iter++;
        }
        return Qab;
    }

     /** Return the matrix of the ambiguity from the global covariance matrix.
      * @param cov global covariance matrix
      * @param indirection array of the position of the ambiguity within the global covariance matrix
      * @return matrix of ambiguities covariance
      */
    private RealMatrix getAmbiguityMatrix(final RealMatrix cov, final int[] indirection) {
        final RealMatrix Qa = MatrixUtils.createRealMatrix(indirection.length, indirection.length);
        for (int i = 0; i < indirection.length; i++) {
            Qa.setEntry(i, i, cov.getEntry(indirection[i], indirection[i]));
            for (int j = 0; j < i; j++) {
                Qa.setEntry(i, j, cov.getEntry(indirection[i], indirection[j]));
                Qa.setEntry(j, i, cov.getEntry(indirection[i], indirection[j]));
            }
        }
        return Qa;
    }

    /** Compute whether or not the integer pos belongs to the indirection array.
     * @param indirection array of the position of ambiguities within the global covariance matrix
     * @param pos integer for which we want to know if it belong to the indirection array.
     * @return true if it belongs.
     */
    private boolean belongTo(final int[] indirection, final int pos) {
        for (int j : indirection) {
            if (pos == j) {
                return true;
            }
        }
        return false;
    }

    /** Transform an array of long to an array of double.
     * @param size size of the destination array
     * @param longArray source array
     * @return the destination array
     */
    private double[] toDoubleArray(final int size, final long[] longArray) {
        // Initialize double array
        final double[] doubleArray = new double[size];
        // Copy the elements
        for (int index = 0; index < size; index++) {
            doubleArray[index] = longArray[index];
        }
        return doubleArray;
    }

}