/* Copyright 2002-2013 CS Systèmes d'Information
    * Licensed to CS Systèmes d'Information (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.semianalytical.dsst.forces;
  
  import java.util.TreeMap;
  
  import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  import org.apache.commons.math3.util.FastMath;
  import org.orekit.bodies.CelestialBody;
  import org.orekit.errors.OrekitException;
  import org.orekit.propagation.SpacecraftState;
  import org.orekit.propagation.events.EventDetector;
  import org.orekit.propagation.semianalytical.dsst.utilities.AuxiliaryElements;
  import org.orekit.propagation.semianalytical.dsst.utilities.CoefficientsFactory;
  import org.orekit.propagation.semianalytical.dsst.utilities.CoefficientsFactory.NSKey;
  import org.orekit.propagation.semianalytical.dsst.utilities.UpperBounds;
  import org.orekit.time.AbsoluteDate;
  
  /** Third body attraction perturbation to the
   *  {@link org.orekit.propagation.semianalytical.dsst.DSSTPropagator DSSTPropagator}.
   *
   *  @author Romain Di Costanzo
   *  @author Pascal Parraud
   */
  public class DSSTThirdBody  implements DSSTForceModel {
  
      /** Max power for summation. */
      private static final int       MAX_POWER = 22;
  
      /** Truncation tolerance for big, eccentric  orbits. */
      private static final double    BIG_TRUNCATION_TOLERANCE = 1.e-1;
  
      /** Truncation tolerance for small orbits. */
      private static final double    SMALL_TRUNCATION_TOLERANCE = 1.e-4;
  
      /** The 3rd body to consider. */
      private final CelestialBody    body;
  
      /** Standard gravitational parameter &mu; for the body in m<sup>3</sup>/s<sup>2</sup>. */
      private final double           gm;
  
      /** Factorial. */
      private final double[]         fact;
  
      /** V<sub>ns</sub> coefficients. */
      private TreeMap<NSKey, Double> Vns;
  
      /** Distance from center of mass of the central body to the 3rd body. */
      private double R3;
  
      // Equinoctial elements (according to DSST notation)
      /** a. */
      private double a;
      /** e<sub>x</sub>. */
      private double k;
      /** e<sub>y</sub>. */
      private double h;
      /** h<sub>x</sub>. */
      private double q;
      /** h<sub>y</sub>. */
      private double p;
  
      /** Eccentricity. */
      private double ecc;
  
      // Direction cosines of the symmetry axis
      /** &alpha;. */
      private double alpha;
      /** &beta;. */
      private double beta;
      /** &gamma;. */
      private double gamma;
  
      // Common factors for potential computation
      /** &Chi; = 1 / sqrt(1 - e<sup>2</sup>) = 1 / B. */
      private double X;
      /** &Chi;<sup>2</sup>. */
      private double XX;
      /** &Chi;<sup>3</sup>. */
      private double XXX;
      /** -2 * a / A. */
      private double m2aoA;
      /** B / A. */
      private double BoA;
      /** 1 / (A * B). */
     private double ooAB;
     /** -C / (2 * A * B). */
     private double mCo2AB;
     /** B / A(1 + B). */
     private double BoABpo;
 
     /** Retrograde factor. */
     private int    I;
 
     /** Maw power for a/R3 in the serie expansion. */
     private int    maxAR3Pow;
 
     /** Maw power for e in the serie expansion. */
     private int    maxEccPow;
 
     /** Complete constructor.
      *  @param body the 3rd body to consider
      *  @see org.orekit.bodies.CelestialBodyFactory
      */
     public DSSTThirdBody(final CelestialBody body) {
         this.body = body;
         this.gm   = body.getGM();
 
         this.maxAR3Pow = Integer.MIN_VALUE;
         this.maxEccPow = Integer.MIN_VALUE;
 
         this.Vns = CoefficientsFactory.computeVns(MAX_POWER);
 
         // Factorials computation
         final int dim = 2 * MAX_POWER;
         this.fact = new double[dim];
         fact[0] = 1.;
         for (int i = 1; i < dim; i++) {
             fact[i] = i * fact[i - 1];
         }
     }
 
     /** Get third body.
      *  @return third body
      */
     public CelestialBody getBody() {
         return body;
     }
 
     /** Computes the highest power of the eccentricity and the highest power
      *  of a/R3 to appear in the truncated analytical power series expansion.
      *  <p>
      *  This method computes the upper value for the 3rd body potential and
      *  determines the maximal powers for the eccentricity and a/R3 producing
      *  potential terms bigger than a defined tolerance.
      *  </p>
      *  @param aux auxiliary elements related to the current orbit
      *  @throws OrekitException if some specific error occurs
      */
     public void initialize(final AuxiliaryElements aux)
         throws OrekitException {
 
         // Initializes specific parameters.
         initializeStep(aux);
 
         // Truncation tolerance.
         final double aor = a / R3;
         final double tol = ( aor > .3 || (aor > .15  && ecc > .25) ) ? BIG_TRUNCATION_TOLERANCE : SMALL_TRUNCATION_TOLERANCE;
 
         // Utilities for truncation
         // Set a lower bound for eccentricity
         final double eo2  = FastMath.max(0.0025, ecc / 2.);
         final double x2o2 = XX / 2.;
         final double[] eccPwr = new double[MAX_POWER];
         final double[] chiPwr = new double[MAX_POWER];
         eccPwr[0] = 1.;
         chiPwr[0] = X;
         for (int i = 1; i < MAX_POWER; i++) {
             eccPwr[i] = eccPwr[i - 1] * eo2;
             chiPwr[i] = chiPwr[i - 1] * x2o2;
         }
 
         // Auxiliary quantities.
         final double ao2rxx = aor / (2. * XX);
         double xmuarn       = ao2rxx * ao2rxx * gm / (X * R3);
         double term         = 0.;
 
         // Compute max power for a/R3 and e.
         maxAR3Pow = 2;
         maxEccPow = 0;
         int n     = 2;
         int m     = 2;
         int nsmd2 = 0;
 
         do {
             // Upper bound for Tnm.
             term =  xmuarn *
                    (fact[n + m] / (fact[nsmd2] * fact[nsmd2 + m])) *
                    (fact[n + m + 1] / (fact[m] * fact[n + 1])) *
                    (fact[n - m + 1] / fact[n + 1]) *
                    eccPwr[m] * UpperBounds.getDnl(XX, chiPwr[m], n + 2, m);
 
             if (term < tol) {
                 if (m == 0) {
                     break;
                 } else if (m < 2) {
                     xmuarn *= ao2rxx;
                     m = 0;
                     n++;
                     nsmd2++;
                 } else {
                     m -= 2;
                     nsmd2++;
                 }
             } else {
                 maxAR3Pow = n;
                 maxEccPow = FastMath.max(m, maxEccPow);
                 xmuarn *= ao2rxx;
                 m++;
                 n++;
             }
         } while (n < MAX_POWER);
 
         maxEccPow = FastMath.min(maxAR3Pow, maxEccPow);
 
     }
 
     /** {@inheritDoc} */
     public void initializeStep(final AuxiliaryElements aux) throws OrekitException {
 
         // Equinoctial elements
         a = aux.getSma();
         k = aux.getK();
         h = aux.getH();
         q = aux.getQ();
         p = aux.getP();
 
         // Retrograde factor
         I = aux.getRetrogradeFactor();
 
         // Eccentricity
         ecc = aux.getEcc();
 
         // Distance from center of mass of the central body to the 3rd body
         final Vector3D bodyPos = body.getPVCoordinates(aux.getDate(), aux.getFrame()).getPosition();
         R3 = bodyPos.getNorm();
 
         // Direction cosines
         final Vector3D bodyDir = bodyPos.normalize();
         alpha = bodyDir.dotProduct(aux.getVectorF());
         beta  = bodyDir.dotProduct(aux.getVectorG());
         gamma = bodyDir.dotProduct(aux.getVectorW());
 
         // Equinoctial coefficients
         final double A = aux.getA();
         final double B = aux.getB();
         final double C = aux.getC();
 
         // &Chi;
         X = 1. / B;
         XX = X * X;
         XXX = X * XX;
         // -2 * a / A
         m2aoA = -2. * a / A;
         // B / A
         BoA = B / A;
         // 1 / AB
         ooAB = 1. / (A * B);
         // -C / 2AB
         mCo2AB = -C * ooAB / 2.;
         // B / A(1 + B)
         BoABpo = BoA / (1. + B);
 
     }
 
     /** {@inheritDoc} */
     public double[] getMeanElementRate(final SpacecraftState currentState) throws OrekitException {
 
         // Compute potential U derivatives
         final double[] dU  = computeUDerivatives();
         final double dUda  = dU[0];
         final double dUdk  = dU[1];
         final double dUdh  = dU[2];
         final double dUdAl = dU[3];
         final double dUdBe = dU[4];
         final double dUdGa = dU[5];
 
         // Compute cross derivatives [Eq. 2.2-(8)]
         // U(alpha,gamma) = alpha * dU/dgamma - gamma * dU/dalpha
         final double UAlphaGamma   = alpha * dUdGa - gamma * dUdAl;
         // U(beta,gamma) = beta * dU/dgamma - gamma * dU/dbeta
         final double UBetaGamma    =  beta * dUdGa - gamma * dUdBe;
         // Common factor
         final double pUAGmIqUBGoAB = (p * UAlphaGamma - I * q * UBetaGamma) * ooAB;
 
         // Compute mean elements rates [Eq. 3.1-(1)]
         final double da =  0.;
         final double dh =  BoA * dUdk + k * pUAGmIqUBGoAB;
         final double dk = -BoA * dUdh - h * pUAGmIqUBGoAB;
         final double dp =  mCo2AB * UBetaGamma;
         final double dq =  mCo2AB * UAlphaGamma * I;
         final double dM =  m2aoA * dUda + BoABpo * (h * dUdh + k * dUdk) + pUAGmIqUBGoAB;
 
         return new double[] {da, dk, dh, dq, dp, dM};
 
     }
 
     /** {@inheritDoc} */
     public double[] getShortPeriodicVariations(final AbsoluteDate date,
                                                final double[] meanElements) throws OrekitException {
         // TODO: not implemented yet, Short Periodic Variations are set to null
         return new double[] {0., 0., 0., 0., 0., 0.};
     }
 
     /** {@inheritDoc} */
     public EventDetector[] getEventsDetectors() {
         return null;
     }
 
     /** Compute potential derivatives.
      *  @return derivatives of the potential with respect to orbital parameters
      *  @throws OrekitException if Hansen coefficients cannot be computed
      */
     private double[] computeUDerivatives() throws OrekitException {
 
         // Hansen coefficients
         final HansenThirdBody hansen = new HansenThirdBody();
         // Gs and Hs coefficients
         final double[][] GsHs = CoefficientsFactory.computeGsHs(k, h, alpha, beta, maxEccPow);
         // Qns coefficients
         final double[][] Qns = CoefficientsFactory.computeQns(gamma, maxAR3Pow, maxEccPow);
         // a / R3 up to power maxAR3Pow
         final double aoR3 = a / R3;
         final double[] aoR3Pow = new double[maxAR3Pow + 1];
         aoR3Pow[0] = 1.;
         for (int i = 1; i <= maxAR3Pow; i++) {
             aoR3Pow[i] = aoR3 * aoR3Pow[i - 1];
         }
 
         // Potential derivatives
         double dUda  = 0.;
         double dUdk  = 0.;
         double dUdh  = 0.;
         double dUdAl = 0.;
         double dUdBe = 0.;
         double dUdGa = 0.;
 
         for (int s = 0; s <= maxEccPow; s++) {
             // Get the current Gs coefficient
             final double gs = GsHs[0][s];
 
             // Compute Gs partial derivatives from 3.1-(9)
             double dGsdh  = 0.;
             double dGsdk  = 0.;
             double dGsdAl = 0.;
             double dGsdBe = 0.;
             if (s > 0) {
                 // First get the G(s-1) and the H(s-1) coefficients
                 final double sxGsm1 = s * GsHs[0][s - 1];
                 final double sxHsm1 = s * GsHs[1][s - 1];
                 // Then compute derivatives
                 dGsdh  = beta  * sxGsm1 - alpha * sxHsm1;
                 dGsdk  = alpha * sxGsm1 + beta  * sxHsm1;
                 dGsdAl = k * sxGsm1 - h * sxHsm1;
                 dGsdBe = h * sxGsm1 + k * sxHsm1;
             }
 
             // Kronecker symbol (2 - delta(0,s))
             final double delta0s = (s == 0) ? 1. : 2.;
 
             for (int n = FastMath.max(2, s); n <= maxAR3Pow; n++) {
                 // (n - s) must be even
                 if ((n - s) % 2 == 0) {
                     // Extract data from previous computation :
                     final double kns   = hansen.getValue(n, s);
                     final double dkns  = hansen.getDerivative(n, s);
                     final double vns   = Vns.get(new NSKey(n, s));
                     final double coef0 = delta0s * aoR3Pow[n] * vns;
                     final double coef1 = coef0 * Qns[n][s];
                     final double coef2 = coef1 * kns;
                     // dQns/dGamma = Q(n, s + 1) from Equation 3.1-(8)
                     // for n = s, Q(n, n + 1) = 0. (Cefola & Broucke, 1975)
                     final double dqns = (n == s) ? 0. : Qns[n][s + 1];
 
                     // Compute dU / da :
                     dUda += coef2 * n * gs;
                     // Compute dU / dh
                     dUdh += coef1 * (kns * dGsdh + h * XXX * gs * dkns);
                     // Compute dU / dk
                     dUdk += coef1 * (kns * dGsdk + k * XXX * gs * dkns);
                     // Compute dU / dAlpha
                     dUdAl += coef2 * dGsdAl;
                     // Compute dU / dBeta
                     dUdBe += coef2 * dGsdBe;
                     // Compute dU / dGamma
                     dUdGa += coef0 * kns * dqns * gs;
                 }
             }
         }
 
         // mu3 / R3
         final double muoR3 = gm / R3;
 
         return new double[] {
             dUda  * muoR3 / a,
             dUdk  * muoR3,
             dUdh  * muoR3,
             dUdAl * muoR3,
             dUdBe * muoR3,
             dUdGa * muoR3
         };
 
     }
 
     /** Hansen coefficients for 3rd body force model.
      *  <p>
      *  Hansen coefficients are functions of the eccentricity.
      *  For a given eccentricity, the computed elements are stored in a map.
      *  </p>
      */
     private class HansenThirdBody {
 
         /** Map to store every Hansen coefficient computed. */
         private TreeMap<NSKey, Double> coefficients;
 
         /** Map to store every Hansen coefficient derivative computed. */
         private TreeMap<NSKey, Double> derivatives;
 
         /** Simple constructor. */
         public HansenThirdBody() {
             coefficients = new TreeMap<CoefficientsFactory.NSKey, Double>();
             derivatives  = new TreeMap<CoefficientsFactory.NSKey, Double>();
             initialize();
         }
 
         /** Get the K<sub>0</sub><sup>n,s</sup> coefficient value.
          *  @param n n value
          *  @param s s value
          *  @return K<sub>0</sub><sup>n,s</sup>
          */
         public double getValue(final int n, final int s) {
             if (coefficients.containsKey(new NSKey(n, s))) {
                 return coefficients.get(new NSKey(n, s));
             } else {
                 // Compute the K0(n,s) coefficients
                 return computeValue(n, s);
             }
         }
 
         /** Get the dK<sub>0</sub><sup>n,s</sup> / d&x; coefficient derivative.
          *  @param n n value
          *  @param s s value
          *  @return dK<sub>j</sub><sup>n,s</sup> / d&x;
          */
         public double getDerivative(final int n, final int s) {
             if (derivatives.containsKey(new NSKey(n, s))) {
                 return derivatives.get(new NSKey(n, s));
             } else {
                 // Compute the dK0(n,s) / dX derivative
                 return computeDerivative(n, s);
             }
         }
 
         /** Initialization. */
         private void initialize() {
             final double ec2 = ecc * ecc;
             final double oX3 = 1. / XXX;
             coefficients.put(new NSKey(0, 0),  1.);
             coefficients.put(new NSKey(0, 1), -1.);
             coefficients.put(new NSKey(1, 0),  1. + 0.5 * ec2);
             coefficients.put(new NSKey(1, 1), -1.5);
             coefficients.put(new NSKey(2, 0),  1. + 1.5 * ec2);
             coefficients.put(new NSKey(2, 1), -2. - 0.5 * ec2);
             derivatives.put(new NSKey(0, 0),  0.);
             derivatives.put(new NSKey(1, 0),  oX3);
             derivatives.put(new NSKey(2, 0),  3. * oX3);
             derivatives.put(new NSKey(2, 1), -oX3);
         }
 
         /** Compute K<sub>0</sub><sup>n,s</sup> from Equation 2.7.3-(7)(8).
          *  @param n n value
          *  @param s s value
          *  @return K<sub>0</sub><sup>n,s</sup>
          */
         private double computeValue(final int n, final int s) {
             // Initialize return value
             double kns = 0.;
 
             if (n == (s - 1)) {
 
                 final NSKey key = new NSKey(s - 2, s - 1);
                 if (coefficients.containsKey(key)) {
                     kns = coefficients.get(key);
                 } else {
                     kns = computeValue(s - 2, s - 1);
                 }
 
                 kns *= -(2. * s - 1.) / s;
 
             } else if (n == s) {
 
                 final NSKey key = new NSKey(s - 1, s);
                 if (coefficients.containsKey(key)) {
                     kns = coefficients.get(key);
                 } else {
                     kns = computeValue(s - 1, s);
                 }
 
                 kns *= (2. * s + 1.) / (s + 1.);
 
             } else if (n > s) {
 
                 final NSKey key1 = new NSKey(n - 1, s);
                 double knM1 = 0.;
                 if (coefficients.containsKey(key1)) {
                     knM1 = coefficients.get(key1);
                 } else {
                     knM1 = computeValue(n - 1, s);
                 }
 
                 final NSKey key2 = new NSKey(n - 2, s);
                 double knM2 = 0.;
                 if (coefficients.containsKey(key2)) {
                     knM2 = coefficients.get(key2);
                 } else {
                     knM2 = computeValue(n - 2, s);
                 }
 
                 final double val1 = (2. * n + 1.) / (n + 1.);
                 final double val2 = (n + s) * (n - s) / (n * (n + 1.) * XX);
 
                 kns = val1 * knM1 - val2 * knM2;
             }
 
             coefficients.put(new NSKey(n, s), kns);
             return kns;
         }
 
         /** Compute dK<sub>0</sub><sup>n,s</sup> / d&x; from Equation 3.2-(3).
          *  @param n n value
          *  @param s s value
          *  @return dK<sub>0</sub><sup>n,s</sup> / d&x;
          */
         private double computeDerivative(final int n, final int s) {
             // Initialize return value
             double dknsdx = 0.;
 
             if (n > s) {
 
                 final NSKey keyNm1 = new NSKey(n - 1, s);
                 double dKnM1 = 0.;
                 if (derivatives.containsKey(keyNm1)) {
                     dKnM1 = derivatives.get(keyNm1);
                 } else {
                     dKnM1 = computeDerivative(n - 1, s);
                 }
 
                 final NSKey keyNm2 = new NSKey(n - 2, s);
                 double dKnM2 = 0.;
                 if (derivatives.containsKey(keyNm2)) {
                     dKnM2 = derivatives.get(keyNm2);
                 } else {
                     dKnM2 = computeDerivative(n - 2, s);
                 }
 
                 final double knM2 = getValue(n - 2, s);
 
                 final double val1 = (2. * n + 1.) / (n + 1.);
                 final double coef = (n + s) * (n - s) / (n * (n + 1.));
                 final double val2 = coef / XX;
                 final double val3 = 2. * coef / XXX;
 
                 dknsdx = val1 * dKnM1 - val2 * dKnM2 + val3 * knM2;
             }
 
             derivatives.put(new NSKey(n, s), dknsdx);
             return dknsdx;
         }
 
     }
 
 }