/* Copyright 2022-2025 Luc Maisonobe
    * 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.bodies;
  
  import org.hipparchus.CalculusFieldElement;
  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
  import org.hipparchus.geometry.euclidean.twod.FieldVector2D;
  import org.hipparchus.util.FastMath;
  import org.hipparchus.util.FieldSinCos;
  import org.orekit.frames.Frame;
  import org.orekit.utils.TimeStampedFieldPVCoordinates;
  
  /**
   * Model of a 2D ellipse in 3D space.
   * <p>
   * These ellipses are mainly created as plane sections of general 3D ellipsoids,
   * but can be used for other purposes.
   * </p>
   * <p>
   * Instances of this class are guaranteed to be immutable.
   * </p>
   * @see Ellipsoid#getPlaneSection(FieldVector3D, FieldVector3D)
   * @param <T> the type of the field elements
   * @since 12.0
   * @author Luc Maisonobe
   */
  public class FieldEllipse<T extends CalculusFieldElement<T>> {
  
      /** Convergence limit. */
      private static final double ANGULAR_THRESHOLD = 1.0e-12;
  
      /** Center of the 2D ellipse. */
      private final FieldVector3D<T> center;
  
      /** Unit vector along the major axis. */
      private final FieldVector3D<T> u;
  
      /** Unit vector along the minor axis. */
      private final FieldVector3D<T> v;
  
      /** Semi major axis. */
      private final T a;
  
      /** Semi minor axis. */
      private final T b;
  
      /** Frame in which the ellipse is defined. */
      private final Frame frame;
  
      /** Semi major axis radius power 2. */
      private final T a2;
  
      /** Semi minor axis power 2. */
      private final T b2;
  
      /** Eccentricity power 2. */
      private final T e2;
  
      /** 1 minus flatness. */
      private final T g;
  
      /** g * g. */
      private final T g2;
  
      /** Evolute factor along major axis. */
      private final T evoluteFactorX;
  
      /** Evolute factor along minor axis. */
      private final T evoluteFactorY;
  
      /** Simple constructor.
       * @param center center of the 2D ellipse
       * @param u unit vector along the major axis
       * @param v unit vector along the minor axis
       * @param a semi major axis
       * @param b semi minor axis
       * @param frame frame in which the ellipse is defined
       */
      public FieldEllipse(final FieldVector3D<T> center, final FieldVector3D<T> u,
                          final FieldVector3D<T> v, final T a, final T b,
                          final Frame frame) {
          this.center = center;
          this.u      = u;
          this.v      = v;
          this.a      = a;
         this.b      = b;
         this.frame  = frame;
         this.a2     = a.square();
         this.g      = b.divide(a);
         this.g2     = g.multiply(g);
         this.e2     = g2.negate().add(1);
         this.b2     = b.square();
         this.evoluteFactorX = a2.subtract(b2).divide(a2.square());
         this.evoluteFactorY = b2.subtract(a2).divide(b2.square());
     }
 
     /** Get the center of the 2D ellipse.
      * @return center of the 2D ellipse
      */
     public FieldVector3D<T> getCenter() {
         return center;
     }
 
     /** Get the unit vector along the major axis.
      * @return unit vector along the major axis
      */
     public FieldVector3D<T> getU() {
         return u;
     }
 
     /** Get the unit vector along the minor axis.
      * @return unit vector along the minor axis
      */
     public FieldVector3D<T> getV() {
         return v;
     }
 
     /** Get the semi major axis.
      * @return semi major axis
      */
     public T getA() {
         return a;
     }
 
     /** Get the semi minor axis.
      * @return semi minor axis
      */
     public T getB() {
         return b;
     }
 
     /** Get the defining frame.
      * @return defining frame
      */
     public Frame getFrame() {
         return frame;
     }
 
     /** Get a point of the 2D ellipse.
      * @param theta angular parameter on the ellipse (really the eccentric anomaly)
      * @return ellipse point at theta, in underlying ellipsoid frame
      */
     public FieldVector3D<T> pointAt(final T theta) {
         final FieldSinCos<T> scTheta = FastMath.sinCos(theta);
         return toSpace(new FieldVector2D<>(a.multiply(scTheta.cos()), b.multiply(scTheta.sin())));
     }
 
     /** Create a point from its ellipse-relative coordinates.
      * @param p point defined with respect to ellipse
      * @return point defined with respect to 3D frame
      * @see #toPlane(FieldVector3D)
      */
     public FieldVector3D<T> toSpace(final FieldVector2D<T> p) {
         return new FieldVector3D<>(a.getField().getOne(), center, p.getX(), u, p.getY(), v);
     }
 
     /** Project a point to the ellipse plane.
      * @param p point defined with respect to 3D frame
      * @return point defined with respect to ellipse
      * @see #toSpace(FieldVector2D)
      */
     public FieldVector2D<T> toPlane(final FieldVector3D<T> p) {
         final FieldVector3D<T> delta = p.subtract(center);
         return new FieldVector2D<>(FieldVector3D.dotProduct(delta, u), FieldVector3D.dotProduct(delta, v));
     }
 
     /** Find the closest ellipse point.
      * @param p point in the ellipse plane to project on the ellipse itself
      * @return closest point belonging to 2D meridian ellipse
      */
     public FieldVector2D<T> projectToEllipse(final FieldVector2D<T> p) {
 
         final T x = FastMath.abs(p.getX());
         final T y = p.getY();
 
         if (x.getReal() <= ANGULAR_THRESHOLD * FastMath.abs(y.getReal())) {
             // the point is almost on the minor axis, approximate the ellipse with
             // the osculating circle whose center is at evolute cusp along minor axis
             final T osculatingRadius = a2.divide(b);
             final T evoluteCuspZ     = FastMath.copySign(a.multiply(e2).divide(g), y.negate());
             final T deltaZ           = y.subtract(evoluteCuspZ);
             final T ratio            = osculatingRadius.divide(FastMath.hypot(deltaZ, x));
             return new FieldVector2D<>(FastMath.copySign(ratio.multiply(x), p.getX()),
                                        evoluteCuspZ.add(ratio.multiply(deltaZ)));
         }
 
         if (FastMath.abs(y.getReal()) <= ANGULAR_THRESHOLD * x.getReal()) {
             // the point is almost on the major axis
 
             final T osculatingRadius = b2.divide(a);
             final T evoluteCuspR     = a.multiply(e2);
             final T deltaR           = x.subtract(evoluteCuspR);
             if (deltaR.getReal() >= 0) {
                 // the point is outside of the ellipse evolute, approximate the ellipse
                 // with the osculating circle whose center is at evolute cusp along major axis
                 final T ratio = osculatingRadius.divide(FastMath.hypot(y, deltaR));
                 return new FieldVector2D<>(FastMath.copySign(evoluteCuspR.add(ratio.multiply(deltaR)), p.getX()),
                                            ratio.multiply(y));
             }
 
             // the point is on the part of the major axis within ellipse evolute
             // we can compute the closest ellipse point analytically
             final T rEllipse = x.divide(e2);
             return new FieldVector2D<>(FastMath.copySign(rEllipse, p.getX()),
                                        FastMath.copySign(g.multiply(FastMath.sqrt(a2.subtract(rEllipse.multiply(rEllipse)))), y));
 
         } else {
 
             // initial point at evolute cusp along major axis
             T omegaX = a.multiply(e2);
             T omegaY = a.getField().getZero();
 
             T projectedX  = x;
             T projectedY  = y;
             double deltaX = Double.POSITIVE_INFINITY;
             double deltaY = Double.POSITIVE_INFINITY;
             int count = 0;
             final double threshold = ANGULAR_THRESHOLD * ANGULAR_THRESHOLD * a2.getReal();
             while ((deltaX * deltaX + deltaY * deltaY) > threshold && count++ < 100) { // this loop usually converges in 3 iterations
 
                 // find point at the intersection of ellipse and line going from query point to evolute point
                 final T dx         = x.subtract(omegaX);
                 final T dy         = y.subtract(omegaY);
                 final T alpha      = b2.multiply(dx).multiply(dx).add(a2.multiply(dy).multiply(dy));
                 final T betaPrime  = b2.multiply(omegaX).multiply(dx).add(a2.multiply(omegaY).multiply(dy));
                 final T gamma      = b2.multiply(omegaX).multiply(omegaX).add(a2.multiply(omegaY).multiply(omegaY)).subtract(a2.multiply(b2));
                 final T deltaPrime = a.linearCombination(betaPrime, betaPrime, alpha.negate(), gamma);
                 final T ratio      = (betaPrime.getReal() <= 0) ?
                                           FastMath.sqrt(deltaPrime).subtract(betaPrime).divide(alpha) :
                                           gamma.negate().divide(FastMath.sqrt(deltaPrime).add(betaPrime));
                 final T previousX  = projectedX;
                 final T previousY  = projectedY;
                 projectedX = omegaX.add(ratio.multiply(dx));
                 projectedY = omegaY.add(ratio.multiply(dy));
 
                 // find new evolute point
                 omegaX     = evoluteFactorX.multiply(projectedX).multiply(projectedX).multiply(projectedX);
                 omegaY     = evoluteFactorY.multiply(projectedY).multiply(projectedY).multiply(projectedY);
 
                 // compute convergence parameters
                 deltaX     = projectedX.subtract(previousX).getReal();
                 deltaY     = projectedY.subtract(previousY).getReal();
 
             }
             return new FieldVector2D<>(FastMath.copySign(projectedX, p.getX()), projectedY);
         }
     }
 
     /** Project position-velocity-acceleration on an ellipse.
      * @param pv position-velocity-acceleration to project, in the reference frame
      * @return projected position-velocity-acceleration
      */
     public TimeStampedFieldPVCoordinates<T> projectToEllipse(final TimeStampedFieldPVCoordinates<T> pv) {
 
         // find the closest point in the meridian plane
         final FieldVector2D<T>p2D = toPlane(pv.getPosition());
         final FieldVector2D<T>e2D = projectToEllipse(p2D);
 
         // tangent to the ellipse
         final T fx = a2.negate().multiply(e2D.getY());
         final T fy = b2.multiply(e2D.getX());
         final T f2 = fx.square().add(fy.square());
         final T f  = FastMath.sqrt(f2);
         final FieldVector2D<T>tangent = new FieldVector2D<>(fx.divide(f), fy.divide(f));
 
         // normal to the ellipse (towards interior)
         final FieldVector2D<T>normal = new FieldVector2D<>(tangent.getY().negate(), tangent.getX());
 
         // center of curvature
         final T x2     = e2D.getX().multiply(e2D.getX());
         final T y2     = e2D.getY().multiply(e2D.getY());
         final T eX     = evoluteFactorX.multiply(x2);
         final T eY     = evoluteFactorY.multiply(y2);
         final T omegaX = eX.multiply(e2D.getX());
         final T omegaY = eY.multiply(e2D.getY());
 
         // velocity projection ratio
         final T rho                = FastMath.hypot(e2D.getX().subtract(omegaX), e2D.getY().subtract(omegaY));
         final T d                  = FastMath.hypot(p2D.getX().subtract(omegaX), p2D.getY().subtract(omegaY));
         final T projectionRatio    = rho.divide(d);
 
         // tangential velocity
         final FieldVector2D<T>pDot2D     = new FieldVector2D<>(FieldVector3D.dotProduct(pv.getVelocity(), u),
                                                               FieldVector3D.dotProduct(pv.getVelocity(), v));
         final T   pDotTangent            = pDot2D.dotProduct(tangent);
         final T   pDotNormal             = pDot2D.dotProduct(normal);
         final T   eDotTangent            = projectionRatio.multiply(pDotTangent);
         final FieldVector2D<T>eDot2D     = new FieldVector2D<>(eDotTangent, tangent);
         final FieldVector2D<T>tangentDot = new FieldVector2D<>(a2.multiply(b2).
                                                                multiply(e2D.getX().multiply(eDot2D.getY()).
                                                                         subtract(e2D.getY().multiply(eDot2D.getX()))).
                                                                divide(f2),
                                                                normal);
 
         // velocity of the center of curvature in the meridian plane
         final T omegaXDot          = eX.multiply(eDotTangent).multiply(tangent.getX()).multiply(3);
         final T omegaYDot          = eY.multiply(eDotTangent).multiply(tangent.getY()).multiply(3);
 
         // derivative of the projection ratio
         final T voz                = omegaXDot.multiply(tangent.getY()).subtract(omegaYDot.multiply(tangent.getX()));
         final T vsz                = pDotNormal.negate();
         final T projectionRatioDot = rho.subtract(d).multiply(voz).subtract(rho.multiply(vsz)).divide(d.multiply(d));
 
         // acceleration
         final FieldVector2D<T>pDotDot2D  = new FieldVector2D<>(FieldVector3D.dotProduct(pv.getAcceleration(), u),
                                                                FieldVector3D.dotProduct(pv.getAcceleration(), v));
         final T   pDotDotTangent         = pDotDot2D.dotProduct(tangent);
         final T   pDotTangentDot         = pDot2D.dotProduct(tangentDot);
         final T   eDotDotTangent         = projectionRatio.multiply(pDotDotTangent.add(pDotTangentDot)).
                                            add(projectionRatioDot.multiply(pDotTangent));
         final FieldVector2D<T>eDotDot2D = new FieldVector2D<>(eDotDotTangent, tangent, eDotTangent, tangentDot);
 
         // back to 3D
         final FieldVector3D<T> e3D       = toSpace(e2D);
         final FieldVector3D<T> eDot3D    = new FieldVector3D<>(eDot2D.getX(), u, eDot2D.getY(), v);
         final FieldVector3D<T> eDotDot3D = new FieldVector3D<>(eDotDot2D.getX(), u, eDotDot2D.getY(), v);
 
         return new TimeStampedFieldPVCoordinates<>(pv.getDate(), e3D, eDot3D, eDotDot3D);
 
     }
 
     /** Find the center of curvature (point on the evolute) at the nadir of a point.
      * @param point point in the ellipse plane
      * @return center of curvature of the ellipse directly at point nadir
      */
     public FieldVector2D<T> getCenterOfCurvature(final FieldVector2D<T> point) {
         final FieldVector2D<T>projected = projectToEllipse(point);
         return new FieldVector2D<>(evoluteFactorX.multiply(projected.getX()).multiply(projected.getX()).multiply(projected.getX()),
                                    evoluteFactorY.multiply(projected.getY()).multiply(projected.getY()).multiply(projected.getY()));
     }
 
 }