AmbiguitySolver

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

/** 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()) {
                                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 int n = freeDrivers.size();
        final int[] indirection = new int[n];
        for (int i = 0; i < n; ++i) {
            indirection[i] = -1;
            final String name = freeDrivers.get(i).getName();
            for (int k = 0; k < measurementsParametersDrivers.size(); ++k) {
                if (name.equals(measurementsParametersDrivers.get(k).getName())) {
                    indirection[i] = startIndex + k;
                    break;
                }
            }
            if (indirection[i] < 0) {
                // the parameter was not found
                final StringBuilder builder = new StringBuilder();
                for (final ParameterDriver driver : measurementsParametersDrivers) {
                    if (builder.length() > 0) {
                        builder.append(", ");
                    }
                    builder.append(driver.getName());
                }
                throw new OrekitIllegalArgumentException(OrekitMessages.UNSUPPORTED_PARAMETER_NAME,
                                                         name, builder.toString());
            }
        }

        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();
        final double[]              floatAmbiguities = ambiguities.stream().mapToDouble(d -> d.getValue()).toArray();
        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);
        for (int i = 0; i < indirection.length; ++i) {
            final ParameterDriver driver = measurementsParametersDrivers.get(indirection[i] - startIndex);
            driver.setMinValue(fixedAmbiguities[i]);
            driver.setMaxValue(fixedAmbiguities[i]);
            fixedDrivers.add(driver);
        }

        // 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);

        for (int i = startIndex + 1; i < covariance.getColumnDimension(); i++) {
            if (!belongTo(indirection, i)) {
                final ParameterDriver driver = measurementsParametersDrivers.get(i - startIndex);
                driver.setValue(driver.getValue() - Y.getEntry(i - startIndex));
            }
        }

        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;
    }

}