/* Copyright 2013-2019 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.rugged.utils;
  
  import java.io.Serializable;
  import java.util.ArrayList;
  import java.util.List;
  import java.util.stream.Collectors;
  
  import org.hipparchus.util.FastMath;
  import org.orekit.frames.Frame;
  import org.orekit.frames.Transform;
  import org.orekit.rugged.errors.DumpManager;
  import org.orekit.rugged.errors.RuggedException;
  import org.orekit.rugged.errors.RuggedMessages;
  import org.orekit.time.AbsoluteDate;
  import org.orekit.utils.AngularDerivativesFilter;
  import org.orekit.utils.CartesianDerivativesFilter;
  import org.orekit.utils.ImmutableTimeStampedCache;
  import org.orekit.utils.TimeStampedAngularCoordinates;
  import org.orekit.utils.TimeStampedCache;
  import org.orekit.utils.TimeStampedPVCoordinates;
  
  /** Provider for observation transforms.
   * @author Luc Maisonobe
   * @author Guylaine Prat
   */
  public class SpacecraftToObservedBody implements Serializable {
  
      /** Serializable UID. */
      private static final long serialVersionUID = 20140909L;
  
      /** Inertial frame. */
      private final Frame inertialFrame;
  
      /** Body frame. */
      private final Frame bodyFrame;
  
      /** Start of search time span. */
      private final AbsoluteDate minDate;
  
      /** End of search time span. */
      private final AbsoluteDate maxDate;
  
      /** Step to use for inertial frame to body frame transforms cache computations. */
      private final double tStep;
  
      /** Tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting. */
      private final double overshootTolerance;
  
      /** Transforms sample from observed body frame to inertial frame. */
      private final List<Transform> bodyToInertial;
  
      /** Transforms sample from inertial frame to observed body frame. */
      private final List<Transform> inertialToBody;
  
      /** Transforms sample from spacecraft frame to inertial frame. */
      private final List<Transform> scToInertial;
  
      /** Simple constructor.
       * @param inertialFrame inertial frame
       * @param bodyFrame observed body frame
       * @param minDate start of search time span
       * @param maxDate end of search time span
       * @param tStep step to use for inertial frame to body frame transforms cache computations
       * @param overshootTolerance tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting
       * slightly the position, velocity and quaternions ephemerides
       * @param positionsVelocities satellite position and velocity
       * @param pvInterpolationNumber number of points to use for position/velocity interpolation
       * @param pvFilter filter for derivatives from the sample to use in position/velocity interpolation
       * @param quaternions satellite quaternions
       * @param aInterpolationNumber number of points to use for attitude interpolation
       * @param aFilter filter for derivatives from the sample to use in attitude interpolation
       */
      public SpacecraftToObservedBody(final Frame inertialFrame, final Frame bodyFrame,
                                      final AbsoluteDate minDate, final AbsoluteDate maxDate, final double tStep,
                                      final double overshootTolerance,
                                      final List<TimeStampedPVCoordinates> positionsVelocities, final int pvInterpolationNumber,
                                      final CartesianDerivativesFilter pvFilter,
                                      final List<TimeStampedAngularCoordinates> quaternions, final int aInterpolationNumber,
                                      final AngularDerivativesFilter aFilter) {
  
          this.inertialFrame      = inertialFrame;
          this.bodyFrame          = bodyFrame;
          this.minDate            = minDate;
         this.maxDate            = maxDate;
         this.overshootTolerance = overshootTolerance;
 
         // safety checks
         final AbsoluteDate minPVDate = positionsVelocities.get(0).getDate();
         final AbsoluteDate maxPVDate = positionsVelocities.get(positionsVelocities.size() - 1).getDate();
         if (minPVDate.durationFrom(minDate) > overshootTolerance) {
             throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, minDate, minPVDate, maxPVDate);
         }
         if (maxDate.durationFrom(maxPVDate) > overshootTolerance) {
             throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, maxDate, minPVDate, maxPVDate);
         }
 
         final AbsoluteDate minQDate  = quaternions.get(0).getDate();
         final AbsoluteDate maxQDate  = quaternions.get(quaternions.size() - 1).getDate();
         if (minQDate.durationFrom(minDate) > overshootTolerance) {
             throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, minDate, minQDate, maxQDate);
         }
         if (maxDate.durationFrom(maxQDate) > overshootTolerance) {
             throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, maxDate, minQDate, maxQDate);
         }
 
         // set up the cache for position-velocities
         final TimeStampedCache<TimeStampedPVCoordinates> pvCache =
                 new ImmutableTimeStampedCache<TimeStampedPVCoordinates>(pvInterpolationNumber, positionsVelocities);
 
         // set up the cache for attitudes
         final TimeStampedCache<TimeStampedAngularCoordinates> aCache =
                 new ImmutableTimeStampedCache<TimeStampedAngularCoordinates>(aInterpolationNumber, quaternions);
 
         final int n = (int) FastMath.ceil(maxDate.durationFrom(minDate) / tStep);
         this.tStep          = tStep;
         this.bodyToInertial = new ArrayList<Transform>(n);
         this.inertialToBody = new ArrayList<Transform>(n);
         this.scToInertial   = new ArrayList<Transform>(n);
         for (AbsoluteDate date = minDate; bodyToInertial.size() < n; date = date.shiftedBy(tStep)) {
 
             // interpolate position-velocity, allowing slight extrapolation near the boundaries
             final AbsoluteDate pvInterpolationDate;
             if (date.compareTo(pvCache.getEarliest().getDate()) < 0) {
                 pvInterpolationDate = pvCache.getEarliest().getDate();
             } else if (date.compareTo(pvCache.getLatest().getDate()) > 0) {
                 pvInterpolationDate = pvCache.getLatest().getDate();
             } else {
                 pvInterpolationDate = date;
             }
             final TimeStampedPVCoordinates interpolatedPV =
                     TimeStampedPVCoordinates.interpolate(pvInterpolationDate, pvFilter,
                             pvCache.getNeighbors(pvInterpolationDate));
             final TimeStampedPVCoordinates pv = interpolatedPV.shiftedBy(date.durationFrom(pvInterpolationDate));
 
             // interpolate attitude, allowing slight extrapolation near the boundaries
             final AbsoluteDate aInterpolationDate;
             if (date.compareTo(aCache.getEarliest().getDate()) < 0) {
                 aInterpolationDate = aCache.getEarliest().getDate();
             } else if (date.compareTo(aCache.getLatest().getDate()) > 0) {
                 aInterpolationDate = aCache.getLatest().getDate();
             } else {
                 aInterpolationDate = date;
             }
             final TimeStampedAngularCoordinates interpolatedQuaternion =
                     TimeStampedAngularCoordinates.interpolate(aInterpolationDate, aFilter,
                             aCache.getNeighbors(aInterpolationDate).collect(Collectors.toList()));
             final TimeStampedAngularCoordinates quaternion = interpolatedQuaternion.shiftedBy(date.durationFrom(aInterpolationDate));
 
             // store transform from spacecraft frame to inertial frame
             scToInertial.add(new Transform(date,
                     new Transform(date, quaternion.revert()),
                     new Transform(date, pv)));
 
             // store transform from body frame to inertial frame
             final Transform b2i = bodyFrame.getTransformTo(inertialFrame, date);
             bodyToInertial.add(b2i);
             inertialToBody.add(b2i.getInverse());
 
         }
     }
 
     /** Simple constructor.
      * @param inertialFrame inertial frame
      * @param bodyFrame observed body frame
      * @param minDate start of search time span
      * @param maxDate end of search time span
      * @param tStep step to use for inertial frame to body frame transforms cache computations
      * @param overshootTolerance tolerance in seconds allowed for {@code minDate} and {@code maxDate} overshooting
      * slightly the position, velocity and quaternions ephemerides
      * @param bodyToInertial transforms sample from observed body frame to inertial frame
      * @param scToInertial transforms sample from spacecraft frame to inertial frame
      */
     public SpacecraftToObservedBody(final Frame inertialFrame, final Frame bodyFrame,
                                     final AbsoluteDate minDate, final AbsoluteDate maxDate, final double tStep,
                                     final double overshootTolerance,
                                     final List<Transform> bodyToInertial, final List<Transform> scToInertial) {
 
         this.inertialFrame      = inertialFrame;
         this.bodyFrame          = bodyFrame;
         this.minDate            = minDate;
         this.maxDate            = maxDate;
         this.tStep              = tStep;
         this.overshootTolerance = overshootTolerance;
         this.bodyToInertial     = bodyToInertial;
         this.scToInertial       = scToInertial;
 
         this.inertialToBody = new ArrayList<Transform>(bodyToInertial.size());
         for (final Transform b2i : bodyToInertial) {
             inertialToBody.add(b2i.getInverse());
         }
 
     }
 
     /** Get the inertial frame.
      * @return inertial frame
      */
     public Frame getInertialFrame() {
         return inertialFrame;
     }
 
     /** Get the body frame.
      * @return body frame
      */
     public Frame getBodyFrame() {
         return bodyFrame;
     }
 
     /** Get the start of search time span.
      * @return start of search time span
      */
     public AbsoluteDate getMinDate() {
         return minDate;
     }
 
     /** Get the end of search time span.
      * @return end of search time span
      */
     public AbsoluteDate getMaxDate() {
         return maxDate;
     }
 
     /** Get the step to use for inertial frame to body frame transforms cache computations.
      * @return step to use for inertial frame to body frame transforms cache computations
      */
     public double getTStep() {
         return tStep;
     }
 
     /** Get the tolerance in seconds allowed for {@link #getMinDate()} and {@link #getMaxDate()} overshooting.
      * @return tolerance in seconds allowed for {@link #getMinDate()} and {@link #getMaxDate()} overshooting
      */
     public double getOvershootTolerance() {
         return overshootTolerance;
     }
 
     /** Get transform from spacecraft to inertial frame.
      * @param date date of the transform
      * @return transform from spacecraft to inertial frame
      */
     public Transform getScToInertial(final AbsoluteDate date) {
         return interpolate(date, scToInertial);
     }
 
     /** Get transform from inertial frame to observed body frame.
      * @param date date of the transform
      * @return transform from inertial frame to observed body frame
      */
     public Transform getInertialToBody(final AbsoluteDate date) {
         return interpolate(date, inertialToBody);
     }
 
     /** Get transform from observed body frame to inertial frame.
      * @param date date of the transform
      * @return transform from observed body frame to inertial frame
      */
     public Transform getBodyToInertial(final AbsoluteDate date) {
         return interpolate(date, bodyToInertial);
     }
 
     /** Interpolate transform.
      * @param date date of the transform
      * @param list transforms list to interpolate from
      * @return interpolated transform
      */
     private Transform interpolate(final AbsoluteDate date, final List<Transform> list) {
 
         // check date range
         if (!isInRange(date)) {
             throw new RuggedException(RuggedMessages.OUT_OF_TIME_RANGE, date, minDate, maxDate);
         }
 
         final double    s     = date.durationFrom(list.get(0).getDate()) / tStep;
         final int       index = FastMath.max(0, FastMath.min(list.size() - 1, (int) FastMath.rint(s)));
 
         DumpManager.dumpTransform(this, index, bodyToInertial.get(index), scToInertial.get(index));
 
         final Transform close = list.get(index);
         return close.shiftedBy(date.durationFrom(close.getDate()));
 
     }
 
     /** Check if a date is in the supported range.
      * @param date date to check
      * @return true if date is in the supported range
      */
     public boolean isInRange(final AbsoluteDate date) {
         return (minDate.durationFrom(date) <= overshootTolerance) &&
                (date.durationFrom(maxDate) <= overshootTolerance);
     }
 
 }