/* Copyright 2002-2015 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.orbits;
  
  import java.io.Serializable;
  import java.util.ArrayList;
  import java.util.Collection;
  import java.util.List;
  
  import org.apache.commons.math3.exception.ConvergenceException;
  import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
  import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
  import org.apache.commons.math3.util.FastMath;
  import org.orekit.errors.OrekitMessages;
  import org.orekit.frames.Frame;
  import org.orekit.time.AbsoluteDate;
  import org.orekit.utils.CartesianDerivativesFilter;
  import org.orekit.utils.PVCoordinates;
  import org.orekit.utils.TimeStampedPVCoordinates;
  
  
  /** This class holds cartesian orbital parameters.
  
   * <p>
   * The parameters used internally are the cartesian coordinates:
   *   <ul>
   *     <li>x</li>
   *     <li>y</li>
   *     <li>z</li>
   *     <li>xDot</li>
   *     <li>yDot</li>
   *     <li>zDot</li>
   *   </ul>
   * contained in {@link PVCoordinates}.
   * </p>
  
   * <p>
   * Note that the implementation of this class delegates all non-cartesian related
   * computations ({@link #getA()}, {@link #getEquinoctialEx()}, ...) to an underlying
   * instance of the {@link EquinoctialOrbit} class. This implies that using this class
   * only for analytical computations which are always based on non-cartesian
   * parameters is perfectly possible but somewhat sub-optimal.
   * </p>
   * <p>
   * The instance <code>CartesianOrbit</code> is guaranteed to be immutable.
   * </p>
   * @see    Orbit
   * @see    KeplerianOrbit
   * @see    CircularOrbit
   * @see    EquinoctialOrbit
   * @author Luc Maisonobe
   * @author Guylaine Prat
   * @author Fabien Maussion
   * @author V&eacute;ronique Pommier-Maurussane
   */
  public class CartesianOrbit extends Orbit {
  
      /** Serializable UID. */
      private static final long serialVersionUID = 20140723L;
  
      /** Underlying equinoctial orbit to which high-level methods are delegated. */
      private transient EquinoctialOrbit equinoctial;
  
      /** Constructor from Cartesian parameters.
       *
       * <p> The acceleration provided in {@code pvCoordinates} is accessible using
       * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
       * use {@code mu} and the position to compute the acceleration, including
       * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}.
       *
       * @param pvaCoordinates the position, velocity and acceleration of the satellite.
       * @param frame the frame in which the {@link PVCoordinates} are defined
       * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
       * @param mu central attraction coefficient (m³/s²)
       * @exception IllegalArgumentException if frame is not a {@link
       * Frame#isPseudoInertial pseudo-inertial frame}
       */
      public CartesianOrbit(final TimeStampedPVCoordinates pvaCoordinates,
                            final Frame frame, final double mu)
          throws IllegalArgumentException {
          super(pvaCoordinates, frame, mu);
          equinoctial = null;
      }
  
      /** Constructor from Cartesian parameters.
      *
      * <p> The acceleration provided in {@code pvCoordinates} is accessible using
      * {@link #getPVCoordinates()} and {@link #getPVCoordinates(Frame)}. All other methods
      * use {@code mu} and the position to compute the acceleration, including
      * {@link #shiftedBy(double)} and {@link #getPVCoordinates(AbsoluteDate, Frame)}.
      *
      * @param pvaCoordinates the position and velocity of the satellite.
      * @param frame the frame in which the {@link PVCoordinates} are defined
      * (<em>must</em> be a {@link Frame#isPseudoInertial pseudo-inertial frame})
      * @param date date of the orbital parameters
      * @param mu central attraction coefficient (m³/s²)
      * @exception IllegalArgumentException if frame is not a {@link
      * Frame#isPseudoInertial pseudo-inertial frame}
      */
     public CartesianOrbit(final PVCoordinates pvaCoordinates, final Frame frame,
                           final AbsoluteDate date, final double mu)
         throws IllegalArgumentException {
         this(new TimeStampedPVCoordinates(date, pvaCoordinates), frame, mu);
     }
 
     /** Constructor from any kind of orbital parameters.
      * @param op orbital parameters to copy
      */
     public CartesianOrbit(final Orbit op) {
         super(op.getPVCoordinates(), op.getFrame(), op.getMu());
         if (op instanceof EquinoctialOrbit) {
             equinoctial = (EquinoctialOrbit) op;
         } else if (op instanceof CartesianOrbit) {
             equinoctial = ((CartesianOrbit) op).equinoctial;
         } else {
             equinoctial = null;
         }
     }
 
     /** {@inheritDoc} */
     public OrbitType getType() {
         return OrbitType.CARTESIAN;
     }
 
     /** Lazy evaluation of equinoctial parameters. */
     private void initEquinoctial() {
         if (equinoctial == null) {
             equinoctial = new EquinoctialOrbit(getPVCoordinates(), getFrame(), getDate(), getMu());
         }
     }
 
     /** {@inheritDoc} */
     public double getA() {
         // lazy evaluation of semi-major axis
         final double r  = getPVCoordinates().getPosition().getNorm();
         final double V2 = getPVCoordinates().getVelocity().getNormSq();
         return r / (2 - r * V2 / getMu());
     }
 
     /** {@inheritDoc} */
     public double getE() {
         final Vector3D pvP   = getPVCoordinates().getPosition();
         final Vector3D pvV   = getPVCoordinates().getVelocity();
         final double rV2OnMu = pvP.getNorm() * pvV.getNormSq() / getMu();
         final double eSE     = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(getMu() * getA());
         final double eCE     = rV2OnMu - 1;
         return FastMath.sqrt(eCE * eCE + eSE * eSE);
     }
 
     /** {@inheritDoc} */
     public double getI() {
         return Vector3D.angle(Vector3D.PLUS_K, getPVCoordinates().getMomentum());
     }
 
     /** {@inheritDoc} */
     public double getEquinoctialEx() {
         initEquinoctial();
         return equinoctial.getEquinoctialEx();
     }
 
     /** {@inheritDoc} */
     public double getEquinoctialEy() {
         initEquinoctial();
         return equinoctial.getEquinoctialEy();
     }
 
     /** {@inheritDoc} */
     public double getHx() {
         final Vector3D w = getPVCoordinates().getMomentum().normalize();
         // Check for equatorial retrograde orbit
         if (((w.getX() * w.getX() + w.getY() * w.getY()) == 0) && w.getZ() < 0) {
             return Double.NaN;
         }
         return -w.getY() / (1 + w.getZ());
     }
 
     /** {@inheritDoc} */
     public double getHy() {
         final Vector3D w = getPVCoordinates().getMomentum().normalize();
         // Check for equatorial retrograde orbit
         if (((w.getX() * w.getX() + w.getY() * w.getY()) == 0) && w.getZ() < 0) {
             return Double.NaN;
         }
         return  w.getX() / (1 + w.getZ());
     }
 
     /** {@inheritDoc} */
     public double getLv() {
         initEquinoctial();
         return equinoctial.getLv();
     }
 
     /** {@inheritDoc} */
     public double getLE() {
         initEquinoctial();
         return equinoctial.getLE();
     }
 
     /** {@inheritDoc} */
     public double getLM() {
         initEquinoctial();
         return equinoctial.getLM();
     }
 
     /** {@inheritDoc} */
     protected TimeStampedPVCoordinates initPVCoordinates() {
         // nothing to do here, as the canonical elements are already the cartesian ones
         return getPVCoordinates();
     }
 
     /** {@inheritDoc} */
     public CartesianOrbit shiftedBy(final double dt) {
         final PVCoordinates shiftedPV = (getA() < 0) ? shiftPVHyperbolic(dt) : shiftPVElliptic(dt);
         return new CartesianOrbit(shiftedPV, getFrame(), getDate().shiftedBy(dt), getMu());
     }
 
     /** {@inheritDoc}
      * <p>
      * The interpolated instance is created by polynomial Hermite interpolation
      * ensuring velocity remains the exact derivative of position.
      * </p>
      * <p>
      * As this implementation of interpolation is polynomial, it should be used only
      * with small samples (about 10-20 points) in order to avoid <a
      * href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a>
      * and numerical problems (including NaN appearing).
      * </p>
      * <p>
      * If orbit interpolation on large samples is needed, using the {@link
      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
      * low-level interpolation. The Ephemeris class automatically handles selection of
      * a neighboring sub-sample with a predefined number of point from a large global sample
      * in a thread-safe way.
      * </p>
      */
     public CartesianOrbit interpolate(final AbsoluteDate date, final Collection<Orbit> sample) {
         final List<TimeStampedPVCoordinates> datedPV = new ArrayList<TimeStampedPVCoordinates>(sample.size());
         for (final Orbit o : sample) {
             datedPV.add(new TimeStampedPVCoordinates(o.getDate(),
                                                      o.getPVCoordinates().getPosition(),
                                                      o.getPVCoordinates().getVelocity(),
                                                      o.getPVCoordinates().getAcceleration()));
         }
         final TimeStampedPVCoordinates interpolated =
                 TimeStampedPVCoordinates.interpolate(date, CartesianDerivativesFilter.USE_PVA, datedPV);
         return new CartesianOrbit(interpolated, getFrame(), date, getMu());
     }
 
     /** Compute shifted position and velocity in elliptic case.
      * @param dt time shift
      * @return shifted position and velocity
      */
     private PVCoordinates shiftPVElliptic(final double dt) {
 
         // preliminary computation
         final Vector3D pvP   = getPVCoordinates().getPosition();
         final Vector3D pvV   = getPVCoordinates().getVelocity();
         final double r       = pvP.getNorm();
         final double rV2OnMu = r * pvV.getNormSq() / getMu();
         final double a       = getA();
         final double eSE     = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(getMu() * a);
         final double eCE     = rV2OnMu - 1;
         final double e2      = eCE * eCE + eSE * eSE;
 
         // we can use any arbitrary reference 2D frame in the orbital plane
         // in order to simplify some equations below, we use the current position as the u axis
         final Vector3D u     = pvP.normalize();
         final Vector3D v     = Vector3D.crossProduct(getPVCoordinates().getMomentum(), u).normalize();
 
         // the following equations rely on the specific choice of u explained above,
         // some coefficients that vanish to 0 in this case have already been removed here
         final double ex      = (eCE - e2) * a / r;
         final double ey      = -FastMath.sqrt(1 - e2) * eSE * a / r;
         final double beta    = 1 / (1 + FastMath.sqrt(1 - e2));
         final double thetaE0 = FastMath.atan2(ey + eSE * beta * ex, r / a + ex - eSE * beta * ey);
         final double thetaM0 = thetaE0 - ex * FastMath.sin(thetaE0) + ey * FastMath.cos(thetaE0);
 
         // compute in-plane shifted eccentric argument
         final double thetaM1 = thetaM0 + getKeplerianMeanMotion() * dt;
         final double thetaE1 = meanToEccentric(thetaM1, ex, ey);
         final double cTE     = FastMath.cos(thetaE1);
         final double sTE     = FastMath.sin(thetaE1);
 
         // compute shifted in-plane cartesian coordinates
         final double exey   = ex * ey;
         final double exCeyS = ex * cTE + ey * sTE;
         final double x      = a * ((1 - beta * ey * ey) * cTE + beta * exey * sTE - ex);
         final double y      = a * ((1 - beta * ex * ex) * sTE + beta * exey * cTE - ey);
         final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS);
         final double xDot   = factor * (-sTE + beta * ey * exCeyS);
         final double yDot   = factor * ( cTE - beta * ex * exCeyS);
 
         final Vector3D shiftedP = new Vector3D(x, u, y, v);
         final double   r2       = x * x + y * y;
         final Vector3D shiftedV = new Vector3D(xDot, u, yDot, v);
         final Vector3D shiftedA = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), shiftedP);
         return new PVCoordinates(shiftedP, shiftedV, shiftedA);
 
     }
 
     /** Compute shifted position and velocity in hyperbolic case.
      * @param dt time shift
      * @return shifted position and velocity
      */
     private PVCoordinates shiftPVHyperbolic(final double dt) {
 
         final PVCoordinates pv = getPVCoordinates();
         final Vector3D pvP   = pv.getPosition();
         final Vector3D pvV   = pv.getVelocity();
         final Vector3D pvM   = pv.getMomentum();
         final double r       = pvP.getNorm();
         final double rV2OnMu = r * pvV.getNormSq() / getMu();
         final double a       = getA();
         final double muA     = getMu() * a;
         final double e       = FastMath.sqrt(1 - Vector3D.dotProduct(pvM, pvM) / muA);
         final double sqrt    = FastMath.sqrt((e + 1) / (e - 1));
 
         // compute mean anomaly
         final double eSH     = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(-muA);
         final double eCH     = rV2OnMu - 1;
         final double H0      = FastMath.log((eCH + eSH) / (eCH - eSH)) / 2;
         final double M0      = e * FastMath.sinh(H0) - H0;
 
         // find canonical 2D frame with p pointing to perigee
         final double v0      = 2 * FastMath.atan(sqrt * FastMath.tanh(H0 / 2));
         final Vector3D p     = new Rotation(pvM, -v0).applyTo(pvP).normalize();
         final Vector3D q     = Vector3D.crossProduct(pvM, p).normalize();
 
         // compute shifted eccentric anomaly
         final double M1      = M0 + getKeplerianMeanMotion() * dt;
         final double H1      = meanToHyperbolicEccentric(M1, e);
 
         // compute shifted in-plane cartesian coordinates
         final double cH     = FastMath.cosh(H1);
         final double sH     = FastMath.sinh(H1);
         final double sE2m1  = FastMath.sqrt((e - 1) * (e + 1));
 
         // coordinates of position and velocity in the orbital plane
         final double x      = a * (cH - e);
         final double y      = -a * sE2m1 * sH;
         final double factor = FastMath.sqrt(getMu() / -a) / (e * cH - 1);
         final double xDot   = -factor * sH;
         final double yDot   =  factor * sE2m1 * cH;
 
         final Vector3D shiftedP = new Vector3D(x, p, y, q);
         final double   r2       = x * x + y * y;
         final Vector3D shiftedV = new Vector3D(xDot, p, yDot, q);
         final Vector3D shiftedA = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), shiftedP);
         return new PVCoordinates(shiftedP, shiftedV, shiftedA);
 
     }
 
     /** Computes the eccentric in-plane argument from the mean in-plane argument.
      * @param thetaM = mean in-plane argument (rad)
      * @param ex first component of eccentricity vector
      * @param ey second component of eccentricity vector
      * @return the eccentric in-plane argument.
      */
     private double meanToEccentric(final double thetaM, final double ex, final double ey) {
         // Generalization of Kepler equation to in-plane parameters
         // with thetaE = eta + E and
         //      thetaM = eta + M = thetaE - ex.sin(thetaE) + ey.cos(thetaE)
         // and eta being counted from an arbitrary reference in the orbital plane
         double thetaE        = thetaM;
         double thetaEMthetaM = 0.0;
         int    iter          = 0;
         do {
             final double cosThetaE = FastMath.cos(thetaE);
             final double sinThetaE = FastMath.sin(thetaE);
 
             final double f2 = ex * sinThetaE - ey * cosThetaE;
             final double f1 = 1.0 - ex * cosThetaE - ey * sinThetaE;
             final double f0 = thetaEMthetaM - f2;
 
             final double f12 = 2.0 * f1;
             final double shift = f0 * f12 / (f1 * f12 - f0 * f2);
 
             thetaEMthetaM -= shift;
             thetaE         = thetaM + thetaEMthetaM;
 
             if (FastMath.abs(shift) <= 1.0e-12) {
                 return thetaE;
             }
 
         } while (++iter < 50);
 
         throw new ConvergenceException();
 
     }
 
     /** Computes the hyperbolic eccentric anomaly from the mean anomaly.
      * <p>
      * The algorithm used here for solving hyperbolic Kepler equation is
      * Danby's iterative method (3rd order) with Vallado's initial guess.
      * </p>
      * @param M mean anomaly (rad)
      * @param ecc eccentricity
      * @return the hyperbolic eccentric anomaly
      */
     private double meanToHyperbolicEccentric(final double M, final double ecc) {
 
         // Resolution of hyperbolic Kepler equation for keplerian parameters
 
         // Initial guess
         double H;
         if (ecc < 1.6) {
             if ((-FastMath.PI < M && M < 0.) || M > FastMath.PI) {
                 H = M - ecc;
             } else {
                 H = M + ecc;
             }
         } else {
             if (ecc < 3.6 && FastMath.abs(M) > FastMath.PI) {
                 H = M - FastMath.copySign(ecc, M);
             } else {
                 H = M / (ecc - 1.);
             }
         }
 
         // Iterative computation
         int iter = 0;
         do {
             final double f3  = ecc * FastMath.cosh(H);
             final double f2  = ecc * FastMath.sinh(H);
             final double f1  = f3 - 1.;
             final double f0  = f2 - H - M;
             final double f12 = 2. * f1;
             final double d   = f0 / f12;
             final double fdf = f1 - d * f2;
             final double ds  = f0 / fdf;
 
             final double shift = f0 / (fdf + ds * ds * f3 / 6.);
 
             H -= shift;
 
             if (FastMath.abs(shift) <= 1.0e-12) {
                 return H;
             }
 
         } while (++iter < 50);
 
         throw new ConvergenceException(OrekitMessages.UNABLE_TO_COMPUTE_HYPERBOLIC_ECCENTRIC_ANOMALY,
                                        iter);
     }
 
     @Override
     public void getJacobianWrtCartesian(final PositionAngle type, final double[][] jacobian) {
         // this is the fastest way to set the 6x6 identity matrix
         for (int i = 0; i < 6; i++) {
             for (int j = 0; j < 6; j++) {
                 jacobian[i][j] = 0;
             }
             jacobian[i][i] = 1;
         }
     }
 
     @Override
     protected double[][] computeJacobianMeanWrtCartesian() {
         // not used
         return null;
     }
     @Override
     protected double[][] computeJacobianEccentricWrtCartesian() {
         // not used
         return null;
     }
 
     @Override
     protected double[][] computeJacobianTrueWrtCartesian() {
         // not used
         return null;
     }
 
     /** {@inheritDoc} */
     public void addKeplerContribution(final PositionAngle type, final double gm,
                                       final double[] pDot) {
 
         final PVCoordinates pv = getPVCoordinates();
 
         // position derivative is velocity
         final Vector3D velocity = pv.getVelocity();
         pDot[0] += velocity.getX();
         pDot[1] += velocity.getY();
         pDot[2] += velocity.getZ();
 
         // velocity derivative is Newtonian acceleration
         final Vector3D position = pv.getPosition();
         final double r2         = position.getNormSq();
         final double coeff      = -gm / (r2 * FastMath.sqrt(r2));
         pDot[3] += coeff * position.getX();
         pDot[4] += coeff * position.getY();
         pDot[5] += coeff * position.getZ();
 
     }
 
     /**  Returns a string representation of this Orbit object.
      * @return a string representation of this object
      */
     public String toString() {
         return "cartesian parameters: " + getPVCoordinates().toString();
     }
 
     /** Replace the instance with a data transfer object for serialization.
      * <p>
      * This intermediate class serializes all needed parameters,
      * including position-velocity which are <em>not</em> serialized by parent
      * {@link Orbit} class.
      * </p>
      * @return data transfer object that will be serialized
      */
     private Object writeReplace() {
         return new DTO(this);
     }
 
     /** Internal class used only for serialization. */
     private static class DTO implements Serializable {
 
         /** Serializable UID. */
         private static final long serialVersionUID = 20140723L;
 
         /** Double values. */
         private double[] d;
 
         /** Frame in which are defined the orbital parameters. */
         private final Frame frame;
 
         /** Simple constructor.
          * @param orbit instance to serialize
          */
         private DTO(final CartesianOrbit orbit) {
 
             final TimeStampedPVCoordinates pv = orbit.getPVCoordinates();
 
             // decompose date
             final double epoch  = FastMath.floor(pv.getDate().durationFrom(AbsoluteDate.J2000_EPOCH));
             final double offset = pv.getDate().durationFrom(AbsoluteDate.J2000_EPOCH.shiftedBy(epoch));
 
             this.d = new double[] {
                 epoch, offset, orbit.getMu(),
                 pv.getPosition().getX(),     pv.getPosition().getY(),     pv.getPosition().getZ(),
                 pv.getVelocity().getX(),     pv.getVelocity().getY(),     pv.getVelocity().getZ(),
                 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
             };
 
             this.frame = orbit.getFrame();
 
         }
 
         /** Replace the deserialized data transfer object with a {@link CartesianOrbit}.
          * @return replacement {@link CartesianOrbit}
          */
         private Object readResolve() {
             return new CartesianOrbit(new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH.shiftedBy(d[0]).shiftedBy(d[1]),
                                                                    new Vector3D(d[3], d[ 4], d[ 5]),
                                                                    new Vector3D(d[6], d[ 7], d[ 8]),
                                                                    new Vector3D(d[9], d[10], d[11])),
                                       frame, d[2]);
         }
 
     }
 
 }