/* 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.numerical;
  
  import java.util.Arrays;
  
  import org.hipparchus.geometry.euclidean.threed.Vector3D;
  import org.hipparchus.ode.ODEIntegrator;
  import org.hipparchus.ode.nonstiff.ClassicalRungeKuttaIntegrator;
  import org.hipparchus.util.FastMath;
  import org.hipparchus.util.MathUtils;
  import org.hipparchus.util.SinCos;
  import org.orekit.attitudes.Attitude;
  import org.orekit.attitudes.AttitudeProvider;
  import org.orekit.data.DataContext;
  import org.orekit.errors.OrekitException;
  import org.orekit.errors.OrekitMessages;
  import org.orekit.frames.Frame;
  import org.orekit.orbits.CartesianOrbit;
  import org.orekit.orbits.Orbit;
  import org.orekit.orbits.OrbitType;
  import org.orekit.orbits.PositionAngle;
  import org.orekit.propagation.PropagationType;
  import org.orekit.propagation.SpacecraftState;
  import org.orekit.propagation.analytical.gnss.data.GLONASSOrbitalElements;
  import org.orekit.propagation.analytical.gnss.data.GNSSConstants;
  import org.orekit.propagation.integration.AbstractIntegratedPropagator;
  import org.orekit.propagation.integration.StateMapper;
  import org.orekit.time.AbsoluteDate;
  import org.orekit.time.GLONASSDate;
  import org.orekit.utils.AbsolutePVCoordinates;
  import org.orekit.utils.Constants;
  import org.orekit.utils.IERSConventions;
  import org.orekit.utils.PVCoordinates;
  import org.orekit.utils.TimeStampedPVCoordinates;
  
  /**
   * This class propagates GLONASS orbits using numerical integration.
   * <p>
   * As recommended by the GLONASS Interface Control Document (ICD),
   * a {@link ClassicalRungeKuttaIntegrator  4th order Runge-Kutta technique}
   * shall be used to integrate the equations.
   * </p>
   * <p>
   * Classical used of this orbit propagator is to compute GLONASS satellite
   * coordinates from the navigation message.
   * </p>
   * <p>
   * If the projections of luni-solar accelerations to axes of
   * Greenwich geocentric coordinates {@link GLONASSOrbitalElements#getXDotDot() X''(tb)},
   * {@link GLONASSOrbitalElements#getYDotDot() Y''(tb)} and {@link GLONASSOrbitalElements#getZDotDot() Z''(tb)}
   * are available in the navigation message; a transformation is performed to convert these
   * accelerations into the correct coordinate system. In the case where they are not
   * available into the navigation message, these accelerations are computed.
   * </p>
   *
   * @see <a href="http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD-GLONASS-CDMA-General.-Edition-1.0-2016.pdf">
   *       GLONASS Interface Control Document</a>
   *
   * @author Bryan Cazabonne
   *
   */
  public class GLONASSNumericalPropagator extends AbstractIntegratedPropagator {
  
      /** Second degree coefficient of normal potential. */
      private static final double GLONASS_J20 = 1.08262575e-3;
  
      /** Equatorial radius of Earth (m). */
      private static final double GLONASS_EARTH_EQUATORIAL_RADIUS = 6378136;
  
      /** Value of the Earth's rotation rate in rad/s (See Ref). */
      private static final double GLONASS_AV = 7.2921151467e-5;
  
      // Data used to solve Kepler's equation
      /** First coefficient to compute Kepler equation solver starter. */
      private static final double A;
  
      /** Second coefficient to compute Kepler equation solver starter. */
      private static final double B;
  
      static {
          final double k1 = 3 * FastMath.PI + 2;
          final double k2 = FastMath.PI - 1;
          final double k3 = 6 * FastMath.PI - 1;
          A  = 3 * k2 * k2 / k1;
         B  = k3 * k3 / (6 * k1);
     }
 
     /** The GLONASS orbital elements used. */
     private final GLONASSOrbitalElements glonassOrbit;
 
     /** Initial date in GLONASS form. */
     private final GLONASSDate initDate;
 
     /** The spacecraft mass (kg). */
     private final double mass;
 
     /** The ECI frame used for GLONASS propagation. */
     private final Frame eci;
 
     /** Direction cosines and distance of perturbing body: Moon.
      * <p>
      * <ul>
      * <li>double[0] = ξ<sub>m</sub></li>
      * <li>double[1] = η<sub>m</sub></li>
      * <li>double[2] = ψ<sub>m</sub></li>
      * <li>double[3] = r<sub>m</sub></li>
      * </ul>
      * </p>
      */
     private double[] moonElements;
 
     /** Direction cosines and distance of perturbing body: Sun.
      * <p>
      * <ul>
      * <li>double[0] = ξ<sub>s</sub></li>
      * <li>double[1] = η<sub>s</sub></li>
      * <li>double[2] = ψ<sub>s</sub></li>
      * <li>double[3] = r<sub>s</sub></li>
      * </ul>
      * </p>
      */
     private double[] sunElements;
 
     /** Flag for availability of projections of acceleration transmitted within the navigation message. */
     private final boolean isAccAvailable;
 
     /** Data context used for propagation. */
     private final DataContext dataContext;
 
     /**
      * Private constructor.
      * @param integrator Runge-Kutta integrator
      * @param glonassOrbit Glonass orbital elements
      * @param eci Earth Centered Inertial frame
      * @param provider Attitude provider
      * @param mass Satellite mass (kg)
      * @param context Data context
      * @param isAccAvailable true if the acceleration  is transmitted within the navigation message
      */
     public GLONASSNumericalPropagator(final ClassicalRungeKuttaIntegrator integrator,
                                       final GLONASSOrbitalElements glonassOrbit,
                                       final Frame eci, final AttitudeProvider provider,
                                       final double mass, final DataContext context,
                                       final boolean isAccAvailable) {
         super(integrator, PropagationType.MEAN);
         this.dataContext = context;
         this.isAccAvailable = isAccAvailable;
         // Stores the GLONASS orbital elements
         this.glonassOrbit = glonassOrbit;
         // Sets the Earth Centered Inertial frame
         this.eci = eci;
         // Sets the mass
         this.mass = mass;
         this.initDate = new GLONASSDate(
                 glonassOrbit.getDate(),
                 dataContext.getTimeScales().getGLONASS());
 
         // Initialize state mapper
         initMapper();
         setInitialState();
         setAttitudeProvider(provider);
         setOrbitType(OrbitType.CARTESIAN);
         // It is not meaningful for propagation in Cartesian parameters
         setPositionAngleType(PositionAngle.TRUE);
         setMu(GNSSConstants.GLONASS_MU);
 
         // As recommended by GLONASS ICD (2016), the direction cosines and distance
         // of perturbing body are calculated one time (at tb).
         if (!isAccAvailable) {
             computeMoonElements(initDate);
             computeSunElements(initDate);
         }
     }
 
     /**
      * Gets the underlying GLONASS orbital elements.
      *
      * @return the underlying GLONASS orbital elements
      */
     public GLONASSOrbitalElements getGLONASSOrbitalElements() {
         return glonassOrbit;
     }
 
     /** {@inheritDoc} */
     @Override
     public SpacecraftState propagate(final AbsoluteDate date) {
         // Spacecraft state in inertial frame
         final SpacecraftState stateInInertial = super.propagate(date);
 
         // Build the spacecraft state in inertial frame
         final PVCoordinates pvInPZ90 = getPVInPZ90(stateInInertial);
         final AbsolutePVCoordinates absPV = new AbsolutePVCoordinates(
                 dataContext.getFrames().getPZ9011(IERSConventions.IERS_2010, true),
                 stateInInertial.getDate(), pvInPZ90);
         final TimeStampedPVCoordinates pvInInertial = absPV.getPVCoordinates(eci);
         final SpacecraftState transformedState = new SpacecraftState(new CartesianOrbit(pvInInertial, eci, pvInInertial.getDate(), GNSSConstants.GLONASS_MU),
                                                                      stateInInertial.getAttitude(),
                                                                      stateInInertial.getMass(),
                                                                      stateInInertial.getAdditionalStatesValues(),
                                                                      stateInInertial.getAdditionalStatesDerivatives());
 
         return transformedState;
     }
 
     /**
      * Set the initial state.
      * <p>
      * The initial conditions on position and velocity are in the ECEF coordinate system PZ-90.
      * Previous to orbit integration, they must be transformed to an absolute inertial coordinate system.
      * </p>
      */
     private void setInitialState() {
 
         // Transform initial PV coordinates to an absolute inertial coordinate system.
         final PVCoordinates pvInInertial = getPVInInertial(initDate);
 
         // Create a new orbit
         final Orbit orbit = new CartesianOrbit(pvInInertial,
                                                eci, initDate.getDate(),
                                                GNSSConstants.GLONASS_MU);
 
         // Reset the initial state to apply the transformation
         resetInitialState(new SpacecraftState(orbit, mass));
     }
 
     /**
      * This method computes the direction cosines and the distance used to
      * compute the gravitational perturbations of the Moon.
      *
      * @param date the computation date in GLONASS scale
      */
     private void computeMoonElements(final GLONASSDate date) {
 
         moonElements = new double[4];
 
         // Constants
         // Semi-major axis of the Moon's orbit (m)
         final double am = 3.84385243e8;
         // The Moon's orbit eccentricity
         final double em = 0.054900489;
         // Mean inclination of the Moon's orbit to the ecliptic (rad)
         final double im = 0.0898041080;
 
         // Computed parameters
         // Time from epoch 2000 to the instant tb of GLONASS ephemeris broadcast
         final double dtoJD = (glonassOrbit.getTime() - 10800.) / Constants.JULIAN_DAY;
         final double t  = (date.getJD0() + dtoJD - 2451545.0) / 36525;
         final double t2 = t * t;
         // Mean inclination of Earth equator to ecliptic (rad)
         final double eps = 0.4090926006 - 0.0002270711 * t;
         // Mean longitude of the Moon's orbit perigee (rad)
         final double gammaM = 1.4547885346 + 71.0176852437 * t - 0.0001801481 * t2;
         // Mean longitude of the ascending node of the Moon (rad)
         final double omegaM = 2.1824391966 - 33.7570459536 * t + 0.0000362262 * t2;
         // Mean anomaly of the Moon (rad)
         final double qm = 2.3555557435 + 8328.6914257190 * t + 0.0001545547 * t2;
 
         // Commons parameters
         final SinCos scOm  = FastMath.sinCos(omegaM);
         final SinCos scIm  = FastMath.sinCos(im);
         final SinCos scEs  = FastMath.sinCos(eps);
         final SinCos scGm  = FastMath.sinCos(gammaM);
         final double cosOm = scOm.cos();
         final double sinOm = scOm.sin();
         final double cosIm = scIm.cos();
         final double sinIm = scIm.sin();
         final double cosEs = scEs.cos();
         final double sinEs = scEs.sin();
         final double cosGm = scGm.cos();
         final double sinGm = scGm.sin();
 
         // Intermediate parameters
         final double psiStar = cosOm * sinIm;
         final double etaStar = sinOm * sinIm;
         final double epsStar = 1. - cosOm * cosOm * (1. - cosIm);
         final double eps11 = sinOm * cosOm * (1. - cosIm);
         final double eps12 = 1. - sinOm * sinOm * (1. - cosIm);
         final double eta11 = epsStar * cosEs - psiStar * sinEs;
         final double eta12 = eps11 * cosEs + etaStar * sinEs;
         final double psi11 = epsStar * sinEs + psiStar * cosEs;
         final double psi12 = eps11 * sinEs - etaStar * cosEs;
 
         // Eccentric Anomaly
         final double ek = getEccentricAnomaly(qm, em);
 
         // True Anomaly
         final double vk    = getTrueAnomaly(ek, em);
         final SinCos scVk  = FastMath.sinCos(vk);
         final double sinVk = scVk.sin();
         final double cosVk = scVk.cos();
 
         // Direction cosine
         final double epsM = eps11 * (sinVk * cosGm + cosVk * sinGm) + eps12 * (cosVk * cosGm - sinVk * sinGm);
         final double etaM = eta11 * (sinVk * cosGm + cosVk * sinGm) + eta12 * (cosVk * cosGm - sinVk * sinGm);
         final double psiM = psi11 * (sinVk * cosGm + cosVk * sinGm) + psi12 * (cosVk * cosGm - sinVk * sinGm);
 
         // Distance
         final double rm = am * (1. - em * FastMath.cos(ek));
 
         moonElements[0] = epsM;
         moonElements[1] = etaM;
         moonElements[2] = psiM;
         moonElements[3] = rm;
 
     }
 
     /**
      * This method computes the direction cosines and the distance used to
      * compute the gravitational perturbations of the Sun.
      *
      * @param date the computation date in GLONASS scale
      */
     private void computeSunElements(final GLONASSDate date) {
 
         sunElements = new double[4];
 
         // Constants
         //  Major semi-axis of the Earth’s orbit around the Sun (m)
         final double as = 1.49598e11;
         // The eccentricity of the Earth’s orbit around the Sun
         final double es = 0.016719;
 
         // Computed parameters
         // Time from epoch 2000 to the instant tb of GLONASS ephemeris broadcast
         final double dtoJD = (glonassOrbit.getTime() - 10800.) / Constants.JULIAN_DAY;
         final double t  = (date.getJD0() + dtoJD - 2451545.0) / 36525;
         final double t2 = t * t;
         // Mean inclination of Earth equator to ecliptic (rad)
         final double eps = 0.4090926006 - 0.0002270711 * t;
         // Mean tropic longitude of the Sun orbit perigee (rad)
         final double ws = -7.6281824375 + 0.0300101976 * t + 0.0000079741 * t2;
         // Mean anomaly of the Sun (rad)
         final double qs = 6.2400601269 + 628.3019551714 * t - 0.0000026820 * t2;
 
         // Eccentric Anomaly
         final double ek = getEccentricAnomaly(qs, es);
 
         // True Anomaly
         final double vk    =  getTrueAnomaly(ek, es);
         final SinCos scVk  = FastMath.sinCos(vk);
         final double sinVk = scVk.sin();
         final double cosVk = scVk.cos();
 
         // Commons parameters
         final SinCos scWs  = FastMath.sinCos(ws);
         final SinCos scEs  = FastMath.sinCos(eps);
         final double cosWs = scWs.cos();
         final double sinWs = scWs.sin();
         final double cosEs = scEs.cos();
         final double sinEs = scEs.sin();
 
         // Direction cosine
         final double epsS = cosVk * cosWs - sinVk * sinWs;
         final double etaS = cosEs * (sinVk * cosWs + cosVk * sinWs);
         final double psiS = sinEs * (sinVk * cosWs + cosVk * sinWs);
 
         // Distance
         final double rs = as * (1. - es * FastMath.cos(ek));
 
         sunElements[0] = epsS;
         sunElements[1] = etaS;
         sunElements[2] = psiS;
         sunElements[3] = rs;
 
     }
 
     /**
      * Computes the elliptic eccentric anomaly from the mean anomaly.
      * <p>
      * The algorithm used here for solving Kepler equation has been published
      * in: "Procedures for  solving Kepler's Equation", A. W. Odell and
      * R. H. Gooding, Celestial Mechanics 38 (1986) 307-334
      * </p>
      * <p>It has been copied from the OREKIT library (KeplerianOrbit class).</p>
      *
      * @param M mean anomaly (rad)
      * @param e eccentricity
      * @return E the eccentric anomaly
      */
     private double getEccentricAnomaly(final double M, final double e) {
 
         // reduce M to [-PI PI) interval
         final double reducedM = MathUtils.normalizeAngle(M, 0.0);
 
         // compute start value according to A. W. Odell and R. H. Gooding S12 starter
         double E;
         if (FastMath.abs(reducedM) < 1.0 / 6.0) {
             E = reducedM + e * (FastMath.cbrt(6 * reducedM) - reducedM);
         } else {
             if (reducedM < 0) {
                 final double w = FastMath.PI + reducedM;
                 E = reducedM + e * (A * w / (B - w) - FastMath.PI - reducedM);
             } else {
                 final double w = FastMath.PI - reducedM;
                 E = reducedM + e * (FastMath.PI - A * w / (B - w) - reducedM);
             }
         }
 
         final double e1 = 1 - e;
         final boolean noCancellationRisk = (e1 + E * E / 6) >= 0.1;
 
         // perform two iterations, each consisting of one Halley step and one Newton-Raphson step
         for (int j = 0; j < 2; ++j) {
             final double f;
             double fd;
             final SinCos scE  = FastMath.sinCos(E);
             final double fdd  = e * scE.sin();
             final double fddd = e * scE.cos();
             if (noCancellationRisk) {
                 f  = (E - fdd) - reducedM;
                 fd = 1 - fddd;
             } else {
                 f  = eMeSinE(E, e) - reducedM;
                 final double s = FastMath.sin(0.5 * E);
                 fd = e1 + 2 * e * s * s;
             }
             final double dee = f * fd / (0.5 * f * fdd - fd * fd);
 
             // update eccentric anomaly, using expressions that limit underflow problems
             final double w = fd + 0.5 * dee * (fdd + dee * fddd / 3);
             fd += dee * (fdd + 0.5 * dee * fddd);
             E  -= (f - dee * (fd - w)) / fd;
 
         }
 
         // expand the result back to original range
         E += M - reducedM;
 
         return E;
 
     }
 
     /**
      * Accurate computation of E - e sin(E).
      *
      * @param E eccentric anomaly
      * @param e eccentricity
      * @return E - e sin(E)
      */
     private static double eMeSinE(final double E, final double e) {
         double x = (1 - e) * FastMath.sin(E);
         final double mE2 = -E * E;
         double term = E;
         double d    = 0;
         // the inequality test below IS intentional and should NOT be replaced by a check with a small tolerance
         for (double x0 = Double.NaN; !Double.valueOf(x).equals(Double.valueOf(x0));) {
             d += 2;
             term *= mE2 / (d * (d + 1));
             x0 = x;
             x = x - term;
         }
         return x;
     }
 
     /**
      * Get true anomaly from eccentric anomaly and eccentricity.
      *
      * @param ek the eccentric anomaly (rad)
      * @param ecc the eccentricity
      * @return the true anomaly (rad)
      */
     private double getTrueAnomaly(final double ek, final double ecc) {
         final SinCos scek = FastMath.sinCos(ek);
         final double svk  = FastMath.sqrt(1. - ecc * ecc) * scek.sin();
         final double cvk  = scek.cos() - ecc;
         return FastMath.atan2(svk, cvk);
     }
 
     /**
      * This method transforms the PV coordinates obtained after the numerical
      * integration in the ECEF PZ-90.
      *
      * @param state spacecraft state after integration
      * @return the PV coordinates in the ECEF PZ-90.
      */
     private PVCoordinates getPVInPZ90(final SpacecraftState state) {
 
         // Compute time difference between start date and end date
         final double dt = state.getDate().durationFrom(initDate.getDate());
 
         // Position and velocity vectors
         final PVCoordinates pv = state.getPVCoordinates();
         final Vector3D pos = pv.getPosition();
         final Vector3D vel = pv.getVelocity();
 
         // Components of position and velocity vectors
         final double x0 = pos.getX();
         final double y0 = pos.getY();
         final double z0 = pos.getZ();
         final double vx0 = vel.getX();
         final double vy0 = vel.getY();
         final double vz0 = vel.getZ();
 
         // Greenwich Mean Sidereal Time (GMST)
         final GLONASSDate gloDate = new GLONASSDate(
                 state.getDate(),
                 dataContext.getTimeScales().getGLONASS());
         final double gmst = gloDate.getGMST();
 
         final double ti = glonassOrbit.getTime() + dt;
         // We use the GMST instead of the GMT as it is recommended into GLONASS ICD (2016)
         final double s = gmst + GLONASS_AV * (ti - 10800.);
 
         // Commons Parameters
         final SinCos scS  = FastMath.sinCos(s);
         final double cosS = scS.cos();
         final double sinS = scS.sin();
 
         // Transformed coordinates
         final double x = x0 * cosS + y0 * sinS;
         final double y = -x0 * sinS + y0 * cosS;
         final double z = z0;
         final double vx = vx0 * cosS + vy0 * sinS + GLONASS_AV * y;
         final double vy = -vx0 * sinS + vy0 * cosS - GLONASS_AV * x;
         final double vz = vz0;
 
         // Transformed orbit
         return new PVCoordinates(new Vector3D(x, y, z),
                                  new Vector3D(vx, vy, vz));
     }
 
     /**
      * This method computes the PV coordinates of the spacecraft center of mass.
      * The returned PV are expressed in inertial coordinates system at the instant tb.
      *
      * @param date the computation date in GLONASS scale
      * @return the PV Coordinates in inertial coordinates system
      */
     private PVCoordinates getPVInInertial(final GLONASSDate date) {
 
         // Greenwich Mean Sidereal Time (GMST)
         final double gmst = date.getGMST();
 
         final double time = glonassOrbit.getTime();
         final double dt   = time - 10800.;
         // We use the GMST instead of the GMT as it is recommended into GLONASS ICD (2016)
         final double s = gmst + GLONASS_AV * dt;
 
         // Commons Parameters
         final SinCos scS  = FastMath.sinCos(s);
         final double cosS = scS.cos();
         final double sinS = scS.sin();
 
         // PV coordinates in inertial frame
         final double x0  = glonassOrbit.getX() * cosS - glonassOrbit.getY() * sinS;
         final double y0  = glonassOrbit.getX() * sinS + glonassOrbit.getY() * cosS;
         final double z0  = glonassOrbit.getZ();
         final double vx0 = glonassOrbit.getXDot() * cosS - glonassOrbit.getYDot() * sinS - GLONASS_AV * y0;
         final double vy0 = glonassOrbit.getXDot() * sinS + glonassOrbit.getYDot() * cosS + GLONASS_AV * x0;
         final double vz0 = glonassOrbit.getZDot();
         return new PVCoordinates(new Vector3D(x0, y0, z0),
                                  new Vector3D(vx0, vy0, vz0));
     }
 
     @Override
     protected StateMapper createMapper(final AbsoluteDate referenceDate, final double mu,
                                        final OrbitType orbitType, final PositionAngle positionAngleType,
                                        final AttitudeProvider attitudeProvider, final Frame frame) {
         return new Mapper(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
     }
 
     /** Internal mapper. */
     private static class Mapper extends StateMapper {
 
         /**
          * Simple constructor.
          *
          * @param referenceDate reference date
          * @param mu central attraction coefficient (m³/s²)
          * @param orbitType orbit type to use for mapping
          * @param positionAngleType angle type to use for propagation
          * @param attitudeProvider attitude provider
          * @param frame inertial frame
          */
         Mapper(final AbsoluteDate referenceDate, final double mu,
                final OrbitType orbitType, final PositionAngle positionAngleType,
                final AttitudeProvider attitudeProvider, final Frame frame) {
             super(referenceDate, mu, orbitType, positionAngleType, attitudeProvider, frame);
         }
 
         @Override
         public SpacecraftState mapArrayToState(final AbsoluteDate date, final double[] y,
                                                final double[] yDot, final PropagationType type) {
             // The parameter meanOnly is ignored for the GLONASS Propagator
             final double mass = y[6];
             if (mass <= 0.0) {
                 throw new OrekitException(OrekitMessages.SPACECRAFT_MASS_BECOMES_NEGATIVE, mass);
             }
 
             final Orbit orbit       = getOrbitType().mapArrayToOrbit(y, yDot, getPositionAngleType(), date, getMu(), getFrame());
             final Attitude attitude = getAttitudeProvider().getAttitude(orbit, date, getFrame());
 
             return new SpacecraftState(orbit, attitude, mass);
         }
 
         @Override
         public void mapStateToArray(final SpacecraftState state, final double[] y,
                                     final double[] yDot) {
             getOrbitType().mapOrbitToArray(state.getOrbit(), getPositionAngleType(), y, yDot);
             y[6] = state.getMass();
         }
 
     }
 
     @Override
     protected MainStateEquations getMainStateEquations(final ODEIntegrator integ) {
         return new Main();
     }
 
     /** Internal class for orbital parameters integration. */
     private class Main implements MainStateEquations {
 
         /** Derivatives array. */
         private final double[] yDot;
 
         /**
          * Simple constructor.
          */
         Main() {
             yDot = new double[7];
         }
 
         @Override
         public double[] computeDerivatives(final SpacecraftState state) {
 
             // Date in Glonass form
             final GLONASSDate gloDate = new GLONASSDate(
                     state.getDate(),
                     dataContext.getTimeScales().getGLONASS());
 
             // Position and Velocity vectors
             final Vector3D vel = state.getPVCoordinates().getVelocity();
             final Vector3D pos = state.getPVCoordinates().getPosition();
 
             Arrays.fill(yDot, 0.0);
 
             // dPos/dt = Vel
             yDot[0] += vel.getX();
             yDot[1] += vel.getY();
             yDot[2] += vel.getZ();
 
             // Components of position and velocity vectors
             final double x0 = pos.getX();
             final double y0 = pos.getY();
             final double z0 = pos.getZ();
 
             // Normalized values
             final double r  = pos.getNorm();
             final double r2 = r * r;
             final double oor = 1. / r;
             final double oor2 = 1. / r2;
             final double x = x0 * oor;
             final double y = y0 * oor;
             final double z = z0 * oor;
             final double g = GNSSConstants.GLONASS_MU * oor2;
             final double ro = GLONASS_EARTH_EQUATORIAL_RADIUS * oor;
 
             yDot[3] += x * (-g + (-1.5 * GLONASS_J20 * g * ro * ro * (1. - 5. * z * z)));
             yDot[4] += y * (-g + (-1.5 * GLONASS_J20 * g * ro * ro * (1. - 5. * z * z)));
             yDot[5] += z * (-g + (-1.5 * GLONASS_J20 * g * ro * ro * (3. - 5. * z * z)));
 
             // Luni-Solar contribution
             final Vector3D acc;
             if (isAccAvailable) {
                 acc = getLuniSolarPerturbations(gloDate);
             } else {
                 final Vector3D accMoon = computeLuniSolarPerturbations(
                         state, moonElements[0], moonElements[1], moonElements[2],
                         moonElements[3],
                         dataContext.getCelestialBodies().getMoon().getGM());
                 final Vector3D accSun = computeLuniSolarPerturbations(
                         state,
                         sunElements[0], sunElements[1], sunElements[2],
                         sunElements[3],
                         dataContext.getCelestialBodies().getSun().getGM());
                 acc = accMoon.add(accSun);
             }
 
             yDot[3] += acc.getX();
             yDot[4] += acc.getY();
             yDot[5] += acc.getZ();
 
             return yDot.clone();
         }
 
         /**
          * This method computes the accelerations induced by gravitational
          * perturbations of the Sun and the Moon if they are not available into
          * the navigation message data.
          *
          * @param state current state
          * @param eps first direction cosine
          * @param eta second direction cosine
          * @param psi third direction cosine
          * @param r distance of perturbing body
          * @param g body gravitational field constant
          * @return a vector containing the accelerations
          */
         private Vector3D computeLuniSolarPerturbations(final SpacecraftState state, final double eps,
                                                        final double eta, final double psi,
                                                        final double r, final double g) {
 
             // Current pv coordinates
             final PVCoordinates pv = state.getPVCoordinates();
 
             final double oor = 1. / r;
             final double oor2 = oor * oor;
 
             // Normalized variable
             final double x = pv.getPosition().getX() * oor;
             final double y = pv.getPosition().getY() * oor;
             final double z = pv.getPosition().getZ() * oor;
             final double gm = g * oor2;
 
             final double epsmX  = eps - x;
             final double etamY  = eta - y;
             final double psimZ  = psi - z;
             final Vector3D vector = new Vector3D(epsmX, etamY, psimZ);
             final double d2 = vector.getNormSq();
             final double deltaM = FastMath.sqrt(d2) * d2;
 
             // Accelerations
             final double accX = gm * ((epsmX / deltaM) - eps);
             final double accY = gm * ((etamY / deltaM) - eta);
             final double accZ = gm * ((psimZ / deltaM) - psi);
 
             return new Vector3D(accX, accY, accZ);
         }
 
         /**
          * Get the accelerations induced by gravitational
          * perturbations of the Sun and the Moon in a geocentric
          * coordinate system.
          * <p>
          * The accelerations are obtained using projections of accelerations
          * transmitted within navigation message data.
          * </p>
          *
          * @param date the computation date in GLONASS scale
          * @return a vector containing the sum of both accelerations
          */
         private Vector3D getLuniSolarPerturbations(final GLONASSDate date) {
 
             // Greenwich Mean Sidereal Time (GMST)
             final double gmst = date.getGMST();
 
             final double time = glonassOrbit.getTime();
             final double dt   = time - 10800.;
             // We use the GMST instead of the GMT as it is recommended into GLONASS ICD (see Ref)
             final double s = gmst + GLONASS_AV * dt;
 
             // Commons Parameters
             final SinCos scS  = FastMath.sinCos(s);
             final double cosS = scS.cos();
             final double sinS = scS.sin();
 
             // Accelerations
             final double accX = glonassOrbit.getXDotDot() * cosS - glonassOrbit.getYDotDot() * sinS;
             final double accY = glonassOrbit.getXDotDot() * sinS + glonassOrbit.getYDotDot() * cosS;
             final double accZ = glonassOrbit.getZDotDot();
 
             return new Vector3D(accX, accY, accZ);
         }
 
     }
 
 }