/* Copyright 2002-2024 Joseph Reed
    * 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.
    * Joseph Reed 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.utils;
  
  import java.util.Map.Entry;
  import java.util.TreeMap;
  
  import org.hipparchus.geometry.euclidean.threed.Vector3D;
  import org.hipparchus.geometry.spherical.twod.Circle;
  import org.hipparchus.geometry.spherical.twod.S2Point;
  import org.hipparchus.util.FastMath;
  import org.orekit.bodies.GeodeticPoint;
  import org.orekit.bodies.LoxodromeArc;
  import org.orekit.bodies.OneAxisEllipsoid;
  import org.orekit.frames.Frame;
  import org.orekit.frames.TopocentricFrame;
  import org.orekit.time.AbsoluteDate;
  
  /** Builder class, enabling incremental building of an {@link PVCoordinatesProvider}
   * instance using waypoints defined on an ellipsoid.
   *
   * Given a series of waypoints ({@code (date, point)} tuples),
   * build a {@link PVCoordinatesProvider} representing the path.
   * The static methods provide implementations for the most common path definitions
   * (cartesian, great-circle, loxodrome). If these methods are insufficient,
   * the public constructor provides a way to customize the path definition.
   *
   * This class connects the path segments using the {@link AggregatedPVCoordinatesProvider}.
   * As such, no effort is made to smooth the velocity between segments.
   * While position is unaffected, the velocity may be discontinuous between adjacent time points.
   * Thus, care should be taken when modeling paths with abrupt direction changes
   * (e.g. fast-moving aircraft); understand how the {@link PVCoordinatesProvider}
   * will be used in the particular application.
   *
   * @author Joe Reed
   * @since 11.3
   */
  public class WaypointPVBuilder {
  
      /** Factory used to create intermediate pv providers between waypoints. */
      private final InterpolationFactory factory;
  
      /** Central body, on which the waypoints are defined. */
      private final OneAxisEllipsoid body;
  
      /** Set of waypoints, indexed by time. */
      private final TreeMap<AbsoluteDate, GeodeticPoint> waypoints;
  
      /** Whether the resulting provider should be invalid or constant prior to the first waypoint. */
      private boolean invalidBefore;
  
      /** Whether the resulting provider should be invalid or constant after to the last waypoint. */
      private boolean invalidAfter;
  
      /** Create a new instance.
       * @param factory The factory used to create the intermediate coordinate providers between waypoints.
       * @param body The central body, on which the way points are defined.
       */
      public WaypointPVBuilder(final InterpolationFactory factory, final OneAxisEllipsoid body) {
          this.factory       = factory;
          this.body          = body;
          this.waypoints     = new TreeMap<>();
          this.invalidBefore = true;
          this.invalidAfter  = true;
      }
  
      /** Construct a waypoint builder interpolating points using a linear cartesian interpolation.
       *
       * @param body the reference ellipsoid on which the waypoints are defined.
       * @return the waypoint builder
       */
      public static WaypointPVBuilder cartesianBuilder(final OneAxisEllipsoid body) {
          return new WaypointPVBuilder(CartesianWaypointPVProv::new, body);
      }
  
      /** Construct a waypoint builder interpolating points using a loxodrome (or Rhumbline).
       *
       * @param body the reference ellipsoid on which the waypoints are defined.
       * @return the waypoint builder
       */
      public static WaypointPVBuilder loxodromeBuilder(final OneAxisEllipsoid body) {
          return new WaypointPVBuilder(LoxodromeWaypointPVProv::new, body);
      }
  
      /** Construct a waypoint builder interpolating points using a great-circle.
      *
      * The altitude of the intermediate points is linearly interpolated from the bounding waypoints.
      * Extrapolating before the first waypoint or after the last waypoint may result in undefined altitudes.
      *
      * @param body the reference ellipsoid on which the waypoints are defined.
      * @return the waypoint builder
      */
     public static WaypointPVBuilder greatCircleBuilder(final OneAxisEllipsoid body) {
         return new WaypointPVBuilder(GreatCircleWaypointPVProv::new, body);
     }
 
     /** Add a waypoint.
      *
      * @param point the waypoint location
      * @param date the waypoint time
      * @return this instance
      */
     public WaypointPVBuilder addWaypoint(final GeodeticPoint point, final AbsoluteDate date) {
         waypoints.put(date, point);
         return this;
     }
 
     /** Indicate the resulting {@link PVCoordinatesProvider} should be invalid before the first waypoint.
      *
      * @return this instance
      */
     public WaypointPVBuilder invalidBefore() {
         invalidBefore = true;
         return this;
     }
 
     /** Indicate the resulting {@link PVCoordinatesProvider} provide
      * a constant location of the first waypoint prior to the first time.
      *
      * @return this instance
      */
     public WaypointPVBuilder constantBefore() {
         invalidBefore = false;
         return this;
     }
 
     /** Indicate the resulting {@link PVCoordinatesProvider} should be invalid after the last waypoint.
      *
      * @return this instance
      */
     public WaypointPVBuilder invalidAfter() {
         invalidAfter = true;
         return this;
     }
 
     /** Indicate the resulting {@link PVCoordinatesProvider} provide
      * a constant location of the last waypoint after to the last time.
      *
      * @return this instance
      */
     public WaypointPVBuilder constantAfter() {
         invalidAfter = false;
         return this;
     }
 
     /** Build a {@link PVCoordinatesProvider} from the waypoints added to this builder.
      *
      * @return the coordinates provider instance.
      */
     public PVCoordinatesProvider build() {
         final PVCoordinatesProvider initialProvider = createInitial(waypoints.firstKey(),
                                                                     waypoints.firstEntry().getValue());
         final AggregatedPVCoordinatesProvider.Builder builder = new AggregatedPVCoordinatesProvider.Builder(initialProvider);
 
         Entry<AbsoluteDate, GeodeticPoint> previousEntry = null;
         for (final Entry<AbsoluteDate, GeodeticPoint> entry: waypoints.entrySet()) {
             if (previousEntry != null) {
                 builder.addPVProviderAfter(previousEntry.getKey(),
                                            factory.create(previousEntry.getKey(),
                                                           previousEntry.getValue(),
                                                           entry.getKey(),
                                                           entry.getValue(),
                                                           body),
                                            true);
             }
             previousEntry = entry;
         }
         // add the point so we're valid at the final waypoint
         builder.addPVProviderAfter(previousEntry.getKey(),
                                    new ConstantPVCoordinatesProvider(previousEntry.getValue(), body),
                                    true);
         // add the final propagator after the final waypoint
         builder.addPVProviderAfter(previousEntry.getKey().shiftedBy(Double.MIN_VALUE),
                                    createFinal(previousEntry.getKey(), previousEntry.getValue()),
                                    true);
 
         return builder.build();
     }
 
     /** Create the initial provider.
      *
      * This method uses the internal {@code validBefore} flag to
      * either return an invalid PVCoordinatesProvider or a constant one.
      *
      * @param firstDate the date at which the first waypoint is reached
      *                  and this provider will no longer be called
      * @param firstPoint the first waypoint
      * @return the coordinate provider
      */
     protected PVCoordinatesProvider createInitial(final AbsoluteDate firstDate, final GeodeticPoint firstPoint) {
         if (invalidBefore) {
             return new AggregatedPVCoordinatesProvider.InvalidPVProvider();
         }
         else {
             return new ConstantPVCoordinatesProvider(firstPoint, body);
         }
     }
 
     /** Create the final provider.
      *
      * This method uses the internal {@code validAfter} flag to
      * either return an invalid PVCoordinatesProvider or a constant one.
      *
      * @param lastDate the date at which the last waypoint is reached
      *                 and this provider will be called
      * @param lastPoint the last waypoint
      * @return the coordinate provider
      */
     protected PVCoordinatesProvider createFinal(final AbsoluteDate lastDate, final GeodeticPoint lastPoint) {
         if (invalidAfter) {
             return new AggregatedPVCoordinatesProvider.InvalidPVProvider();
         }
         else {
             return new ConstantPVCoordinatesProvider(lastPoint, body);
         }
     }
 
     /**
      * Factory interface, creating the {@link PVCoordinatesProvider} instances between the provided waypoints.
      */
     @FunctionalInterface
     public interface InterpolationFactory {
 
         /** Create a {@link PVCoordinatesProvider} which interpolates between the provided waypoints.
          *
          * @param date1 the first waypoint's date
          * @param point1 the first waypoint's location
          * @param date2 the second waypoint's date
          * @param point2 the second waypoint's location
          * @param body the body on which the waypoints are defined
          * @return a {@link PVCoordinatesProvider} providing the locations at times between the waypoints.
          */
         PVCoordinatesProvider create(AbsoluteDate date1, GeodeticPoint point1,
                                      AbsoluteDate date2, GeodeticPoint point2,
                                      OneAxisEllipsoid body);
     }
 
     /**
      * Coordinate provider interpolating along the great-circle between two points.
      */
     static class GreatCircleWaypointPVProv implements PVCoordinatesProvider {
 
         /** Great circle estimation. */
         private final Circle circle;
         /** Duration between the two points (seconds). */
         private final double duration;
         /** Phase along the circle of the first point. */
         private final double phase0;
         /** Phase length from the first point to the second. */
         private final double phaseLength;
         /** Time at which interpolation results in the initial point. */
         private final AbsoluteDate t0;
         /** Body on which the great circle is defined. */
         private final OneAxisEllipsoid body;
         /** Phase of one second. */
         private double oneSecondPhase;
         /** Altitude of the initial point. */
         private double initialAltitude;
         /** Time-derivative of the altitude. */
         private double altitudeSlope;
 
         /** Class constructor. Aligns to the {@link InterpolationFactory} functional interface.
          *
          * @param date1 the first waypoint's date
          * @param point1 the first waypoint's location
          * @param date2 the second waypoint's date
          * @param point2 the second waypoint's location
          * @param body the body on which the waypoints are defined
          * @see InterpolationFactory
          */
         GreatCircleWaypointPVProv(final AbsoluteDate date1, final GeodeticPoint point1,
                                   final AbsoluteDate date2, final GeodeticPoint point2,
                                   final OneAxisEllipsoid body) {
             this.t0 = date1;
             this.duration = date2.durationFrom(date1);
             this.body = body;
             final S2Point s0 = toSpherical(point1);
             final S2Point s1 = toSpherical(point2);
             circle = new Circle(s0, s1, 1e-9);
 
             phase0 = circle.getPhase(s0.getVector());
             phaseLength = circle.getPhase(s1.getVector()) - phase0;
 
             oneSecondPhase = phaseLength / duration;
             altitudeSlope = (point2.getAltitude() - point1.getAltitude()) / duration;
             initialAltitude = point1.getAltitude();
         }
 
         @Override
         public Vector3D getPosition(final AbsoluteDate date, final Frame frame) {
             final double d = date.durationFrom(t0);
             final double fraction = d / duration;
             final double phase = fraction * phaseLength;
 
             final S2Point sp = new S2Point(circle.getPointAt(phase0 + phase));
             final GeodeticPoint point = toGeodetic(sp, initialAltitude + d * altitudeSlope);
             final Vector3D p = body.transform(point);
 
             return body.getBodyFrame().getStaticTransformTo(frame, date).transformPosition(p);
 
         }
 
         @Override
         public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
             final double d = date.durationFrom(t0);
             final double fraction = d / duration;
             final double phase = fraction * phaseLength;
 
             final S2Point sp = new S2Point(circle.getPointAt(phase0 + phase));
             final GeodeticPoint point = toGeodetic(sp, initialAltitude + d * altitudeSlope);
             final Vector3D p = body.transform(point);
 
             // add 1 second to get another point along the circle, to use for velocity
             final S2Point sp2 = new S2Point(circle.getPointAt(phase0 + phase + oneSecondPhase));
             final GeodeticPoint point2 = toGeodetic(sp2, initialAltitude + (d + 1) * altitudeSlope);
             final Vector3D p2 = body.transform(point2);
             final Vector3D v = p2.subtract(p);
 
             final TimeStampedPVCoordinates tpv = new TimeStampedPVCoordinates(date, p, v);
             return body.getBodyFrame().getTransformTo(frame, date).transformPVCoordinates(tpv);
         }
 
         /** Converts the given geodetic point to a point on the 2-sphere.
          * @param point input geodetic point
          * @return a point on the 2-sphere
          */
         static S2Point toSpherical(final GeodeticPoint point) {
             return new S2Point(point.getLongitude(), 0.5 * FastMath.PI - point.getLatitude());
         }
 
         /** Converts a 2-sphere point to a geodetic point.
          * @param point point on the 2-sphere
          * @param alt point altitude
          * @return a geodetic point
          */
         static GeodeticPoint toGeodetic(final S2Point point, final double alt) {
             return new GeodeticPoint(0.5 * FastMath.PI - point.getPhi(), point.getTheta(), alt);
         }
     }
 
     /**
      * Coordinate provider interpolating along the loxodrome between two points.
      */
     static class LoxodromeWaypointPVProv implements PVCoordinatesProvider {
 
         /** Arc along which the interpolation occurs. */
         private final LoxodromeArc arc;
         /** Time at which the interpolation begins (at arc start). */
         private final AbsoluteDate t0;
         /** Total duration to get the length of the arc (seconds). */
         private final double duration;
         /** Velocity along the arc (m/s). */
         private final double velocity;
 
         /** Class constructor. Aligns to the {@link InterpolationFactory} functional interface.
          *
          * @param date1 the first waypoint's date
          * @param point1 the first waypoint's location
          * @param date2 the second waypoint's date
          * @param point2 the second waypoint's location
          * @param body the body on which the waypoints are defined
          * @see InterpolationFactory
          */
         LoxodromeWaypointPVProv(final AbsoluteDate date1, final GeodeticPoint point1, final AbsoluteDate date2,
                 final GeodeticPoint point2, final OneAxisEllipsoid body) {
             this.arc = new LoxodromeArc(point1, point2, body);
             this.t0 = date1;
             this.duration = date2.durationFrom(date1);
             this.velocity = arc.getDistance() / duration;
         }
 
         @Override
         public Vector3D getPosition(final AbsoluteDate date, final Frame frame) {
             final double fraction = date.durationFrom(t0) / duration;
             final GeodeticPoint point = arc.calculatePointAlongArc(fraction);
             final Vector3D p = arc.getBody().transform(point);
 
             return arc.getBody().getBodyFrame().getStaticTransformTo(frame, date).transformPosition(p);
         }
 
         @Override
         public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
             final double fraction = date.durationFrom(t0) / duration;
             final GeodeticPoint point = arc.calculatePointAlongArc(fraction);
             final Vector3D p = arc.getBody().transform(point);
             final Vector3D vp = arc.getBody().transform(
                     new TopocentricFrame(arc.getBody(), point, "frame")
                         .pointAtDistance(arc.getAzimuth(), 0, velocity));
 
             final TimeStampedPVCoordinates tpv = new TimeStampedPVCoordinates(date, p, vp.subtract(p));
             return arc.getBody().getBodyFrame().getTransformTo(frame, date).transformPVCoordinates(tpv);
         }
     }
 
     /**
      * Coordinate provider interpolating along the cartesian (3-space) line between two points.
      */
     static class CartesianWaypointPVProv implements PVCoordinatesProvider {
 
         /** Date at which the position is valid. */
         private final AbsoluteDate t0;
         /** Initial point. */
         private final Vector3D p0;
         /** Velocity. */
         private final Vector3D vel;
         /** Frame in which the point and velocity are defined. */
         private final Frame sourceFrame;
 
         /** Class constructor. Aligns to the {@link InterpolationFactory} functional interface.
          *
          * @param date1 the first waypoint's date
          * @param point1 the first waypoint's location
          * @param date2 the second waypoint's date
          * @param point2 the second waypoint's location
          * @param body the body on which the waypoints are defined
          * @see InterpolationFactory
          */
         CartesianWaypointPVProv(final AbsoluteDate date1, final GeodeticPoint point1,
                                 final AbsoluteDate date2, final GeodeticPoint point2,
                                 final OneAxisEllipsoid body) {
             this.t0 = date1;
             this.p0 = body.transform(point1);
             this.vel = body.transform(point2).subtract(p0).scalarMultiply(1. / date2.durationFrom(t0));
             this.sourceFrame = body.getBodyFrame();
         }
 
         @Override
         public Vector3D getPosition(final AbsoluteDate date, final Frame frame) {
             final double d = date.durationFrom(t0);
             final Vector3D p = p0.add(vel.scalarMultiply(d));
             return sourceFrame.getStaticTransformTo(frame, date).transformPosition(p);
         }
 
         @Override
         public TimeStampedPVCoordinates getPVCoordinates(final AbsoluteDate date, final Frame frame) {
             final double d = date.durationFrom(t0);
             final Vector3D p = p0.add(vel.scalarMultiply(d));
             final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, p, vel);
             return sourceFrame.getTransformTo(frame, date).transformPVCoordinates(pv);
         }
 
     }
 }