/* 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.
   */
  
  /*
   * MIT License
   *
   * Copyright (c) 2019 Romain Serra
   *
   * Permission is hereby granted, free of charge, to any person obtaining a copy
   * of this software and associated documentation files (the "Software"), to deal
   * in the Software without restriction, including without limitation the rights
   * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   * copies of the Software, and to permit persons to whom the Software is
   * furnished to do so, subject to the following conditions:
   *
   * The above copyright notice and this permission notice shall be included in all
   * copies or substantial portions of the Software.
   *
   * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
   * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
   * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
   * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
   * SOFTWARE.
   */
  package org.orekit.ssa.collision.shorttermencounter.probability.twod;
  
  import org.hipparchus.CalculusFieldElement;
  import org.hipparchus.Field;
  import org.hipparchus.linear.FieldVector;
  import org.hipparchus.linear.MatrixUtils;
  import org.hipparchus.stat.StatUtils;
  import org.hipparchus.util.FastMath;
  import org.hipparchus.util.MathArrays;
  import org.hipparchus.util.MathUtils;
  import org.orekit.ssa.metrics.FieldProbabilityOfCollision;
  import org.orekit.ssa.metrics.ProbabilityOfCollision;
  
  /**
   * Compute the probability of collision using the method described in : "SERRA, Romain, ARZELIER, Denis, JOLDES, Mioara, et
   * al. Fast and accurate computation of orbital collision probability for short-term encounters. Journal of Guidance,
   * Control, and Dynamics, 2016, vol. 39, no 5, p. 1009-1021.".
   * <p>
   * It is one of the recommended methods to use.
   * <p>
   * It assumes :
   *     <ul>
   *         <li>Short encounter leading to a linear relative motion.</li>
   *         <li>Spherical collision object.</li>
   *         <li>Uncorrelated positional covariance.</li>
   *         <li>Gaussian distribution of the position uncertainties.</li>
   *         <li>Deterministic velocity i.e. no velocity uncertainties.</li>
   *     </ul>
   * <p>
   * The following constants are defined when using the empty constructor :
   * <ul>
   *     <li>A default absolute accuracy of 1e-30.</li>
   *     <li>A maximum number of computed terms of 37000.</li>
   * </ul>
   * <p>
   * This implementation has been translated from python from the provided source code of Romain SERRA on the
   * <a href="https://github.com/Serrof/SST/blob/master/collision/short_term_poc.py">following github account</a>
   *
   * @author Vincent Cucchietti
   * @author Romain Serra
   * @since 12.0
   */
  public class Laas2015 extends AbstractShortTermEncounter2DPOCMethod {
  
      /** Default scaling threshold to use when sum becomes large. */
      public static final double DEFAULT_SCALING_THRESHOLD = 1e10;
  
      /**
       * Defines the absolute accuracy of this method. For example, given an absolute accuracy of 1e-10, the probability of
       * collision will be exact until its 1e-10 digit.
       */
      private final double absoluteAccuracy;
  
      /** Defines the max number of terms that the method will use, thus reducing the computation time in particular cases. */
      private final int maxNumberOfTerms;
  
      /**
       * Default constructor.
       * <p>
      * It uses a default absolute accuracy of 1e-30 and a maximum number of terms of 37000 which is the max number of terms
      * computed based on Romain SERRA's observation (p.56 of "Romain Serra. Opérations de proximité en orbite : * évaluation
      * du risque de collision et calcul de manoeuvres optimales pour l’évitement et le rendez-vous. Automatique / *
      * Robotique. INSA de Toulouse, 2015. Français. NNT : 2015ISAT0035. tel-01261497") about Alfano test case 5 where he
      * explains that 37000 terms were enough to meet the required precision of 5 significant digits.
      */
     public Laas2015() {
         this(1.E-30, 37000);
     }
 
     /** Simple constructor.
      * @param absoluteAccuracy absolute accuracy of the result
      * @param maxNumberOfTerms max number of terms to compute
      */
     public Laas2015(final double absoluteAccuracy, final int maxNumberOfTerms) {
         super(ShortTermEncounter2DPOCMethodType.LAAS_2015.name());
         this.absoluteAccuracy = absoluteAccuracy;
         this.maxNumberOfTerms = maxNumberOfTerms;
     }
 
     /** {@inheritDoc} */
     public final ProbabilityOfCollision compute(final double xm, final double ym,
                                                 final double sigmaX,
                                                 final double sigmaY,
                                                 final double radius) {
 
         // CHECKSTYLE: stop Indentation check
         // Initializing recurrent terms
         final double xmSquared     = xm * xm;
         final double ymSquared     = ym * ym;
         final double sigmaXSq      = sigmaX * sigmaX;
         final double sigmaYSq      = sigmaY * sigmaY;
         final double radiusSquared = radius * radius;
 
         final double p        = 1. / (2. * (sigmaX * sigmaX));
         final double phiY     = 1. - (sigmaX / sigmaY * sigmaX / sigmaY);
         final double omegaX   = (xm / (2. * sigmaXSq)) * (xm / (2. * sigmaXSq));
         final double omegaY   = (ym / (2. * sigmaYSq)) * (ym / (2. * sigmaYSq));
         final double bigOmega = phiY * 0.5 + (omegaX + omegaY) / p;
 
         final double alpha0 = 0.5 * FastMath.exp(-(xmSquared / sigmaXSq + ymSquared / sigmaYSq) * 0.5) / sigmaX / sigmaY;
 
         // Lower boundary
         final double l0 = alpha0 * (1. - FastMath.exp(-p * radiusSquared)) / p;
 
         // Upper boundary
         final double u0Temp = alpha0 * (FastMath.exp(p * bigOmega * radiusSquared) -
                 FastMath.exp(-p * radiusSquared)) / (p * (1. + bigOmega));
         final double u0 = u0Temp > 1 ? 1 : u0Temp;
 
         // If the boundaries are close enough to the actual value according to defined relative accuracy
         if (u0 - l0 <= absoluteAccuracy) {
             return new ProbabilityOfCollision(StatUtils.mean(u0, l0), u0, l0, getName(),
                                               isAMaximumProbabilityOfCollisionMethod());
         }
         // Otherwise
         else {
             final int n1 = (int) (2. * FastMath.ceil(FastMath.E * p * radiusSquared * (1. + bigOmega)));
             final double n2_inter =
                     alpha0 * FastMath.exp(p * radiusSquared * bigOmega) /
                             (absoluteAccuracy * p * FastMath.sqrt(MathUtils.TWO_PI * n1) *
                                     (1. + bigOmega));
             final int n2 = (int) FastMath.ceil(FastMath.log(2, n2_inter));
 
             // Number of terms to get the relative accuracy desired
             int nMax = FastMath.max(n1, n2) - 1;
             nMax = FastMath.min(nMax, maxNumberOfTerms);
 
             // Initializing terms used in the equations
             final double pSquared                  = p * p;
             final double pCubed                    = pSquared * p;
             final double pTimesRadiusSquared       = p * radiusSquared;
             final double radiusFourth              = radiusSquared * radiusSquared;
             final double radiusSixth               = radiusFourth * radiusSquared;
             final double phiYSquared               = phiY * phiY;
             final double pPhiY                     = p * phiY;
             final double omegaSum                  = omegaX + omegaY;
             final double pSqTimesHalfPhiYSqPlusOne = pSquared * MathArrays.linearCombination(0.5, phiYSquared, 1., 1.);
 
             final double recurrentTerm0 = MathArrays.linearCombination(0.5, phiY, 1., 1.);
             final double recurrentTerm1 = MathArrays.linearCombination(p, recurrentTerm0, 1., omegaSum);
             final double recurrentTerm2 = MathArrays.linearCombination(1., pSqTimesHalfPhiYSqPlusOne, 2., pPhiY * omegaY);
             final double recurrentTerm3 = recurrentTerm1 * recurrentTerm1;
 
             final double auxiliaryTerm0 = radiusSixth * pCubed * phiYSquared * omegaX;
             final double auxiliaryTerm1 = radiusFourth * pSquared * phiY;
             final double auxiliaryTerm2 = 2. * omegaX * recurrentTerm0;
 
             final double auxiliaryTerm3 = phiY * MathArrays.linearCombination(2., omegaX, 1.5, p) + omegaSum;
             final double auxiliaryTerm4 = pPhiY * recurrentTerm0 * 2.;
             final double auxiliaryTerm5 = p * (2. * phiY + 1.);
 
             double kPlus2 = 2.;
             double kPlus3 = 3.;
             double kPlus4 = 4.;
             double kPlus5 = 5.;
             double halfY  = 2.5;
 
             // Initialize recurrence
             double c0 = alpha0 * radiusSquared;
             double c1 = c0 * radiusSquared * 0.5 * recurrentTerm1;
             double c2 = c0 * (radiusFourth / 12.) * (recurrentTerm3 + recurrentTerm2);
             double c3 = c0 * (radiusSixth / 144.) * (recurrentTerm1 * (recurrentTerm3 + 3. * recurrentTerm2) +
                     2. * (pCubed * (1. + phiYSquared * phiY * 0.5) + 3. * pSquared * phiYSquared * omegaY));
             final double[] initialCoefficients = new double[] { c0, c1, c2, c3 };
 
             double sum              = 0.;
             double rescalingCounter = 0.;
             for (int i = 0; i < FastMath.min(nMax, 4); i++) {
                 sum += initialCoefficients[i];
 
                 // Rescale quantities if necessary
                 if (sum > DEFAULT_SCALING_THRESHOLD) {
                     rescalingCounter += FastMath.log10(DEFAULT_SCALING_THRESHOLD);
                     c0 /= DEFAULT_SCALING_THRESHOLD;
                     c1 /= DEFAULT_SCALING_THRESHOLD;
                     c2 /= DEFAULT_SCALING_THRESHOLD;
                     c3 /= DEFAULT_SCALING_THRESHOLD;
                     sum /= DEFAULT_SCALING_THRESHOLD;
                 }
 
             }
 
             // Iterate
             double temp;
             for (int k = 0; k < nMax - 4; k++) {
 
                 // Rescale quantities if necessary
                 if (sum > DEFAULT_SCALING_THRESHOLD) {
                     rescalingCounter += FastMath.log10(DEFAULT_SCALING_THRESHOLD);
                     c0 /= DEFAULT_SCALING_THRESHOLD;
                     c1 /= DEFAULT_SCALING_THRESHOLD;
                     c2 /= DEFAULT_SCALING_THRESHOLD;
                     c3 /= DEFAULT_SCALING_THRESHOLD;
                     sum /= DEFAULT_SCALING_THRESHOLD;
                 }
 
                 // Recurrence relation
                 final double denominator = kPlus4 * kPlus3;
 
                 temp = c3 * MathArrays.linearCombination(1, recurrentTerm1, kPlus3, auxiliaryTerm5);
                 temp -= c2 * pTimesRadiusSquared * MathArrays.linearCombination(halfY, auxiliaryTerm4, 1, auxiliaryTerm3) /
                         kPlus4;
                 temp += c1 * auxiliaryTerm1 * MathArrays.linearCombination(halfY, pPhiY, 1., auxiliaryTerm2) / denominator;
                 temp -= c0 * auxiliaryTerm0 / (denominator * kPlus2);
                 temp *= radiusSquared / (kPlus4 * kPlus5);
 
                 c0 = c1;
                 c1 = c2;
                 c2 = c3;
                 c3 = temp;
 
                 // Update intermediate variables
                 kPlus2 = kPlus3;
                 kPlus3 = kPlus4;
                 kPlus4 = kPlus5;
                 kPlus5 = kPlus5 + 1.;
                 halfY += 1.;
 
                 // Update sum
                 sum += c3;
 
             }
             // CHECKSTYLE: resume Indentation check
             final double value = sum *
                     FastMath.exp(MathArrays.linearCombination(FastMath.log(10.), rescalingCounter, -p, radiusSquared));
 
             return new ProbabilityOfCollision(value, l0, u0, getName(), isAMaximumProbabilityOfCollisionMethod());
         }
     }
 
     /** {@inheritDoc} */
     public final <T extends CalculusFieldElement<T>> FieldProbabilityOfCollision<T> compute(final T xm, final T ym,
                                                                                             final T sigmaX,
                                                                                             final T sigmaY,
                                                                                             final T radius) {
 
         // CHECKSTYLE: stop Indentation check
         // Initializing recurrent terms
         final Field<T> field = xm.getField();
         final T        zero  = field.getZero();
         final T        one   = field.getOne();
 
         final T xmSquared     = xm.square();
         final T ymSquared     = ym.square();
         final T sigmaXSquared = sigmaX.square();
         final T sigmaYSquared = sigmaY.square();
         final T twoSigmaXY    = sigmaX.multiply(sigmaY).multiply(2);
         final T radiusSquared = radius.square();
         final T radiusFourth  = radiusSquared.square();
         final T radiusSixth   = radiusFourth.multiply(radiusSquared);
 
         final T p                   = sigmaX.square().reciprocal().multiply(0.5);
         final T pTimesRadiusSquared = p.multiply(radiusSquared);
         final T phiY                = sigmaXSquared.divide(sigmaYSquared).negate().add(1.);
         final T omegaX              = xm.divide(sigmaXSquared.multiply(2.)).pow(2.);
         final T omegaY              = ym.divide(sigmaYSquared.multiply(2.)).pow(2.);
         final T omegaSum            = omegaX.add(omegaY);
         final T bigOmega            = phiY.multiply(0.5).add(omegaX.add(omegaY).divide(p));
 
         final T minusP                    = p.negate();
         final T pSquared                  = p.square();
         final T pCubed                    = p.multiply(pSquared);
         final T pPhiY                     = p.multiply(phiY);
         final T phiYSquared               = phiY.square();
         final T pRadiusSquaredBigOmega    = p.multiply(radiusSquared).multiply(bigOmega);
         final T bigOmegaPlusOne           = bigOmega.add(1.);
         final T pSqTimesHalfPhiYSqPlusOne = pSquared.multiply(phiYSquared.multiply(0.5).add(1.));
 
         final T alpha0 = xmSquared.divide(sigmaXSquared).add(ymSquared.divide(sigmaYSquared)).multiply(-0.5).exp()
                                   .divide(twoSigmaXY);
 
         // Lower boundary
         final T l0 = alpha0.multiply(radiusSquared.multiply(minusP).exp().negate().add(1.)).divide(p);
 
         // Upper boundary
         final T u0Temp = alpha0.multiply(pRadiusSquaredBigOmega.exp().subtract(radiusSquared.multiply(minusP).exp()))
                                .divide(p.multiply(bigOmegaPlusOne));
         final T u0 = u0Temp.getReal() > 1 ? one : u0Temp;
 
         // If the boundaries are close enough to the actual value according to defined relative accuracy
         if (u0.getReal() - l0.getReal() <= absoluteAccuracy) {
             return new FieldProbabilityOfCollision<>(u0.add(l0).multiply(0.5), u0, l0, getName(),
                                                      isAMaximumProbabilityOfCollisionMethod());
         }
         // Otherwise
         else {
             final int n1 = (int) (2. *
                     FastMath.ceil(FastMath.E * p.multiply(radiusSquared).multiply(bigOmegaPlusOne).getReal()));
             final double n2_inter =
                     alpha0.getReal() * FastMath.exp(pRadiusSquaredBigOmega.getReal()) /
                             (absoluteAccuracy * p.getReal() * FastMath.sqrt(MathUtils.TWO_PI * n1) *
                                     (1. + bigOmega.getReal()));
             final int n2 = (int) FastMath.ceil(FastMath.log(2., n2_inter));
 
             // Number of terms to get the relative accuracy desired
             int nMax = FastMath.max(n1, n2) - 1;
             nMax = FastMath.min(nMax, maxNumberOfTerms);
 
             // Recurrent term in the equations
             final T recurrentTerm0 = phiY.multiply(0.5).add(1);
             final T recurrentTerm1 = p.multiply(recurrentTerm0).add(omegaSum);
             final T recurrentTerm2 = pSqTimesHalfPhiYSqPlusOne.add(pPhiY.multiply(omegaY).multiply(2.));
             final T recurrentTerm3 = recurrentTerm1.multiply(recurrentTerm1);
 
             final T auxiliaryTerm0 = radiusSixth.multiply(pCubed).multiply(phiYSquared).multiply(omegaX);
             final T auxiliaryTerm1 = radiusFourth.multiply(pSquared).multiply(phiY);
             final T auxiliaryTerm2 = omegaX.multiply(recurrentTerm0).multiply(2.);
             final T auxiliaryTerm3 = phiY.multiply(omegaX.multiply(2.).add(p.multiply(1.5))).add(omegaSum);
             final T auxiliaryTerm4 = pPhiY.multiply(recurrentTerm0).multiply(2.);
             final T auxiliaryTerm5 = p.multiply(phiY.multiply(2.).add(1.));
 
             T kPlus2 = one.newInstance(2.);
             T kPlus3 = one.newInstance(3.);
             T kPlus4 = one.newInstance(4.);
             T kPlus5 = one.newInstance(5.);
             T halfY  = one.newInstance(2.5);
 
             // Initialize recurrence
             T c0 = alpha0.multiply(radiusSquared);
             T c1 = c0.multiply(radiusSquared).multiply(0.5).multiply(recurrentTerm1);
             T c2 = c0.multiply(radiusFourth.divide(12.)).multiply(recurrentTerm3.add(recurrentTerm2));
             T c3 = c0.multiply(radiusSixth.divide(144.)).multiply(
                     recurrentTerm1.multiply(recurrentTerm2.multiply(3.).add(recurrentTerm3))
                                   .add(pCubed.multiply(2.).multiply(phiYSquared.multiply(phiY).multiply(0.5).add(1.)))
                                   .add(pSquared.multiply(phiYSquared).multiply(omegaY).multiply(6.)));
             final FieldVector<T> initialCoefficients = MatrixUtils.createFieldVector(field, 4);
             initialCoefficients.setEntry(0, c0);
             initialCoefficients.setEntry(1, c1);
             initialCoefficients.setEntry(2, c2);
             initialCoefficients.setEntry(3, c3);
 
             T sum              = zero;
             T rescalingCounter = zero;
             for (int i = 0; i < FastMath.min(nMax, 4); i++) {
                 sum = sum.add(initialCoefficients.getEntry(i));
 
                 // Rescale quantities if necessary
                 if (sum.getReal() > DEFAULT_SCALING_THRESHOLD) {
                     rescalingCounter = rescalingCounter.add(FastMath.log10(DEFAULT_SCALING_THRESHOLD));
                     c0               = c0.divide(DEFAULT_SCALING_THRESHOLD);
                     c1               = c1.divide(DEFAULT_SCALING_THRESHOLD);
                     c2               = c2.divide(DEFAULT_SCALING_THRESHOLD);
                     c3               = c3.divide(DEFAULT_SCALING_THRESHOLD);
                     sum              = sum.divide(DEFAULT_SCALING_THRESHOLD);
                 }
 
             }
 
             // Iterate
             T temp;
             for (int k = 0; k < nMax - 4; k++) {
 
                 // Rescale quantities if necessary
                 if (sum.getReal() > DEFAULT_SCALING_THRESHOLD) {
                     rescalingCounter = rescalingCounter.add(FastMath.log10(DEFAULT_SCALING_THRESHOLD));
                     c0               = c0.divide(DEFAULT_SCALING_THRESHOLD);
                     c1               = c1.divide(DEFAULT_SCALING_THRESHOLD);
                     c2               = c2.divide(DEFAULT_SCALING_THRESHOLD);
                     c3               = c3.divide(DEFAULT_SCALING_THRESHOLD);
                     sum              = sum.divide(DEFAULT_SCALING_THRESHOLD);
                 }
 
                 // Recurrence relation
                 final T denominator = kPlus4.multiply(kPlus3);
 
                 temp = c3.multiply(recurrentTerm1.add(auxiliaryTerm5.multiply(kPlus3)));
                 temp = temp.subtract(
                         c2.multiply(pTimesRadiusSquared).multiply(auxiliaryTerm4.multiply(halfY).add(auxiliaryTerm3))
                           .divide(kPlus4));
                 temp = temp.add(
                         c1.multiply(auxiliaryTerm1).multiply(pPhiY.multiply(halfY).add(auxiliaryTerm2)).divide(denominator));
                 temp = temp.subtract(c0.multiply(auxiliaryTerm0).divide(denominator.multiply(kPlus2)));
                 temp = temp.multiply(radiusSquared.divide(kPlus4.multiply(kPlus5)));
 
                 c0 = c1;
                 c1 = c2;
                 c2 = c3;
                 c3 = temp;
 
                 // Update intermediate variables
                 kPlus2 = kPlus3;
                 kPlus3 = kPlus4;
                 kPlus4 = kPlus5;
                 kPlus5 = kPlus5.add(1.);
                 halfY  = halfY.add(1.);
 
                 // Update sum
                 sum = sum.add(c3);
 
             }
             final T value =
                     sum.multiply((rescalingCounter.multiply(FastMath.log(10.)).subtract(pTimesRadiusSquared)).exp());
 
             return new FieldProbabilityOfCollision<>(value, l0, u0, getName(), isAMaximumProbabilityOfCollisionMethod());
             // CHECKSTYLE: resume Indentation check
         }
     }
 
     /** {@inheritDoc} */
     @Override
     public ShortTermEncounter2DPOCMethodType getType() {
         return ShortTermEncounter2DPOCMethodType.LAAS_2015;
     }
 
 }