PositionAngleDetector.java

/* Copyright 2002-2022 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.propagation.events;

import java.util.function.Function;

import org.hipparchus.analysis.UnivariateFunction;
import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
import org.hipparchus.util.FastMath;
import org.hipparchus.util.MathUtils;
import org.orekit.errors.OrekitIllegalArgumentException;
import org.orekit.errors.OrekitMessages;
import org.orekit.orbits.CircularOrbit;
import org.orekit.orbits.EquinoctialOrbit;
import org.orekit.orbits.KeplerianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.SpacecraftState;
import org.orekit.propagation.events.handlers.EventHandler;
import org.orekit.propagation.events.handlers.StopOnIncreasing;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.TimeSpanMap;

/** Detector for in-orbit position angle.
 * <p>
 * The detector is based on anomaly for {@link OrbitType#KEPLERIAN Keplerian}
 * orbits, latitude argument for {@link OrbitType#CIRCULAR circular} orbits,
 * or longitude argument for {@link OrbitType#EQUINOCTIAL equinoctial} orbits.
 * It does not support {@link OrbitType#CARTESIAN Cartesian} orbits. The
 * angles can be either {@link PositionAngle#TRUE true}, {link {@link PositionAngle#MEAN
 * mean} or {@link PositionAngle#ECCENTRIC eccentric} angles.
 * </p>
 * @author Luc Maisonobe
 * @since 7.1
 */
public class PositionAngleDetector extends AbstractDetector<PositionAngleDetector> {

    /** Orbit type defining the angle type. */
    private final OrbitType orbitType;

    /** Type of position angle. */
    private final PositionAngle positionAngle;

    /** Fixed angle to be crossed. */
    private final double angle;

    /** Position angle extraction function. */
    private final Function<Orbit, Double> positionAngleExtractor;

    /** Estimators for the offset angle, taking care of 2π wrapping and g function continuity. */
    private TimeSpanMap<OffsetEstimator> offsetEstimators;

    /** Build a new detector.
     * <p>The new instance uses default values for maximal checking interval
     * ({@link #DEFAULT_MAXCHECK}) and convergence threshold ({@link
     * #DEFAULT_THRESHOLD}).</p>
     * @param orbitType orbit type defining the angle type
     * @param positionAngle type of position angle
     * @param angle fixed angle to be crossed
     * @exception OrekitIllegalArgumentException if orbit type is {@link OrbitType#CARTESIAN}
     */
    public PositionAngleDetector(final OrbitType orbitType, final PositionAngle positionAngle,
                                 final double angle)
        throws OrekitIllegalArgumentException {
        this(DEFAULT_MAXCHECK, DEFAULT_THRESHOLD, orbitType, positionAngle, angle);
    }

    /** Build a detector.
     * @param maxCheck maximal checking interval (s)
     * @param threshold convergence threshold (s)
     * @param orbitType orbit type defining the angle type
     * @param positionAngle type of position angle
     * @param angle fixed angle to be crossed
     * @exception OrekitIllegalArgumentException if orbit type is {@link OrbitType#CARTESIAN}
     */
    public PositionAngleDetector(final double maxCheck, final double threshold,
                                 final OrbitType orbitType, final PositionAngle positionAngle,
                                 final double angle)
        throws OrekitIllegalArgumentException {
        this(maxCheck, threshold, DEFAULT_MAX_ITER, new StopOnIncreasing<PositionAngleDetector>(),
             orbitType, positionAngle, angle);
    }

    /** Private constructor with full parameters.
     * <p>
     * This constructor is private as users are expected to use the builder
     * API with the various {@code withXxx()} methods to set up the instance
     * in a readable manner without using a huge amount of parameters.
     * </p>
     * @param maxCheck maximum checking interval (s)
     * @param threshold convergence threshold (s)
     * @param maxIter maximum number of iterations in the event time search
     * @param handler event handler to call at event occurrences
     * @param orbitType orbit type defining the angle type
     * @param positionAngle type of position angle
     * @param angle fixed angle to be crossed
     * @exception OrekitIllegalArgumentException if orbit type is {@link OrbitType#CARTESIAN}
     */
    private PositionAngleDetector(final double maxCheck, final double threshold,
                                     final int maxIter, final EventHandler<? super PositionAngleDetector> handler,
                                     final OrbitType orbitType, final PositionAngle positionAngle,
                                     final double angle)
        throws OrekitIllegalArgumentException {

        super(maxCheck, threshold, maxIter, handler);

        this.orbitType        = orbitType;
        this.positionAngle    = positionAngle;
        this.angle            = angle;
        this.offsetEstimators = null;

        switch (orbitType) {
            case KEPLERIAN:
                positionAngleExtractor = o -> ((KeplerianOrbit) orbitType.convertType(o)).getAnomaly(positionAngle);
                break;
            case CIRCULAR:
                positionAngleExtractor = o -> ((CircularOrbit) orbitType.convertType(o)).getAlpha(positionAngle);
                break;
            case EQUINOCTIAL:
                positionAngleExtractor = o -> ((EquinoctialOrbit) orbitType.convertType(o)).getL(positionAngle);
                break;
            default:
                final String sep = ", ";
                throw new OrekitIllegalArgumentException(OrekitMessages.ORBIT_TYPE_NOT_ALLOWED,
                                                         orbitType,
                                                         OrbitType.KEPLERIAN   + sep +
                                                         OrbitType.CIRCULAR    + sep +
                                                         OrbitType.EQUINOCTIAL);
        }

    }

