/* Copyright 2022-2026 Romain Serra
    * 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.orbits;
  
  import org.hipparchus.geometry.euclidean.threed.Vector3D;
  import org.hipparchus.util.FastMath;
  import org.hipparchus.util.SinCos;
  import org.orekit.errors.OrekitIllegalArgumentException;
  import org.orekit.errors.OrekitMessages;
  import org.orekit.utils.PVCoordinates;
  
  /**
   * Class for converting between equinoctial elements and Cartesian coordinates.
   * @author Romain Serra
   * @see EquinoctialParameters
   * @since 14.0
   */
  public class EquinoctialParametersConverter {
  
      /** Central body gravitational parameter. */
      private final double mu;
  
      /**
       * Constructor.
       * @param mu central body gravitational parameter
       */
      public EquinoctialParametersConverter(final double mu) {
          this.mu = mu;
      }
  
      /**
       * Convert Cartesian coordinates to equinoctial elements.
       * @param cartesian position and velocity in inertial frame
       * @param positionAngleType type of position angle to use
       * @return equinoctial elements
       */
      public EquinoctialParameters toParameters(final PVCoordinates cartesian, final PositionAngleType positionAngleType) {
          //  compute semi-major axis
          final Vector3D pvP   = cartesian.getPosition();
          final Vector3D pvV   = cartesian.getVelocity();
          final double r2      = pvP.getNorm2Sq();
          final double r       = FastMath.sqrt(r2);
          final double V2      = pvV.getNorm2Sq();
          final double rV2OnMu = r * V2 / mu;
  
          // compute semi-major axis
          final double a = r / (2 - rV2OnMu);
  
          if (a <= 0.) {
              throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
                      getClass().getName());
          }
  
          // compute inclination vector
          final Vector3D w = cartesian.getMomentum().normalize();
          final double d = 1.0 / (1 + w.getZ());
          final double hx = -d * w.getY();
          final double hy =  d * w.getX();
  
          // compute true longitude argument
          final double cLv = (pvP.getX() - d * pvP.getZ() * w.getX()) / r;
          final double sLv = (pvP.getY() - d * pvP.getZ() * w.getY()) / r;
          final double lV = FastMath.atan2(sLv, cLv);
  
          // compute eccentricity vector
          final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(mu * a);
          final double eCE = rV2OnMu - 1;
          final double e2  = eCE * eCE + eSE * eSE;
          final double f   = eCE - e2;
          final double g   = FastMath.sqrt(1 - e2) * eSE;
          final double ex = a * (f * cLv + g * sLv) / r;
          final double ey = a * (f * sLv - g * cLv) / r;
  
          final EquinoctialParameters elements = new EquinoctialParameters(a, ex, ey, hx, hy, lV, PositionAngleType.TRUE);
          return positionAngleType == PositionAngleType.TRUE ? elements : elements.withPositionAngleType(positionAngleType);
      }
  
      /**
       * Convert equinoctial elements to Cartesian coordinates.
       * @param elements equinoctial elements
       * @return position and velocity in inertial frame
       */
      public PVCoordinates toCartesian(final EquinoctialParameters elements) {
          // get equinoctial parameters
          final double a = elements.a();
         final double ex = elements.ex();
         final double ey = elements.ey();
         final double hx = elements.hx();
         final double hy = elements.hy();
         final double lE = elements.positionAngleType() == PositionAngleType.ECCENTRIC ? elements.longitudeArgument() :
                 elements.withPositionAngleType(PositionAngleType.ECCENTRIC).longitudeArgument();
 
         // inclination-related intermediate parameters
         final double hx2   = hx * hx;
         final double hy2   = hy * hy;
         final double factH = 1. / (1 + hx2 + hy2);
 
         // reference axes defining the orbital plane
         final double ux = (1 + hx2 - hy2) * factH;
         final double uy =  2 * hx * hy * factH;
         final double uz = -2 * hy * factH;
 
         final double vx = uy;
         final double vy = (1 - hx2 + hy2) * factH;
         final double vz =  2 * hx * factH;
 
         // eccentricity-related intermediate parameters
         final double exey = ex * ey;
         final double ex2  = ex * ex;
         final double ey2  = ey * ey;
         final double e2   = ex2 + ey2;
         final double eta  = 1 + FastMath.sqrt(1 - e2);
         final double beta = 1. / eta;
 
         // eccentric longitude argument
         final SinCos scLe   = FastMath.sinCos(lE);
         final double cLe    = scLe.cos();
         final double sLe    = scLe.sin();
         final double exCeyS = ex * cLe + ey * sLe;
 
         // coordinates of position and velocity in the orbital plane
         final double x      = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - ex);
         final double y      = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - ey);
 
         final double factor = FastMath.sqrt(mu / a) / (1 - exCeyS);
         final double xdot   = factor * (-sLe + beta * ey * exCeyS);
         final double ydot   = factor * ( cLe - beta * ex * exCeyS);
 
         final Vector3D position =
                 new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
         final Vector3D velocity =
                 new Vector3D(xdot * ux + ydot * vx, xdot * uy + ydot * vy, xdot * uz + ydot * vz);
         return new PVCoordinates(position, velocity);
     }
 }