    /** {@inheritDoc} */
    @Override
    protected PositionAngleDetector create(final double newMaxCheck, final double newThreshold,
                                              final int newMaxIter,
                                              final EventHandler<? super PositionAngleDetector> newHandler) {
        return new PositionAngleDetector(newMaxCheck, newThreshold, newMaxIter, newHandler,
                                         orbitType, positionAngle, angle);
    }

    /** Get the orbit type defining the angle type.
     * @return orbit type defining the angle type
     */
    public OrbitType getOrbitType() {
        return orbitType;
    }

    /** Get the type of position angle.
     * @return type of position angle
     */
    public PositionAngle getPositionAngle() {
        return positionAngle;
    }

    /** Get the fixed angle to be crossed (radians).
     * @return fixed angle to be crossed (radians)
     */
    public double getAngle() {
        return angle;
    }

    /** {@inheritDoc} */
    public void init(final SpacecraftState s0, final AbsoluteDate t) {
        super.init(s0, t);
        offsetEstimators = new TimeSpanMap<>(new OffsetEstimator(s0.getOrbit(), +1.0));
    }

    /** Compute the value of the detection function.
     * <p>
     * The value is the angle difference between the spacecraft and the fixed
     * angle to be crossed, with some sign tweaks to ensure continuity.
     * These tweaks imply the {@code increasing} flag in events detection becomes
     * irrelevant here! As an example, the angle always increase in a Keplerian
     * orbit, but this g function will increase and decrease so it
     * will cross the zero value once per orbit, in increasing and decreasing
     * directions on alternate orbits..
     * </p>
     * @param s the current state information: date, kinematics, attitude
     * @return angle difference between the spacecraft and the fixed
     * angle, with some sign tweaks to ensure continuity
     */
    public double g(final SpacecraftState s) {

        final Orbit orbit = s.getOrbit();

        // angle difference
        OffsetEstimator estimator = offsetEstimators.get(s.getDate());
        double          delta     = estimator.delta(orbit);

        // we use a value greater than π for handover in order to avoid
        // several switches to be estimated as the calling propagator
        // and Orbit.shiftedBy have different accuracy. It is sufficient
        // to have a handover roughly opposite to the detected position angle
        while (FastMath.abs(delta) >= 3.5) {
            // we are too far away from the current estimator, we need to set up a new one
            // ensuring that we do have a crossing event in the current orbit
            // and we ensure sign continuity with the current estimator

            // find when the previous estimator becomes invalid
            final AbsoluteDate handover = estimator.dateForOffset(FastMath.copySign(FastMath.PI, delta), orbit);

            // perform handover to a new estimator at this date
            estimator = new OffsetEstimator(orbit, delta);
            delta     = estimator.delta(orbit);
            if (isForward()) {
                offsetEstimators.addValidAfter(estimator, handover.getDate(), false);
            } else {
                offsetEstimators.addValidBefore(estimator, handover.getDate(), false);
            }

        }

        return delta;

    }

    /** Local class for estimating offset angle, handling 2π wrap-up and sign continuity. */
    private class OffsetEstimator {

        /** Target angle. */
        private final double target;

        /** Sign correction to offset. */
        private final double sign;

        /** Reference angle. */
        private final double r0;

        /** Slope of the linearized model. */
        private final double r1;

        /** Reference date. */
        private final AbsoluteDate t0;

        /** Simple constructor.
         * @param orbit current orbit
         * @param currentSign desired sign of the offset at current orbit time (magnitude is ignored)
         */
        OffsetEstimator(final Orbit orbit, final double currentSign) {
            r0     = positionAngleExtractor.apply(orbit);
            target = MathUtils.normalizeAngle(angle, r0);
            sign   = FastMath.copySign(1.0, (r0 - target) * currentSign);
            r1     = orbit.getKeplerianMeanMotion();
            t0     = orbit.getDate();
        }

        /** Compute offset from reference angle.
         * @param orbit current orbit
         * @return offset between current angle and reference angle
         */
        public double delta(final Orbit orbit) {
            final double rawAngle        = positionAngleExtractor.apply(orbit);
            final double linearReference = r0 + r1 * orbit.getDate().durationFrom(t0);
            final double linearizedAngle = MathUtils.normalizeAngle(rawAngle, linearReference);
            return sign * (linearizedAngle - target);
        }

        /** Find date at which offset reaches specified value.
         * <p>
         * This computation is an approximation because it relies on
         * {@link Orbit#shiftedBy(double)} only.
         * </p>
         * @param offset target value for offset angle
         * @param orbit current orbit
         * @return approximate date at which offset reached specified value
         */
        public AbsoluteDate dateForOffset(final double offset, final Orbit orbit) {

            // bracket the search
            final double period = orbit.getKeplerianPeriod();
            final double delta0 = delta(orbit);
            final double searchInf;
            final double searchSup;
            if ((delta0 - offset) * sign >= 0) {
                // the date is before current orbit
                searchInf = -period;
                searchSup = 0;
            } else {
                // the date is after current orbit
                searchInf = 0;
                searchSup = +period;
            }

            // find the date as an offset from current orbit
            final BracketingNthOrderBrentSolver solver = new BracketingNthOrderBrentSolver(getThreshold(), 5);
            final UnivariateFunction            f      = dt -> delta(orbit.shiftedBy(dt)) - offset;
            final double                        root   = solver.solve(getMaxIterationCount(), f, searchInf, searchSup);

            return orbit.getDate().shiftedBy(root);

        }

    }

}