package nxm.sys.lib;

import static java.lang.Math.asin;
import static java.lang.Math.atan2;
import static java.lang.Math.max;
import static java.lang.Math.min;
import static java.lang.Math.rint;
import static java.lang.Math.sqrt;
import static nxm.sys.lib.FileUtil.recDef;
import static nxm.sys.lib.Time.roundToMicro;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.LinkedHashMap;
import java.util.List;
import java.util.Map;

import nxm.sys.inc.Constants;
import nxm.sys.inc.DataTypes;
import nxm.sys.inc.Keyable;
import nxm.sys.inc.ListFile;
import nxm.sys.inc.MidasReference;
import nxm.sys.inc.PlotFile;
import nxm.sys.inc.ProvisionalUseOnly;
import nxm.sys.inc.Units;

/** A class to define a state vector. A state vector can take two forms: (a) a
    single instance in time, or (b) a set of instances in time where the 'time'
    (or 'timecode') can be set anywhere between thereby updating the instantaneous
    state values. <br>
    <br>
    Prior to NeXtMidas 3.3.0, the only way to use option (b) was to provide a
    Midas BLUE Type 5000 file with applicable state-vector fields. The format
    required was very specific any variation from what was expected would
    produce undefined results (or errors). <br>
    <br>
    Since NeXtMidas 3.3.0, this also supports a variety of additional sets of
    state vector data. Including:
    <pre>
      mode=null                                 (same as pre-3.3.0 with no file)
        Instantaneous data describing a single point in time.

        <i>Note that using {@link #setFile(Object,Object,int,SVMode)} with
        mode=null will cause StateVector to attempt to auto-identify the mode.
        It will do this by checking each of the below options, in the order
        listed here and set the mode to match the first applicable.</i>

      mode=T5Data                                (same as pre-3.3.0 with a file)
        Type 5000 state vector with applicable transform, data in ECR, ECI,
        TOP or TOPOCENT as specified in the QUADWORDS of the Type 5000 file.
        Does not support use of a TIME column in the file (the pre-3.3.0 version
        would read in files with a TIME column, but ignored the TIME values
        resulting in some files being processed incorrectly).

      mode=T5Entries
        Same as mode=Entries (option #1) but with data in ECR, ECI, TOP or
        TOPOCENT as specified in the QUADWORDS of the Type 5000 file.

        The key differences between T5Data and T5Entries are:
          1. T5Entries will permit a TIME column to override the computed
             TIME values while T5Data will always compute the TIME
             values based on the header start and delta values.
          2. T5Entries is less rigid with the data format used, and
          3. Internal handling of the data from the file differs (required by #2).

      mode=Entries                                  (see {@link #toFileEntries})
        Either:
          1. Entries from any ListFile file with columns:
               TIME=     - Time of entry (J1950 time)
               POS=      - Position
               VEL=      - Velocity     (optional)
               ACC=      - Acceleration (optional, requires VEL=)
             If TIME= is not specified, and the file implements PlotFile, the
             TIME= entry will be set using .getTimeAt(..). All values must be
             in Cartesian ECR form (SI units).

          2. Tabular entries with:
               TIME=     - Time of entry (J1950 time)
               POS=      - Position
               VEL=      - Velocity     (optional)
               ACC=      - Acceleration (optional, requires VEL=)
             When given in this form, POS/VEL/ACC can either be:
               a. A Data object with (x,y,z)
               b. A double[] with {x,y,z}
               c. A Table with {X=x,Y=y,Z=z}
               d. A Table with {LON=x,LAT=y,ALT=z}

             For a,b,c the values must be in Cartesian ECR (SI units) form, for
             d the values must be in geodetic form.

      mode=KeywordsEph                              (see {@link #toKeywordsEph})
        Ephemeral keywords read from a file. The keywords must contain (n=1..N):
          ETIMn=    - Time of entry (J1950 time)
          POSn=     - Position
          VELn=     - Velocity     (optional)
          ACCn=     - Acceleration (optional, requires VELn=)
        Note that the time entry is spelled "ETIM" and not "TIME" as this matches
        historic X-Midas usage and is short for "Ephemeral TIMe." Similarly the
        indexing is 1..N and not 0..N-1. All values must be in Cartesian ECR (SI
        units).

      mode=KeywordsVec                              (see {@link #toKeywordsVec})
        Vectored keywords read from a file. The keywords must contain:
          ETIM_VEC=                     - Array of time of entries (J1950 time)
          POSX_VEC=/POSY_VEC=/POSZ_VEC= - Arrays position (X,Y,Z) values
          VELX_VEC=/VELY_VEC=/VELZ_VEC= - Arrays velocity (X,Y,Z) values
          ACCX_VEC=/ACCY_VEC=/ACCZ_VEC= - Arrays acceleration (X,Y,Z) values

      mode=KeywordsPlat                            (see {@link #toKeywordsPlat})
        Ephemeral keywords read from a Platinum file. This will be a state
        vector with:
          prefix.TIME=           - Array of time of entries
          prefix_POS.X=/.Y=/.Z=  - Arrays position (X,Y,Z) values
          prefix_VEL.X=/.Y=/.Z=  - Arrays velocity (X,Y,Z) values
          prefix_ACC.X=/.Y=/.Z=  - Arrays acceleration (X,Y,Z) values
        Where 'prefix' is the applicable prefix ("EPH" being the default).

        Per the Platinum specification the units used must be {@link Units#COUNTS},
        {@link Units#DISTANCE_M}, {@link Units#SPEED_M_S}, and {@link Units#ACCELERATION_M_S2}
        respectively (as such the supplemental .UNIT(S) keyword may be omitted). As
        an exception, NeXtMidas permits files to be read in with the .TIME in
        {@link Units#TIME_S} (per Vol. 1 of the Platinum ICD) even though that is not
        strictly legal under Vol. 2 of the Platinum ICD <i>(a warning will be
        issued in this case)</i>. Opening the file with the {@link DataFile#NOABORT}
        flag set will turn any units errors into warnings and will then proceed
        to interpret the values assuming the units specified in Vol. 2 of the
        Platinum ICD. <i>[There are a few buggy systems that are known to write
        Platinum files with bogus units attached to the time-valued entries,
        this special behavior is explicitly designed to provide a mechanism
        for reading those files.]</i>

      mode=KeywordsLLA                              (see {@link #toKeywordsLLA})
        LON/LAT/ALT keywords read from a Platinum file. This will be a single
        instantaneous point (usually for a fixed location) described using:
          prefix.LON=    - Longitude of location (deg)
          prefix.LAT=    - Latitude  of location (deg)
          prefix.ALT=    - Altitude  of location (meters)
        Where 'prefix' is the applicable prefix (unlike KeywordsPlat this does
        NOT default to "EPH").

        Per the Platinum specification the units used must be {@link Units#ANGLE_DEG},
        {@link Units#ANGLE_DEG}, and {@link Units#DISTANCE_M}. As an exception,
        NeXtMidas permits files to be read in with {@link Units#LONGITUDE_DEG},
        {@link Units#LATITUDE_DEG}, and {@link Units#ALTITUDE_M} as these are
        in common usage within Type 5000 files produced following the BLUE 1.1
        ICD <i>(a warning will be issued in this case)</i>.

      Note that although KeywordsPlat and KeywordsLLA follow keywords specified
      in the Platinum standard, they can be used with any BLUE file. It is
      common to encounter this in situations where the Platinum file keywords
      have been propagated to a BLUE file within a Midas processor (e.g. as
      part of a sub-band tuning operation).

      For more details about the Platinum formats (KeywordsPlat and KeywordsLLA)
      please see sections 2.4.1 and 2.4.1 of the "Platinum BLUE Data Exchange
      Format Statement (Revision 4) / Volume 2: Standard Platinum Keywords."
    </pre>
    @author Jeff Schoen / J. Czechowski IV
 */
public class StateVector extends Position implements Constants, Keyable {
  /** The interpolation order to use with {@link #interpolate}. Options include:
      Linear, Cubic Hermite Spline, and Quintic Hermite Spline.
      @since NeXtMidas 3.3.0
   */
  public static enum InterpolationOrder {
      /** Linear interpolation order */
      Linear,
      /** Cubic Hermite Spline interpolation order */
      CubicHermite,
      /** Quintic Hermite Spline interpolation order */
      QuinticHermite
    };

  private static final double SPEED_OF_LIGHT_INV = 1.0/SPEED_OF_LIGHT; // used by computeMeasurements(..)
  private static final double FOUR_PI_INV        = 1.0/(4*PI);         // used by computeMeasurements(..)

  /** Velocity (meters/second). These are only public for use by LayerGeo (they should be protected), use public gets/sets. */
  public double vx,vy,vz;

  /** Velocity (meters/second) in north, east, up directions (geodetic mode).
      These were made protected in 3.1.1, use public gets/sets.
      @since 2.9.2
   */
  protected double vn,ve,vu;

  /** Acceleration (meters/second^2) in X, Y, and X directions. These were made protected in 3.1.1, use public gets/sets. */
  protected double ax,ay,az;
  /** Acceleration (meters/second^2) in north, east, and up directions (geodetic mode).
      These were made protected in 3.1.1, use public gets/sets.
      @since 2.9.2
   */
  protected double an,ae,au;

  /** Index and offset of "VEL" record in DataFile. -1 if the named record does not exist. */
  protected int iVel,iVelOff;
  /** Index and offset of "ACC" record in DataFile. -1 if the named record does not exist. */
  protected int iAcc,iAccOff;

  /** The interpolation order to use, CubicHermite matches pre-3.3.0 functionality. */
  protected InterpolationOrder interpolationOrder = InterpolationOrder.CubicHermite;

  /** Creates a {@link #GEODETIC} state vector with all values set to 0. */
  public StateVector () {
    referenceFrame = GEODETIC;
  }

  /** Creates a state vector using the specified file (see {@link #setFile(Object,Object)}).
   *  @param ref  The Midas reference
   *  @param name The file from which to initialize <i>(Prior to NeXtMidas 3.3.0 this
   *              only accepted a string. This was changed in NeXtMidas 3.3.0 to match
   *              the usage in {@link BaseFile} which permits {@link FileName}, etc.)</i>
   *  @deprecated Since NeXtMidas 3.5.4, use {@link StateVector#StateVector(MidasReference,Object)}
   */
  @Deprecated
  @SuppressWarnings("deprecation")
  public StateVector (Object ref, Object name) {
    super(ref,name);
  }

  /** Creates a state vector using the specified file (see {@link #setFile(Object,Object)}).
   *  @param ref  The Midas reference
   *  @param name The file from which to initialize <i>(Prior to NeXtMidas 3.3.0 this
   *              only accepted a string. This was changed in NeXtMidas 3.3.0 to match
   *              the usage in {@link BaseFile} which permits {@link FileName}, etc.)</i>
   *  @since NeXtMidas 3.5.4
   */
  public StateVector (MidasReference ref, Object name) {
    super(ref,name);
  }

  /** Create a Position given a position file.
   *  @param file The file to use.
   *  @since NeXtMidas 3.5.4
   */
  @ProvisionalUseOnly("Added in NeXtMidas 3.5.4")
  public StateVector (BaseFile file) {
    super(file);
  }

  /** Creates a new StateVector with a copy of the data in this StateVector. Note that this copies
      only the data (x,y,z,vx,vy,vz, etc.), and does not copy any file references. This is typically
      used in cases where one wants to make a "snapshot in time" of a given state vector (often used
      with multi-threaded applications to allow separate threads to compute values at different times
      without interfering with each other). <br>
      <br>
      <b>Due to possible disagreement between {@link #getTime()} and {@link #getTimeCode()}, the
      new instance is *not* required to maintain both values and typically only retains the value
      from {@link #getTime()}.</b>
      @return A new instance with the same values as this object.
   */
  @Override
  public StateVector copy () {
    return copyTo(new StateVector());
  }

  /** Copies values to given object.
   *  @param sv The destination.
   *  @return The destination (allows chaining).
   */
  protected StateVector copyTo (StateVector sv) {
    super.copyTo(sv);
    sv.vx = vx; sv.vy = vy; sv.vz = vz;
    sv.ax = ax; sv.ay = ay; sv.az = az;
    sv.vn = vn; sv.ve = ve; sv.vu = vu;
    sv.an = an; sv.ae = ae; sv.au = au;
    return sv;
  }

  @Override
  public Table toTable () {
    Table tbl = super.toTable();
    tbl.put("VX", getVX());
    tbl.put("VY", getVY());
    tbl.put("VZ", getVZ());
    tbl.put("AX", getAX());
    tbl.put("AY", getAY());
    tbl.put("AZ", getAZ());
    tbl.put("SPEED",    getSpeed());    // Legacy getSpeed()
    tbl.put("HEADING",  getHeading());
    tbl.put("TIMECODE", getTimeCode());
    tbl.put("VU", getVU());             // Velocity-Up
    tbl.put("VE", getVE());             // Velocity-East
    tbl.put("VN", getVN());             // Velocity-North
    tbl.put("AU", getAU());             // Acceleration-Up
    tbl.put("AE", getAE());             // Acceleration-East
    tbl.put("AN", getAN());             // Acceleration-North
    return tbl;
  }

  @Override
  protected boolean setFileT5Data (DataFile df) {
    if (!super.setFileT5Data(df)) return false;
    iVel = df.findRec("VEL");
    iAcc = df.findRec("ACC");
    iVelOff = (iVel < 0)? -1 : df.getCompOffset(iVel);
    iAccOff = (iAcc < 0)? -1 : df.getCompOffset(iAcc);
    return true;
  }

  /** Gets a copy of all of the state entries at the entry points specified in the file.
   *  @return copy of the state entries
   *  @since NeXtMidas 3.3.0
   */
  public List<Table> getStateEntries () {
    return getStateEntries(null,true);
  }

  /** Gets a copy of the state entries at the specified times.
   *  @param times The specified times to get the ephemeris for.
   *  @return copy of state entries
   *  @since NeXtMidas 3.7.0
   */
  public List<Table> getStateEntries (List<Time> times) {
    return getStateEntries(times,true);
  }

  /** Internal use only: Gets the entries, but don't force a copy of them. */
  private List<Table> getStateEntries (List<Time> times, boolean copy) {
    if (times != null) {
      // Special case where user wants specified time entries.
      List<Table> entries = new ArrayList<>();
      for (Time t : times) {
        setCurrentTime(t);
        entries.add(toTable());
      }
    return entries;
    }
    if (data != null) {
      if (copy) {
        List<Table> entries = new ArrayList<>();
        for (Table e : data) entries.add(e.copy());
        return entries;
      }
      else {
        return data;
      }
    }
    else if (db == null) {
      return Arrays.asList(toTable());
    }
    else {
      List<Table> entries = new ArrayList<>();

      DataFile df = getDataFile();
      for (int i = 0; i < df.getSize(); i++) {
        int off = i * db.bpe;

        Table row = new Table();
        row.put("TIME",        df.getTimeAt(i));       // double - "TIME" can hold times relative to start of file or from J1950 (see getTime())
        row.put("CURRENTTIME", df.getTimeAt(i, null)); // since 3.7.0: Time - always from J1950 (Bug 2981)
        if (iPos >= 0) {
          double[] p = unpack(off,iPosOff,db,null);
          row.put("X" ,p[0]); row.put("Y" ,p[1]); row.put("Z" ,p[2]);
        }
        if (iVel >= 0) {
          double[] v = unpack(off,iVelOff,db,null);
          row.put("VX",v[0]); row.put("VY",v[1]); row.put("VZ",v[2]);
        }
        if (iAcc >= 0) {
          double[] a = unpack(off,iAccOff,db,null);
          row.put("AX",a[0]); row.put("AY",a[1]); row.put("AZ",a[2]);
        }

        entries.add(row);
      }
      return entries;
    }
  }

  /** Gets the data for the vector-style keywords, returns {time,posX,posY,posZ,...}.
   *  @param times     The times (J1950) to get entries for (null = use default).
   *  @param timeEpoch The time epoch for PLATINUM keywords (null if not PLATINUM).
   *  @param timeDelta The time delta for PLATINUM keywords.
   */
  private Data[] _toKeywords (List<Time> times, Time timeEpoch, double timeDelta) {
    Transform   xform= Transform.toTransform("ECR");
    List<Table> ent  = getStateEntries(times,false);
    int         len  = ent.size();
    Time[]      time = new Time[len];
    double[]    posX = new double[len], posY = new double[len], posZ = new double[len];
    double[]    velX = new double[len], velY = new double[len], velZ = new double[len];
    double[]    accX = new double[len], accY = new double[len], accZ = new double[len];
    Object      tc;

    int i = 0;
    for (Table e : ent) {
      double[] pva = new double[] { e.getD("X" ), e.getD("Y" ), e.getD("Z" ),
                                    e.getD("VX"), e.getD("VY"), e.getD("VZ"),
                                    e.getD("AX"), e.getD("AY"), e.getD("AZ") };
      Time _time = getCurrentTime(e); // convert the time object to current time based on start time of file since 4.1.0
      Transform.fsu2fsu(pva, Transform.POSVELACC, transform, xform, _time);

      time[i] = _time;
      posX[i] = pva[0];  posY[i] = pva[1];  posZ[i] = pva[2];
      velX[i] = pva[3];  velY[i] = pva[4];  velZ[i] = pva[5];
      accX[i] = pva[6];  accY[i] = pva[7];  accZ[i] = pva[8];
      i++;
    }

    if (timeEpoch == null) {
      double[] _tc = new double[time.length];
      for (int j = 0; j < time.length; j++) {
        _tc[j] = time[j].getSec();
      }
      tc = _tc;
    }
    else if (timeDelta == 0) {
      double[] _tc = new double[time.length];
      for (int j = 0; j < time.length; j++) {
        _tc[j] = time[j].diff(timeEpoch);
      }
      tc = _tc;
    }
    else {
      long[] _tc = new long[time.length];
      for (int j = 0; j < time.length; j++) {
        _tc[j] = time[j].toTimeTicks(timeEpoch, timeDelta);
      }
      tc = _tc;
    }

    return new Data[] { new Data(tc), new Data(posX), new Data(posY), new Data(posZ),
                                      new Data(velX), new Data(velY), new Data(velZ),
                                      new Data(accX), new Data(accY), new Data(accZ) };
  }

  /** Gets the set of state vector keywords that correspond to this state vector
   *  in the form applicable for an X-Midas file (see {@link StateVector} for a
   *  description of the "KeywordsEph" keywords used).
   *  @return The keywords for the {@link DataFile}.
   *  @since NeXtMidas 3.3.0
   */
  public Map<String,Object> toKeywordsEph () {
    return toKeywordsEph(null);
  }

  /** Gets the set of state vector keywords that correspond to this state vector
   *  in the form applicable for an X-Midas file (see {@link StateVector} for a
   *  description of the "KeywordsEph" keywords used).
   *  @param times The list of times (J1950) to get entries for (null = use default).
   *  @return The keywords for the {@link DataFile}.
   *  @since NeXtMidas 3.7.0
   */
  public Map<String,Object> toKeywordsEph (List<Time> times) {
    Map<String,Object> kwds = new LinkedHashMap<>();
    Data[]             vals = _toKeywords(times, null, 0.0);

    for (int i=0,j=1; i < vals[0].getSize(); i++,j++) {
      kwds.put("D:ETIM"+j, vals[0].getNumber(i));
      kwds.put("D:POSX"+j, vals[1].getNumber(i));
      kwds.put("D:POSY"+j, vals[2].getNumber(i));
      kwds.put("D:POSZ"+j, vals[3].getNumber(i));
      kwds.put("D:VELX"+j, vals[4].getNumber(i));
      kwds.put("D:VELY"+j, vals[5].getNumber(i));
      kwds.put("D:VELZ"+j, vals[6].getNumber(i));
      kwds.put("D:ACCX"+j, vals[7].getNumber(i));
      kwds.put("D:ACCY"+j, vals[8].getNumber(i));
      kwds.put("D:ACCZ"+j, vals[9].getNumber(i));
    }
    kwds.put("D:USE_SV_KEYWORDS", 1.0); // <-- Matches X-Midas usage
    return kwds;
  }

  /** Gets the set of state vector keywords that correspond to this state vector
   *  in the form applicable for an X-Midas file (see {@link StateVector} for a
   *  description of the "KeywordsVec" keywords used).
   *  @return The keywords for the {@link DataFile}.
   *  @since NeXtMidas 3.3.0
   */
  public Map<String,Object> toKeywordsVec () {
    return toKeywordsVec(null);
  }

  /** Gets the set of state vector keywords that correspond to this state vector
   *  in the form applicable for an X-Midas file (see {@link StateVector} for a
   *  description of the "KeywordsVec" keywords used).
   *  @param times The times to get entries for (null = use default).
   *  @return The keywords for the {@link DataFile}.
   *  @since NeXtMidas 3.7.0
   */
  public Map<String,Object> toKeywordsVec (List<Time> times) {
    Map<String,Object> kwds = new LinkedHashMap<>();
    Data[]             vals = _toKeywords(times, null, 0.0);
    kwds.put("D:ETIM_VEC", vals[0]);
    kwds.put("D:POSX_VEC", vals[1]);
    kwds.put("D:POSY_VEC", vals[2]);
    kwds.put("D:POSZ_VEC", vals[3]);
    kwds.put("D:VELX_VEC", vals[4]);
    kwds.put("D:VELY_VEC", vals[5]);
    kwds.put("D:VELZ_VEC", vals[6]);
    kwds.put("D:ACCX_VEC", vals[7]);
    kwds.put("D:ACCY_VEC", vals[8]);
    kwds.put("D:ACCZ_VEC", vals[9]);
    return kwds;
  }

  /** Gets the set of state vector keywords that correspond to this state vector
   *  in the form applicable for a Platinum file (see {@link StateVector} for a
   *  description of the "KeywordsPlat" keywords used).
   *  @param timeEpoch  The time epoch used in the platinum file (null=Use Midas
   *                    time epoch of 1950-01-01).
   *  @param timeDelta  The time delta used in the platinum file. Use
   *                    <code>timeDelta=0.0</code> to use units of time (in seconds)
   *                    rather than time ticks.
   *  @param prefix     Override the prefix used (null = use default, "EPH").
   *  @return The keywords for the {@link DataFile}.
   *  @since NeXtMidas 3.3.0
   */
  public Map<String,Object> toKeywordsPlat (Time timeEpoch, double timeDelta, String prefix) {
    return toKeywordsPlat(null, timeEpoch, timeDelta, prefix);
  }

  /** Gets the set of state vector keywords that correspond to this state vector
   *  in the form applicable for a Platinum file (see {@link StateVector} for a
   *  description of the "KeywordsPlat" keywords used).
   *  @param times      The times to get entries for (null = use default).
   *  @param timeEpoch  The time epoch used in the platinum file (null=Use Midas
   *                    time epoch of 1950-01-01).
   *  @param timeDelta  The time delta used in the platinum file. Use
   *                    <code>timeDelta=0.0</code> to use units of time (in seconds)
   *                    rather than time ticks.
   *  @param prefix     Override the prefix used (null = use default, "EPH").
   *  @return The keywords for the {@link DataFile}.
   *  @since NeXtMidas 3.7.0
   */
  public Map<String,Object> toKeywordsPlat (List<Time> times, Time timeEpoch, double timeDelta, String prefix) {
    if (prefix == null) prefix = "EPH";
    char charDOUBLE = (char)DataTypes.DOUBLE;

    Map<String,Object> kwds  = new LinkedHashMap<>();
    Data[]             vals  = _toKeywords(times, timeEpoch, timeDelta);
    char               type  = (char)((timeDelta != 0)? DataTypes.XLONG : DataTypes.DOUBLE);
    int                units = (timeDelta != 0)? Units.COUNTS : Units.TIME_S;

    putWithUnits(kwds, type,prefix+".TIME",  vals[0], units);
    putWithUnits(kwds, charDOUBLE, prefix+"_POS.X", vals[1], Units.DISTANCE_M);
    putWithUnits(kwds, charDOUBLE, prefix+"_POS.Y", vals[2], Units.DISTANCE_M);
    putWithUnits(kwds, charDOUBLE, prefix+"_POS.Z", vals[3], Units.DISTANCE_M);
    putWithUnits(kwds, charDOUBLE, prefix+"_VEL.X", vals[4], Units.SPEED_M_S);
    putWithUnits(kwds, charDOUBLE, prefix+"_VEL.Y", vals[5], Units.SPEED_M_S);
    putWithUnits(kwds, charDOUBLE, prefix+"_VEL.Z", vals[6], Units.SPEED_M_S);
    putWithUnits(kwds, charDOUBLE, prefix+"_ACC.X", vals[7], Units.ACCELERATION_M_S2);
    putWithUnits(kwds, charDOUBLE, prefix+"_ACC.Y", vals[8], Units.ACCELERATION_M_S2);
    putWithUnits(kwds, charDOUBLE, prefix+"_ACC.Z", vals[9], Units.ACCELERATION_M_S2);
    return kwds;
  }

  /** Gets the set of state vector keywords that correspond to this state vector
   *  in the form applicable for a Platinum file (see {@link StateVector} for a
   *  description of the "KeywordsPlat" keywords used). <br>
   *  <br>
   *  This simply takes 'timeEpoch' and 'timeDelta' from the file's keywords and
   *  'SVPREFIX' from the file's qualifiers and then calls:
   *  <code>toFileKeywordsPlat(timeEpoch, timeDelta, prefix)</code>.
   *  @param df The file to take the 'timeEpoch' and 'timeDelta' from.
   *  @return The keywords for the {@link DataFile}.
   *  @since NeXtMidas 3.3.0
   */
  public Map<String,Object> toKeywordsPlat (DataFile df) {
    Keywords kw        = df.getKeywordsObject();
    Time     timeEpoch = Time.toTime(kw.get("TIME_EPOCH"), Time.ISO8601);
    double   timeDelta = kw.getD("TIME_DELTA", 0.0);
    String   prefix    = df.getQualifiers().getS("SVPREFIX", "EPH");

    return toKeywordsPlat(timeEpoch, timeDelta, prefix);
  }

  /** Gets the set of state vector keywords in LON/LAT/ALT form that correspond
   *  to the <b>single instantaneous point</b> represented by this state vector
   *  in the form applicable for a Platinum file (see {@link StateVector} for a
   *  description of the "KeywordsLLA" keywords used).
   *  @param prefix The prefix used. (required)
   *  @return The keywords for the {@link DataFile}.
   *  @since NeXtMidas 3.3.0
   */
  public Map<String,Object> toKeywordsLLA (String prefix) {
    if (prefix == null) throw new NullPointerException("Prefix can not be null.");

    // The use of ANGLE_DEG for LAT/LON and DISTANCE_M for ALT since this matches
    // the Platinum standard which (as of rev 4) does not recognize LATITUDE_DEG,
    // LONGITUDE_DEG, or ALTITUDE_M which are from the BLUE 1.0 ICD.
    Map<String,Object> kwds = new LinkedHashMap<>();
    putWithUnits(kwds, prefix+".LON", getLon(), Units.ANGLE_DEG);
    putWithUnits(kwds, prefix+".LAT", getLat(), Units.ANGLE_DEG);
    putWithUnits(kwds, prefix+".ALT", getAlt(), Units.DISTANCE_M);
    return kwds;
  }

  /** Gets the set of state vector keywords in LON/LAT/ALT form that correspond
      to the <b>single instantaneous point</b> represented by this state vector
      in the form applicable for a Platinum file (see {@link StateVector} for a
      description of the "KeywordsLLA" keywords used).
      @param df The file to get the SVPREFIX from.
      @return The keywords for the {@link DataFile}.
      @since NeXtMidas 3.3.0
   */
  public Map<String,Object> toKeywordsLLA (DataFile df) {
    String prefix = df.getQualifiers().getS("SVPREFIX", null);
    return toKeywordsLLA(prefix);
  }

  /** Gets the set of data rows for a {@link ListFile} that correspond to this state vector.
      (See {@link StateVector} for a description of the "Entries" used).
      @param xf  The output transform (see {@link Transform#toTransform}), set to null to use the current transform.
      @return The rows for the {@link ListFile}.
      @since NeXtMidas 3.3.0
   */
  public List<Table> toFileEntries (Object xf) {
    Transform   xform    = Transform.toTransform(xf);
    List<Table> fileData = new ArrayList<>();

    for (Table e : getStateEntries(null,false)) {
      Time     tc  = Time.toTime(e.get("TIME"));        // this can be relative from start of file -or- from J1950
      Time curTime = getCurrentTime(e); // Created function to get CURRENTTIME or figure it out using TIME since 4.1.0
      Table    row = new Table();
      double[] pva = new double[] { e.getD("X" ), e.getD("Y" ), e.getD("Z" ),
                                    e.getD("VX"), e.getD("VY"), e.getD("VZ"),
                                    e.getD("AX"), e.getD("AY"), e.getD("AZ") };
      Transform.fsu2fsu(pva, Transform.POSVELACC, transform, xform, tc); // TODO: check if we should use "CURRENTTIME" instead?
      Data dat = new Data(pva);
      row.put("TIME", tc);
      row.put("CURRENTTIME", curTime);
      row.put("POS",  dat.getElementAt( 0, "VD", 1));
      row.put("VEL",  dat.getElementAt(24, "VD", 1));
      row.put("ACC",  dat.getElementAt(48, "VD", 1));
      fileData.add(row);
    }
    return fileData;
  }

  /** Gets the set of data rows for a {@link ListFile} that correspond to this
      state vector. (See {@link StateVector} for a description of the "Entries"
      used). Unlike {@link #toFileEntries(Object)} which uses whatever time-spacing
      is already present, this will force the times to be evenly spaced. This
      is done by varying the time-code of the state vector and using the resulting
      values to populate the file. <b>This will make calls to {@link #setTimeCode}
      and result in the state vector having a different instantaneous value than before.</b> <br>
      <br>
      The "inclusiveness" of t1 is explicitly defined such that the last point
      in the returned data will be "at or beyond t1 but before t1+dt". This way
      t1 is always included even if the choice of dt (possibly due to rounding)
      doesn't exactly satisfy the equation "t1=t0+dt*N".
      @param t0  The start time <b>(inclusive)</b>. If null, the time of the
                 first point in the state vector will be used.
      @param t1  The ending time <b>(inclusive)</b>. If null, the time of the
                 last point in the state vector will be used.
      @param dt  The time delta between points. If null, an attempt will be made
                 to determine a reasonable time delta based on the data available,
                 failing that, a default of 1 minute (60.0 seconds) will be used.
      @param xf  The output transform (see {@link Transform#toTransform}), set
                 to null to use the current transform.
      @return The rows for the {@link ListFile}.
      @since NeXtMidas 3.3.0
   */
  public List<Table> toFileEntries (Time t0, Time t1, Double dt, Object xf) {
    // FILL IN ANY MISSING DEFAULTS ============================================
    if ((dt != null) && (dt <= 0)) dt = null;

    int len = (data == null)? -1 : data.size();

    if (db != null) {
      if (t0 == null) t0 = getStartTime();
      if (dt == null) dt = getDataFile().getDelta();
      if (t1 == null) t1 = getEndTime();
    }
    else if (len > 1) {
      Time   _t0 = getStartTime();
      Time   _t1 = getEndTime();
      double _dt = _t1.diff(_t0) / (len - 1);

      if (t0 == null) t0 = _t0;
      if (t1 == null) t1 = _t1;
      if (dt == null) dt = (t1.diff(t0) > 1)? max(1.0,  rint(_dt))
                                            : max(1e-6, roundToMicro(_dt));
    }
    else if (len == 1) {
      if (t0 == null) t0 = getStartTime();
      if (dt == null) dt = 60.0;
      if (t1 == null) t1 = getEndTime();
    }
    else {
      if (t0 == null) t0 = getCurrentTime();
      if (dt == null) dt = 60.0;
      if (t1 == null) t1 = getCurrentTime();
    }

    // COMPUTE TIME VALUES =====================================================
    List<Time> times = new ArrayList<>();
    for (int i = 0; true; i++) {
      Time tc = Time.add(t0, dt * i);
      times.add(tc);
      if (tc.compareTo(t1) >= 0) break;
    }
    return toFileEntries(times, xf);
  }

  /** Gets the set of data rows for a {@link ListFile} that correspond to this state vector.
      (See {@link StateVector} for a description of the "Entries" used).
      Unlike {@link #toFileEntries(Object)} which uses whatever time-spacing
      is already present, this will force the times to match those passed in. This
      is done by varying the time-code of the state vector and using the resulting
      values to populate the file. <b>This will make calls to {@link #setTimeCode}
      and result in the state vector having a different instantaneous value than before.</b>
      @param times The list of times (J1950) to use.
      @param xf    The output transform (see {@link Transform#toTransform}), set
                   to null to use the current transform.
      @return The rows for the {@link ListFile}, null if list of times is null.
      @since NeXtMidas 3.3.0
   */
  public List<Table> toFileEntries (List<Time> times, Object xf) {
    if (times == null) return null;

    Transform   xform    = Transform.toTransform(xf);
    List<Table> fileData = new ArrayList<>();

    for (Time tc : times) {
      setCurrentTime(tc);
      Table    row = new Table();
      double[] pva = new double[] { getX(),  getY(),  getZ(),
                                    getVX(), getVY(), getVZ(),
                                    getAX(), getAY(), getAZ() };
      Transform.fsu2fsu(pva, Transform.POSVELACC, transform, xform, tc);
      Data dat = new Data(pva, 3);
      row.put("TIME", tc);
      row.put("CURRENTTIME", convertTimeToCurrentTime(tc)); // convert the time object to current time based on start time of file since 4.1.0
      row.put("POS",  dat.getElementAt( 0, "VD", 1));
      row.put("VEL",  dat.getElementAt(24, "VD", 1));
      row.put("ACC",  dat.getElementAt(48, "VD", 1));
      fileData.add(row);
    }
    return fileData;
  }

  /** Gets the set of data rows for a {@link ListFile} that correspond to this state vector.
      If the given file is a {@link DataFile} the output transform
      will be set to match the file's quadwords, otherwise ECR is used.
      @param pf  The file to get the t0, t1, and dt from.
      @return The rows for the {@link ListFile}.
      @since NeXtMidas 3.3.0
   */
  public final List<Table> toFileEntries (PlotFile pf) {
    Time   t0   = (pf instanceof DataFile)? ((DataFile)pf).getTime()
                                          : new Time(pf.getStart());
    double dt   = pf.getDelta();
    double size = pf.getSize();
    Object xf   = (pf instanceof DataFile)? pf : "ECR";

    return toFileEntries(t0, Time.add(t0, dt*(size-1)), dt, xf);
  }

  /** Gets the set of record definitions applicable for a {@link ListFile} or
      {@link DataFile} created using the data from {@link #toFileEntries(Object)}
      or {@link #toFileEntries(Time,Time,Double,Object)}.
      This will specifically include: "TIME", "POS", "VEL", "ACC" where the
      units and coordinate systems match the state-vector's transform.
      @return table containing applicable record definitions
   */
  public final Table toFileRecDefs () {
    Table   recDefs  = new Table();
    String  coordSys = transform.getCoordinateSystem();
    boolean hasPOS   = (db == null) || (iPos >= 0);
    boolean hasVEL   = (db == null) || (iVel >= 0);
    boolean hasACC   = (db == null) || (iAcc >= 0);

    if ((data != null) && !data.isEmpty()) {
      Table tbl = data.get(0);
      hasPOS = tbl.containsKey("X" );
      hasVEL = tbl.containsKey("VX");
      hasACC = tbl.containsKey("AX");
    }

    // The ordering of POS,VEL,ACC,TIME matches old X-Midas commands LATLONTOECI, LATLONTOECR, etc.
    if (hasPOS) recDefs.put("POS",  recDef("POS",  "VD", null, coordSys, transform.getPosUnits()));
    if (hasVEL) recDefs.put("VEL",  recDef("VEL",  "VD", null, coordSys, transform.getVelUnits()));
    if (hasACC) recDefs.put("ACC",  recDef("ACC",  "VD", null, coordSys, transform.getAccUnits()));
    if (true  ) recDefs.put("TIME", recDef("TIME", "SD", null, "SCALAR", "TIME_S"));
    return recDefs;
  }

  /** Gets the data file being used for the state vector.
      @return data file being used for the state vector
      @since NeXtMidas 2.5.5
   */
  public DataFile getDataFile () {
    return (inFile instanceof DataFile)? (DataFile)inFile : null;
  }

  /** Sets the interpolation order used and updates the internal values accordingly.
      @param order The interpolation order to use. Note that setting this to null
                   indicates that the "best available" interpolation should be used,
                   rather than the backwards-compatible version. Setting this to
                   {@link InterpolationOrder#CubicHermite} matches NeXtMidas 3.2 and earlier.
      @since NeXtMidas 3.3.0
   */
  public final void setInterpolationOrder (String order) {
    if (StringUtil.isNull(order)) {
      setInterpolationOrder((InterpolationOrder)null);
    }
    else {
      setInterpolationOrder(Util.valueOf(InterpolationOrder.class, order));
    }
  }

  /** Sets the interpolation order used and updates the internal values accordingly.
      @param order The interpolation order to use. Note that setting this to null
                   indicates that the "best available" interpolation should be used,
                   rather than the backwards-compatible version. Setting this to
                   {@link InterpolationOrder#CubicHermite} matches NeXtMidas 3.2 and earlier.
      @since NeXtMidas 3.3.0
   */
  public void setInterpolationOrder (InterpolationOrder order) {
    interpolationOrder = order;
    process();
  }

  /** Gets the interpolation order used.
      @return interpolation order used
      @since NeXtMidas 3.3.0
   */
  public InterpolationOrder getInterpolationOrder () {
    return interpolationOrder;
  }

  /** Process the file to set position, velocity and accelerations */
  @Override
  public void process () {
    if ((data == null) && (db == null)) { x=y=z=vx=vy=vz=ax=ay=az=lon=lat=alt=0; return; }

    // this method mostly moved to getPosVelAccAtTime(..) below in 3.1.1 for Bug 2561
    double[] pva;
    if (db != null) {
      pva = getPosVelAccAtTime(getDataFile(), t, db, false, interpolationOrder);
    }
    else {
      pva = getPosVelAccAtTime(data, currentTime, transform, false, interpolationOrder);
    }

    double[] p = { pva[0],pva[1],pva[2] }; // position information
    double[] v = { pva[3],pva[4],pva[5] }; // velocity
    double[] a = { pva[6],pva[7],pva[8] }; // acceleration

    setCar(p, v, a);
  }

  /** Get an array of doubles that corresponds to a Position, Velocity, and Acceleration
      of the StateVector at a given time. This is identical to:
      <code>getPosVelAccAtTime(df, t, db, convertToLLA, InterpolationOrder.CubicHermite)</code>
      where t is the time-code (not time relative to start of file).
      @param df   The StateVector file
      @param time The time for which to retrieve the position. Relative to file's XStart.
      @param db   The data object containing the data from the file (optional).
      @param convertToLLA Whether or not to convert/transform the coordinates to LON/LAT/ALT.
      @return An array of doubles of size = 9. The array breaks down as such:
              <pre>
                array[0] = PosX (Lon)   array[1] = PosY (Lat)     array[2] = PosZ (Alt)
                array[3] = VelX         array[4] = VelY           array[5] = VelZ
                array[6] = AccX         array[7] = AccY           array[8] = AccZ
              </pre>
      @since NeXtMidas 3.1.1
   */
  public static double[] getPosVelAccAtTime (DataFile df, double time, Data db, boolean convertToLLA) {
    return getPosVelAccAtTime(df, time, db, convertToLLA, InterpolationOrder.CubicHermite);
  }

  /** <b>Work in progress may change within this beta series:</b>
      Get an array of doubles that corresponds to a Position, Velocity, and Acceleration
      of the StateVector at a given time. If the given time falls between two elements
      in the file, then the value will be determined via the {@link #interpolate} method.
      @param df   The StateVector file
      @param time The time for which to retrieve the position. Relative to file's XStart.
      @param db   The data object containing the data from the file. If null, this method will
                  load it from the given DataFile. If not null, then the Data will be used
                  "as-is" from the Data Object and MUST already contain the Data from the given
                  file. For performance reasons, when calling this method
                  from a loop (e.g. to retrieve multiple positions), this Data object should already
                  be loaded to avoid extraneous processing.
      @param convertToLLA Whether or not to convert/transform the coordinates to LON/LAT/ALT.
      @param order        The interpolation order to use.
      @return An array of doubles of size = 9. The array breaks down as such:
              <pre>
                array[0] = PosX (Lon)   array[1] = PosY (Lat)     array[2] = PosZ (Alt)
                array[3] = VelX         array[4] = VelY           array[5] = VelZ
                array[6] = AccX         array[7] = AccY           array[8] = AccZ
              </pre>
      Note: Any missing values (not contained in the file or Data object) will be inserted as 0;
      @since NeXtMidas 3.3.0
   */
  public static double[] getPosVelAccAtTime (DataFile df, double time, Data db, boolean convertToLLA,
                                             InterpolationOrder order) {
    double[] pva  = new double[9];     // Values to return
    double[] p    = new double[3];
    double[] v    = new double[3];
    double[] a    = new double[3];
    int iPos, iPosOff;
    int iVel, iVelOff;
    int iAcc, iAccOff;

    // if given a null Data object, load it from the file
    if (db==null) {
      df.open();
      int dfSize = (int) df.size;
      db         = df.getDataBuffer(dfSize);
      int ndo    = (int)df.avail();
      df.read(db, ndo);
      df.close();
    }

    // need to add use of accelerations for interpolation here
    double offset = (time - df.getXStart())/df.getXDelta();
    int    size   = (int)df.getSize();

    //Determine position, velocity and acceleration data
    iVel = df.findRec("VEL");
    iAcc = df.findRec("ACC");
    iPos = df.findRec("POS");
    iPosOff = (iPos < 0)? -1 : df.getCompOffset(iPos);
    iVelOff = (iVel < 0)? -1 : df.getCompOffset(iVel);
    iAccOff = (iAcc < 0)? -1 : df.getCompOffset(iAcc);

    // Bound the value within the file. If not within the file, use the start/end point in the
    // file as its value. When plotted this will then be a flat line outside of the limits of
    // the file which should suggest to the user that they ran out of data points.
    if (offset < 0     ) offset = 0;
    if (offset > size-1) offset = size - 1;

    int off = (int)offset; // Integral offset into the file
    if (off == offset) { // exact, no interpolation needed
      int k = off * db.bpe;

      p = unpack(k,iPosOff,db,p);
      v = unpack(k,iVelOff,db,v);
      a = unpack(k,iAccOff,db,a);
    }
    else {                   // need to interpolate based on two nearest points
      int off0 = off;
      int off1 = off+1;
      if (off1 == df.getSize()) { off0--; off1--; } // after last point, extrapolate from two previous

      int k0 = off0 * db.bpe;
      int k1 = off1 * db.bpe;

      double[] p0 = unpack(k0,iPosOff,db,null);
      double[] v0 = unpack(k0,iVelOff,db,null);
      double[] a0 = unpack(k0,iAccOff,db,null);
      double[] p1 = unpack(k1,iPosOff,db,null);
      double[] v1 = unpack(k1,iVelOff,db,null);
      double[] a1 = unpack(k1,iAccOff,db,null);

      double t0 = df.getXStart() + df.getXDelta()*off0;
      double t1 = df.getXStart() + df.getXDelta()*off1;
      interpolate(p0,v0,a0,t0, p1,v1,a1,t1, p,v,a,time, 3, order);
    }
    // convert the position to Lon/Lat/Alt
    if (convertToLLA) {
      Transform  transform = new Transform(df); // Transform to use (based on DataFile quadwords)
      transform.toLonLatAlt(p, 1);
    }

    // add the position to the array of doubles
    pva[0] = p[0]; pva[1] = p[1]; pva[2] = p[2];  // first three doubles are position
    pva[3] = v[0]; pva[4] = v[1]; pva[5] = v[2];  // next three doubles are velocity
    pva[6] = a[0]; pva[7] = a[1]; pva[8] = a[2];  // last three doubles are accel

    return pva;
  }

  /** Unpacks data from the file. */
  private static double[] unpack (int off, int boff, Data db, double[] def) {
    if (boff < 0) return def;
    int k = off + boff;
    return new double[] {db.unpackD(k + 0), db.unpackD(k + 8), db.unpackD(k + 16)};
  }

  /** Unpacks data from the table. */
  private static double[] unpack (String x, String y, String z, Table row, double[] def) {
    if (!row.containsKey(x)) return def;
    return new double[] { row.getD(x), row.getD(y), row.getD(z) };
  }

  /** Get an array of doubles that corresponds to a Position, Velocity, and Acceleration
      of the StateVector at a given time. If the given time falls between two elements
      in the file, then the value will be determined via the {@link #interpolate} method.
      @param data         The StateVector data in table form.
      @param time         The time for which to retrieve the position. Relative to file's XStart.
      @param transform    The transform to use.
      @param convertToLLA Whether or not to convert/transform the coordinates to LON/LAT/ALT.
      @param order        The interpolation order to use.
      @return An array of doubles of size = 9. The array breaks down as such:
              <pre>
                array[0] = PosX (Lon)   array[1] = PosY (Lat)     array[2] = PosZ (Alt)
                array[3] = VelX         array[4] = VelY           array[5] = VelZ
                array[6] = AccX         array[7] = AccY           array[8] = AccZ
              </pre>
      Note: Any missing values (not contained in the file or Data object) will be inserted as 0;
      @since NeXtMidas 3.3.0
   */
  public static double[] getPosVelAccAtTime (List<Table> data, Time time, Transform transform, boolean convertToLLA,
                                             InterpolationOrder order) {
    double[] pva  = new double[9];     // Values to return
    double[] p    = new double[3];
    double[] v    = new double[3];
    double[] a    = new double[3];
    int      off0 = -1;
    int      off1 = 0;

    // With off0 and off1 as indices into data, find the two elements that bracket the given time.
    // In the case of an exact match, set both indices to the matching element.  Do not extrapolate
    // if the given time is before the first element or after the last element.
    for (Table table : data) {
      int cmp = time.compareTo(Time.toTime(table.getO("TIME")));
      if (cmp<0) break;
      ++off0;
      if (cmp==0) break;
      ++off1;
    }
    if (off0==-1         ) ++off0; // not going to extrapolate states occurring before start
    if (off1==data.size()) --off1; // not going to extrapolate states occurring after end

    if (off0 == off1) {
      Table row0 = data.get(off0);
      p = unpack( "X", "Y", "Z",row0,p);
      v = unpack("VX","VY","VZ",row0,v);
      a = unpack("AX","AY","AZ",row0,v);
    }
    else {
      Table row0 = data.get(off0);
      Table row1 = data.get(off1);
      Time  t0   = Time.toTime(row0.get("TIME"));
      Time  t1   = Time.toTime(row1.get("TIME"));

      double[] p0 = unpack( "X", "Y", "Z",row0,null);
      double[] v0 = unpack("VX","VY","VZ",row0,null);
      double[] a0 = unpack("AX","AY","AZ",row0,null);
      double[] p1 = unpack( "X", "Y", "Z",row1,null);
      double[] v1 = unpack("VX","VY","VZ",row1,null);
      double[] a1 = unpack("AX","AY","AZ",row1,null);

      interpolate(p0,v0,a0,t0, p1,v1,a1,t1, p,v,a,time, 3, order);
    }

    // convert the position to Lon/Lat/Alt
    if (convertToLLA) {
      transform.toLonLatAlt(p, 1);
    }

    // add the position to the array of doubles
    pva[0] = p[0]; pva[1] = p[1]; pva[2] = p[2];  // first three doubles are position
    pva[3] = v[0]; pva[4] = v[1]; pva[5] = v[2];  // next three doubles are velocity
    pva[6] = a[0]; pva[7] = a[1]; pva[8] = a[2];  // last three doubles are accel

    return pva;
  }

  @Override
  public String toString () {
    return "SVPos(x "+x+",y "+y+",z "+z
                +",vx "+vx+",vy "+vy+",vz "+vz
                +",ax "+ax+",ay "+ay+",az "+az
                +",vu "+vu+",ve "+ve+",vn "+vn
                +",au "+au+",ae "+ae+",an "+an
                +",t "+t+", lon "+lon+",lat "+lat+",alt "+alt+")";
  }

  /** Gets the current time in J1950 seconds. Since NeXtMidas 3.3.0 this is
      identical to <code>setCurrentTime(new Time(value))</code>.
      @param value Time in seconds since midnight, 1 Jan 1950.
      @return true if the time-code is within the range of the current state vector file, false if not.
              This can be used to identify cases where the file is no longer valid and must be changed.
      @since NeXtMidas 2.5.5
      @see #setCurrentTime(Time)
   */
  public boolean setTimeCode (double value) {
    return setCurrentTime(new Time(value));
  }

  /** Gets the current time in J1950 seconds. Since NeXtMidas 3.3.0 this is
      identical to <code>getCurrentTime().getSec()</code>.
      @return Time in seconds since midnight, 1 Jan 1950.
      @since NeXtMidas 2.5.5
      @see #getCurrentTime()
   */
  public double getTimeCode () {
    return getCurrentTime().getSec();
  }

  /** Sets velocity using heading and speed. At this time, this computation assumes
      evenly spaced degrees of latitude and longitude (see {@link #METERS2DEG}).
      @param heading The heading in deg (0=North,90=East,etc).
      @param speed   The speed in m/s.
   */
  public void setHeadingSpeed (double heading, double speed) {
    if (speed == 0) {
      setVel(0.0, 0.0, 0.0);
      return;
    }

    if (heading >= 360) heading = heading % 360; // this should be rare

    double deg  = heading % 90;
    double rad  = deg * (Math.PI / 180.0);
    double mLon; // m/s along line of longitude
    double mLat; // m/s along line of latitude

         if (heading <  90) { mLon =  speed * Math.sin(rad);  mLat =  speed * Math.cos(rad); } // NE quadrant
    else if (heading < 180) { mLon =  speed * Math.cos(rad);  mLat = -speed * Math.sin(rad); } // SE quadrant
    else if (heading < 270) { mLon = -speed * Math.sin(rad);  mLat = -speed * Math.cos(rad); } // SW quadrant
    else                    { mLon = -speed * Math.cos(rad);  mLat =  speed * Math.sin(rad); } // NW quadrant
    double dLon = mLon * METERS2DEG; // deg/s along line of longitude
    double dLat = mLat * METERS2DEG; // deg/s along line of latitude

    Position p = Position.fromGeo(getAlt(), getLat()+dLat, getLon()+dLon);
    setVel(p.getX()-getX(), p.getY()-getY(), p.getZ()-getZ());
  }

  /** Sets the velocity using heading and speed and takes into account the
      unevenly spaced latitude and longitude degrees. Using these values, this
      method will set the Velocity North, East, and Up values, which can be
      determined when the bearing and overall speed are provided.
      @param heading The heading in degrees (0=North, 90=East, etc.).
      @param speed The object's speed in meters per second (m/s).
      @since NeXtMidas 2.9.2
   */
  public void setHeadingSpeedGeodetic (double heading, double speed) {
    if (speed == 0) {
      setVel(0.0, 0.0, 0.0);
      return;
    }

    if (heading >= 360) heading = heading % 360; // this should be rare

    double rad = Constants.DEG2RAD * heading;
    double e = Math.sin(rad) * speed;
    double n = Math.cos(rad) * speed;
    double u = 0.0d;

    // Set Velocity East, North, and Up on this instance of StateVector
    // These values are needed to properly compute geodetic heading
    setVelGeo(e, n, u);

    if (referenceFrame == NORMAL)
      Transform.geo2car(this);
  }

  /** Set CARTESIAN position, velocity and acceleration. This is the same as calling
      <code>setCar(pos)</code>, <code>setVel(vel)</code>, and <code>setAcc(acc)</code>, except that
      any null array is the same as using <code>{0,0,0}</code>.
      @param pos position offset { px, py, pz }
      @param vel velocity        { vx, vy, vz }
      @param acc acceleration    { ax, ay, az }
      @since NeXtMidas 2.7.0
   */
  public void setCar (double[] pos, double[] vel, double[] acc) {
    if (pos != null) setCar(pos); else setCar(0,0,0);
    if (vel != null) setVel(vel); else setVel(0,0,0);
    if (acc != null) setAcc(acc); else setAcc(0,0,0);
  }

  /** Set the velocity. This is identical to <code>setVel(vel[0],vel[1],vel[2])</code>.
      @param vel velocity { vx, vy, vz }
      @since NeXtMidas 2.7.0
   */
  public void setVel (double[] vel) {
    setVel(vel[0],vel[1],vel[2]);
  }

  /** Set the velocity (m/s). This is identical to <code>setVelCar(vel[0],vel[1],vel[2])</code>.
      @param vel velocity { vx, vy, vz }
      @since NeXtMidas 2.9.2
   */
  public void setVelCar (double[] vel) {
    setVelCar(vel[0],vel[1],vel[2]);
  }

  /** Set the velocity (m/s). This is identical to <code>setVelGeo(vel[0],vel[1],vel[2])</code>.
      @param vel velocity { ve, vn, vu }
      @since NeXtMidas 2.9.2
   */
  public void setVelGeo (double[] vel) {
    setVelGeo(vel[0],vel[1],vel[2]);
  }

  /** Set the velocity.
      @param vx The velocity (m/s) in the x-direction
      @param vy The velocity (m/s) in the y-direction
      @param vz The velocity (m/s) in the z-direction
   */
  public void setVel (double vx, double vy, double vz) {
    this.vx = vx; this.vy = vy; this.vz=vz;
  }

  /** Set the velocity in Cartesian mode (x, y, z).
      @param vx The velocity (m/s) in the x-direction
      @param vy The velocity (m/s) in the y-direction
      @param vz The velocity (m/s) in the z-direction
   */
  public void setVelCar (double vx, double vy, double vz) {
    this.vx = vx; this.vy = vy; this.vz=vz;
  }

  /** Set the geodetic mode velocity (m/s) (East, North, Up).
      @param ve The velocity in the east-direction
      @param vn The velocity in the north-direction
      @param vu The velocity in the up-direction
      @since NeXtMidas 2.9.2
   */
  public void setVelGeo (double ve, double vn, double vu) {
    this.ve = ve; this.vn = vn; this.vu=vu;
  }

  /** Set the velocity (m/s) in the x-direction.
      @param val velocity (m/s) in the x-direction
      @since NeXtMidas 2.7.0
   */
  public void setVX (double val) { vx = val; }

  /** Set the velocity (m/s) in the y-direction.
      @param val velocity (m/s) in the y-direction
      @since NeXtMidas 2.7.0
   */
  public void setVY (double val) { vy = val; }

  /** Set the velocity (m/s) in the z-direction.
      @param val velocity (m/s) in the z-direction
      @since NeXtMidas 2.7.0
   */
  public void setVZ (double val) { vz = val; }

  /** Set the acceleration (m/s^2) in the x-direction.
      @param val acceleration (m/s^2) in the x-direction
      @since NeXtMidas 2.7.0
   */
  public void setAX (double val) { ax = val; }

  /** Set the acceleration (m/s^2) in the y-direction.
      @param val acceleration (m/s^2) in the y-direction
      @since NeXtMidas 2.7.0
   */
  public void setAY (double val) { ay = val; }

  /** Set the acceleration (m/s^2) in the z-direction.
      @param val acceleration (m/s^2) in the z-direction
      @since NeXtMidas 2.7.0
   */
  public void setAZ (double val) { az = val; }

  /** Sets the velocity (m/s) in the North direction.
      @param val velocity (m/s) in the North direction
      @since NeXtMidas 2.9.2
   */
  public void setVN (double val) { vn = val; }

  /** Sets the velocity (m/s) in the East direction.
      @param val velocity (m/s) in the East direction
      @since NeXtMidas 2.9.2
   */
  public void setVE (double val) { ve = val; }

  /** Sets the velocity (m/s) in the Up direction.
      @param val velocity (m/s) in the Up direction
      @since NeXtMidas 2.9.2
   */
  public void setVU (double val) { vu = val; }

  /** Sets the acceleration (m/s^2) in the North direction.
      @param val acceleration (m/s^2) in the North direction
      @since NeXtMidas 3.3.0
   */
  public void setAN (double val) { an = val; }

  /** Sets the acceleration (m/s^2) in the East direction.
      @param val acceleration (m/s^2) in the East direction
      @since NeXtMidas 3.3.0
   */
  public void setAE (double val) { ae = val; }

  /** Sets the acceleration (m/s^2) in the Up direction.
      @param val acceleration (m/s^2) in the Up direction
      @since NeXtMidas 3.3.0
   */
  public void setAU (double val) { au = val; }

  @Override public double getVX () { return vx; }
  @Override public double getVY () { return vy; }
  @Override public double getVZ () { return vz; }
  @Override public double getAX () { return ax; }
  @Override public double getAY () { return ay; }
  @Override public double getAZ () { return az; }

  @Override public double getVN () { return vn; }
  @Override public double getVE () { return ve; }
  @Override public double getVU () { return vu; }
  @Override public double getAN () { return an; }
  @Override public double getAE () { return ae; }
  @Override public double getAU () { return au; }

  /** Set the acceleration. This is identical to <code>setAcc(acc[0],acc[1],acc[2])</code>.
      @param acc acceleration { ax, ay, az }
      @since NeXtMidas 2.7.0
   */
  public void setAcc (double[] acc) {
    setAcc(acc[0], acc[1], acc[2]);
  }
  /** Set the acceleration.
      @param ax The acceleration in the x-direction
      @param ay The acceleration in the y-direction
      @param az The acceleration in the z-direction
   */
  public void setAcc (double ax, double ay, double az) {
    this.ax = ax; this.ay = ay; this.az = az;
  }

  /** Set the acceleration in Cartesian mode (x, y, z).
      @param ax The acceleration in the x-direction
      @param ay The acceleration in the y-direction
      @param az The acceleration in the z-direction
      @since NeXtMidas 2.9.2
   */
  public void setAccCar (double ax, double ay, double az) {
    this.ax = ax; this.ay = ay; this.az = az;
  }

  /** Set the acceleration. This is identical to <code>setAccCar(acc[0],acc[1],acc[2])</code>.
      @param acc acceleration { ax, ay, az }
      @since NeXtMidas 2.9.2
   */
  public void setAccCar (double[] acc) {
    setAccCar(acc[0],acc[1],acc[2]);
  }

  /** Set the acceleration (meters/second^2) in north, east, up directions (geodetic mode).
      @param ae The acceleration in the north direction
      @param an The acceleration in the east direction
      @param au The acceleration in the up  direction
      @since NeXtMidas 3.1.1
   */
  public void setAccGeo (double ae, double an, double au) {
    this.ae = ae; this.an = an; this.au = au;
  }

   /** Set the acceleration (m/s^2). This is identical to <code>setAccGeo(acc[0],acc[1],acc[2])</code>.
       @param acc velocity { ae, an, au }
       @since NeXtMidas 2.9.2
   */
  public void setAccGeo (double[] acc) {
    setAccGeo(acc[0], acc[1], acc[2]);
  }

  /** As of NeXtMidas 2.9.2, keys array contains velocity/acceleration North, East, and Up values,
      added to end of array for backwards-compatibility. */
  private static String[] keys = {"X","Y","Z","VX","VY","VZ","AX","AY",
    "AZ","R","T","LAT","LON","ALT","FRAME", "VE", "VN", "VU", "AE", "AN", "AU"};

  @Override
  public String[] getKeys () { return keys; }

  @Override
  public Object setKey (String name, Object value) {
         if (name.equals("VX")) setVX(Convert.o2d(value));
    else if (name.equals("VY")) setVY(Convert.o2d(value));
    else if (name.equals("VZ")) setVZ(Convert.o2d(value));
    else if (name.equals("VE")) setVE(Convert.o2d(value));
    else if (name.equals("VN")) setVN(Convert.o2d(value));
    else if (name.equals("VU")) setVU(Convert.o2d(value));
    else if (name.equals("AX")) setAX(Convert.o2d(value));
    else if (name.equals("AY")) setAY(Convert.o2d(value));
    else if (name.equals("AZ")) setAZ(Convert.o2d(value));
    else if (name.equals("AE")) setAE(Convert.o2d(value));
    else if (name.equals("AN")) setAN(Convert.o2d(value));
    else if (name.equals("AU")) setAU(Convert.o2d(value));
    else return super.setKey(name,value);
    return value;
  }

  @Override
  public Object getKey (String name) {
         if (name.equals("VX")) return Convert.d2o(getVX());
    else if (name.equals("VY")) return Convert.d2o(getVY());
    else if (name.equals("VZ")) return Convert.d2o(getVZ());
    else if (name.equals("VE")) return Convert.d2o(getVE());
    else if (name.equals("VN")) return Convert.d2o(getVN());
    else if (name.equals("VU")) return Convert.d2o(getVU());
    else if (name.equals("AX")) return Convert.d2o(getAX());
    else if (name.equals("AY")) return Convert.d2o(getAY());
    else if (name.equals("AZ")) return Convert.d2o(getAZ());
    else if (name.equals("AE")) return Convert.d2o(getAE());
    else if (name.equals("AN")) return Convert.d2o(getAN());
    else if (name.equals("AU")) return Convert.d2o(getAU());
    else return super.getKey(name);
  }

  /** Interpolates position, velocity and acceleration vectors between two measurement points.
      This is identical to calling:
      <code>interpolate(p0,v0,a0,t0, p1,v1,a1,t1, p,v,a,t, dim, InterpolationOrder.Cubic)</code>
      <i>Note that this explicitly uses {@link InterpolationOrder#CubicHermite} to match
      the behavior of pre-NeXtMidas 3.3.0 versions.</i> <br>
      <br>
      <i>Prior to NeXtMidas 3.3.2 some well-known uses of this function erroneously
      used time epochs that were not measured in seconds causing incorrect scaling of
      some of the higher-order terms. Any users who may have used these
      functions as "reference material" are advised to re-check their usage.</i>
      @param p0  (IN)  Position vector at t0.
      @param v0  (IN)  Velocity vector at t0.
      @param a0  (IN)  Acceleration vector at t0.
      @param t0  (IN)  Time of initial measurement.
      @param p1  (IN)  Position vector at t1.
      @param v1  (IN)  Velocity vector at t1.
      @param a1  (IN)  Acceleration vector at t1.
      @param t1  (IN)  Time of final measurement.
      @param p   (OUT) Position vector at t.
      @param v   (OUT) Velocity vector at t.
      @param a   (OUT) Acceleration vector at t.
      @param t   (IN)  Time between t0 and t1 to solve for.
      @param dim (IN)  Dimensionality of the input and output vectors
                       (<code>1={x}</code>, <code>2={x,y}</code>, or <code>3={x,y,z}</code>).
      @throws IllegalArgumentException If <code>t0 &gt; t1</code>.
      @throws NullPointerException If any of the arrays is null.
      @throws ArrayIndexOutOfBoundsException If the length of any array is less than <code>dim</code>.
   */
  public static void interpolate (double[] p0, double[] v0, double[] a0, double t0,
                                  double[] p1, double[] v1, double[] a1, double t1,
                                  double[] p,  double[] v,  double[] a,  double t,  int dim) {
    interpolate(p0,v0,a0,t0, p1,v1,a1,t1, p,v,a,t, dim, InterpolationOrder.CubicHermite);
  }

  /** Interpolates position, velocity and acceleration vectors between two measurement points.
      The values are calculated using a Linear, Cubic Hermite Spline or Quintic Hermite Spline
      (see <a href="http://en.wikipedia.org/wiki/Cubic_Hermite_spline">
      http://en.wikipedia.org/wiki/Cubic_Hermite_spline</a> and
      <a href="www.rose-hulman.edu/~finn/CCLI/Notes/day09.pdf">
      www.rose-hulman.edu/~finn/CCLI/Notes/day09.pdf</a>).
      <br>
      <br>
      The computation for position via the Hermite Splines is as follows (velocity and
      acceleration are similar):
      <pre>
        Subinterval:
          t = time normalized to a range of (0,1)

        Cubic/Quintic Hermite basis functions:
          h00(t) =  2t^3 - 3t^2 + 1           h0(t) =  -6 t^5 + 15 t^4 - 10 t^3 + 1
          h10(t) =   t^3 - 2t^2 + t           h1(t) =  -3 t^5 +  8 t^4 -  6 t^3 + t
          h01(t) = -2t^3 + 3t^2               h2(t) = -0.5t^5 + 1.5t^4 - 1.5t^3 + 0.5t^2
          h11(t) =   t^3 -  t^2               h3(t) =  0.5t^5 -    t^4 + 0.5t^3
                                              h4(t) =  -3 t^5 +  7 t^4 -  4 t^3
                                              h5(t) =   6 t^5 - 15 t^4 + 10 t^3

        Polynomials:
          p(t) = h00(t)*p0 + h11(t)*v0 + h01(t)*p1 + h11(t)*v1

          p(t) = h0(t)*p0 + h1(t)*v0 + h2(t)*a0 + h3(t)*a1 + h4(t)*v1 + h5(t)*p1

          where v0 and v1 are taken to be the tangent vectors at t0 and t1.
      </pre>
      Since jerk (derivative of acceleration over time) is not specified, it is assumed to be
      linear between <code>t0</code> and <code>t1</code>. If acceleration is not given, it is assumed
      to be linear <code>t0</code> and <code>t1</code>. If velocity is not given, it is assumed
      to be linear <code>t0</code> and <code>t1</code>. <br>
      <br>
      This method is for interpolation only. If given a value of <code>t &lt;= t0</code>, or a value of
      <code>t &gt;= t1</code> the values at <code>t0</code> or <code>t1</code> are simply returned. without any
      error or warning (this is specifically done do avoid issues that may be caused by rounding
      errors when <code>t</code> is extremely close to <code>t0</code> or <code>t1</code>). <br>
      <br>
      Note that it is an error to give one acceleration input or one velocity input and not the
      others. It is also an error to give acceleration with no velocity. <br>
      <br>
      The time values given may either be in J1950 time or may be in seconds relative to some
      arbitrary epoch. In general, if time values are available it is advisable to use the
      version of this function which takes in {@link Time} values as doing so will minimize
      the loss of precision that would otherwise accompany calling this function using the
      results from {@link Time#getSec()}.
      @param p0    (IN)  Position vector at t0.
      @param v0    (IN)  Velocity vector at t0.     (null if unknown)
      @param a0    (IN)  Acceleration vector at t0. (null if unknown)
      @param t0    (IN)  Time of initial measurement (in sec).
      @param p1    (IN)  Position vector at t1.
      @param v1    (IN)  Velocity vector at t1.     (null if unknown)
      @param a1    (IN)  Acceleration vector at t1. (null if unknown)
      @param t1    (IN)  Time of final measurement (in sec).
      @param p     (OUT) Position vector at t.
      @param v     (OUT) Velocity vector at t.
      @param a     (OUT) Acceleration vector at t.
      @param t     (IN)  Time (in sec) between t0 and t1 to solve for.
      @param dim   (IN)  Dimensionality of the input and output vectors
                         (<code>1={x}</code>, <code>2={x,y}</code>, or <code>3={x,y,z}</code>).
      @param order (IN)  The order of the interpolation (null=use "best available",
                         note that the "best available" may change between releases).
      @throws IllegalArgumentException If <code>t0 &gt; t1</code>.
      @throws NullPointerException If any of the arrays is null.
      @throws ArrayIndexOutOfBoundsException If the length of any array is less than <code>dim</code>.
      @since  NeXtMidas 3.3.0
   */
  public static void interpolate (double[] p0, double[] v0, double[] a0, double t0,
                                  double[] p1, double[] v1, double[] a1, double t1,
                                  double[] p,  double[] v,  double[] a,  double t,
                                  int dim, InterpolationOrder order) {
    // ERROR CHECKING
    if ((t0 > t1)) throw new IllegalArgumentException("Given value of t0="+t0+" exceeds t1="+t1);
    if ((p0 == null) || (p1 == null)) throw new NullPointerException("Position vector can not be null");
    if ((v0 == null) != (v1 == null)) throw new NullPointerException("One of the velocity vectors was null");
    if ((a0 == null) != (a1 == null)) throw new NullPointerException("One of the velocity vectors was null");
    if ((v0 == null) && (a0 != null)) throw new NullPointerException("Velocity vector null, but acceleration given");

    // HANDLE SPECIAL CASES
    if (t <= t0) {
      System.arraycopy(p0, 0, p, 0, dim);
      System.arraycopy(v0, 0, v, 0, dim);
      System.arraycopy(a0, 0, a, 0, dim);
      return;
    }
    if (t >= t1) {
      System.arraycopy(p1, 0, p, 0, dim);
      System.arraycopy(v1, 0, v, 0, dim);
      System.arraycopy(a1, 0, a, 0, dim);
      return;
    }

    if (order == null) {
      order = InterpolationOrder.QuinticHermite;
    }

    // INTERPOLATION BETWEEN t0 AND t1 NORMALIZED TO A RANGE OF [0,1]
    double t1t0 = t1 - t0;   // only compute (t1-t0) once since it is reused several times
    double dt   = t  - t0;   // time delta
    double tt   = dt / t1t0; // (t - t0) / (t1 - t0);

    if (order == InterpolationOrder.Linear) {
      if (v0 == null) {                                    // POS ONLY
        for (int i = 0; i < dim; i++) {
          p[i] = p0[i] + (p1[i] - p0[i]) * tt;
        }
      }
      else if (a0 == null) {                               // POS+VEL ONLY
        for (int i = 0; i < dim; i++) {
          v[i] = v0[i] + (v1[i] - v0[i]) * tt;
          p[i] = p0[i] + (p1[i] - p0[i]) * tt;
        }
      }
      else {                                               // POS+VEL+ACC
        for (int i = 0; i < dim; i++) {
          a[i] = a0[i] + (a1[i] - a0[i]) * tt;
          v[i] = v0[i] + (v1[i] - v0[i]) * tt;
          p[i] = p0[i] + (p1[i] - p0[i]) * tt;
        }
      }
    }
    else if (order == InterpolationOrder.CubicHermite) {
      double tt2 = tt * tt;   // tt^2
      double tt3 = tt * tt2;  // tt^3

      // Hermite Basis functions
      double h00 =  2*tt3 - 3*tt2 + 1;
      double h10 =    tt3 - 2*tt2 + tt;
      double h01 = -2*tt3 + 3*tt2;
      double h11 =    tt3 -   tt2;

      if (v0 == null) {                                    // POS ONLY
        for (int i = 0; i < dim; i++) {
          double vel_t1t0 = p1[i] - p0[i]; // vel*t1t0
          p[i] = h00*p0[i] + h10*vel_t1t0   + h01*p1[i] + h11*vel_t1t0;
        }
      }
      else if (a0 == null) {                               // POS+VEL ONLY
        for (int i = 0; i < dim; i++) {
          double acc_t1t0 = v1[i] - v0[i]; // acc*t1t0
          v[i] = h00*v0[i] + h10*acc_t1t0   + h01*v1[i] + h11*acc_t1t0;
          p[i] = h00*p0[i] + h10*v0[i]*t1t0 + h01*p1[i] + h11*v1[i]*t1t0;
        }
      }
      else {                                               // POS+VEL+ACC
        for (int i = 0; i < dim; i++) {
          double jerk_t1t0 = a1[i] - a0[i]; // jerk*t1t0
          a[i] = h00*a0[i] + h10*jerk_t1t0  + h01*a1[i] + h11*jerk_t1t0;
          v[i] = h00*v0[i] + h10*a0[i]*t1t0 + h01*v1[i] + h11*a1[i]*t1t0;
          p[i] = h00*p0[i] + h10*v0[i]*t1t0 + h01*p1[i] + h11*v1[i]*t1t0;
        }
      }
    }
    else if (order == InterpolationOrder.QuinticHermite) {
      double tt2 = tt * tt;   // tt^2
      double tt3 = tt * tt2;  // tt^3
      double tt4 = tt * tt3;  // tt^4
      double tt5 = tt * tt4;  // tt^5

      // Hermite Basis functions
      double h0 = -6.0*tt5 + 15.0*tt4 - 10.0*tt3 + 1;
      double h1 = -3.0*tt5 +  8.0*tt4 -  6.0*tt3 + tt;
      double h2 = -0.5*tt5 +  1.5*tt4 -  1.5*tt3 + 0.5*tt2;
      double h3 =  0.5*tt5 -      tt4 +  0.5*tt3;
      double h4 = -3.0*tt5 +  7.0*tt4 -  4.0*tt3;
      double h5 =  6.0*tt5 - 15.0*tt4 + 10.0*tt3;

      if (v0 == null) {                                    // POS ONLY
        for (int i = 0; i < dim; i++) {
          double vel_t1t0 = p1[i] - p0[i]; // vel*t1t0
          p[i] = h0*p0[i] + h1*vel_t1t0   + 0             + 0             + h4*vel_t1t0   + h5*p1[i];
        }
      }
      else if (a0 == null) {                               // POS+VEL ONLY
        for (int i = 0; i < dim; i++) {
          double acc_t1t0 = v1[i] - v0[i]; // acc*t1t0
          v[i] = h0*v0[i] + h1*acc_t1t0   + 0             + 0             + h4*acc_t1t0   + h5*v1[i];
          p[i] = h0*p0[i] + h1*v0[i]*t1t0 + h2*acc_t1t0   + h3*acc_t1t0   + h4*v1[i]*t1t0 + h5*p1[i];
        }
      }
      else {                                               // POS+VEL+ACC
        for (int i = 0; i < dim; i++) {
          double jerk_t1t0 = a1[i] - a0[i]; // jerk*t1t0
          a[i] = h0*a0[i] + h1*jerk_t1t0  + 0             + 0             + h4*jerk_t1t0  + h5*a1[i];
          v[i] = h0*v0[i] + h1*a0[i]*t1t0 + h2*jerk_t1t0  + h3*jerk_t1t0  + h4*a1[i]*t1t0 + h5*v1[i];
          p[i] = h0*p0[i] + h1*v0[i]*t1t0 + h2*a0[i]*t1t0 + h3*a1[i]*t1t0 + h4*v1[i]*t1t0 + h5*p1[i];
        }
      }
    }
    else {
      throw new AssertionError("Unknown InterpolationOrder "+order);
    }
  }

  /** Interpolates position, velocity and acceleration vectors between two measurement points.
      This is identical to:
      <code>interpolate(p0,v0,a0,0.0,p1,v1,a1,t1.diff(t0),p,v,a,t.diff(t0),dim,order)</code>.
      @param p0    (IN)  Position vector at t0.
      @param v0    (IN)  Velocity vector at t0.     (null if unknown)
      @param a0    (IN)  Acceleration vector at t0. (null if unknown)
      @param t0    (IN)  Time of first measurement point (in sec).
      @param p1    (IN)  Position vector at t1.
      @param v1    (IN)  Velocity vector at t1.     (null if unknown)
      @param a1    (IN)  Acceleration vector at t1. (null if unknown)
      @param t1    (IN)  Time of second measurement point (in sec).
      @param p     (OUT) Position vector at t.
      @param v     (OUT) Velocity vector at t.
      @param a     (OUT) Acceleration vector at t.
      @param t     (IN)  Time (in sec) time for which to retrieve the position
      @param dim   (IN)  Dimensionality of the input and output vectors
                         (<code>1={x}</code>, <code>2={x,y}</code>, or <code>3={x,y,z}</code>).
      @param order (IN)  The order of the interpolation (null=use "best available",
                         note that the "best available" may change between releases).
      @since NeXtMidas 3.3.0
   */
  public static void interpolate (double[] p0, double[] v0, double[] a0, Time t0,
                                  double[] p1, double[] v1, double[] a1, Time t1,
                                  double[] p,  double[] v,  double[] a,  Time t,
                                  int dim, InterpolationOrder order) {
    interpolate(p0,v0,a0,0.0,p1,v1,a1,t1.diff(t0),p,v,a,t.diff(t0),dim,order);
  }

  /** Computes statistics about two state vectors. This is identical to using
      <code>computeMeasurements(a, b, rf, tbl, shortNames, null, -1)</code>.
      @param a          The first state vector.
      @param b          The second state vector.
      @param rf         The RF to use for Doppler computation.
      @param tbl        The table to put the results in (null = create new table).
                        If a table is provided, entries will be added to it, but
                        the table will not otherwise be cleared (this permits the
                        inclusion of additional pre-computed statistics, though
                        at the risk of them being over-written).
      @param shortNames Include the short names in the table? (The long names are
                        always included.)
      @return <code>tbl</code> or a new table if <code>tbl</code> was null.
      @since NeXtMidas 3.3.0
   */
  public static Table computeMeasurements (Position a, Position b, double rf, Table tbl, boolean shortNames) {
    return computeMeasurements(a, b, rf, tbl, shortNames, null, -1);
  }

  /** Computes statistics about two state vectors. The statistics computed here
      are similar to those computed by the X-Midas <code>DUAL5000</code> command (see
      <code>DUAL5000;UCL</code> in NeXtMidas), but are done at a single point-in-time
      for the given state vectors. <br>
      <br>
      Since NeXtMidas 3.3.2 this function also supports the option for a
      light-time correction which is not present in the X-Midas version (see
      below).<br>
      <br>
      As part of this computation, the state vectors will be converted to
      CARTESIAN (see {@link #NORMAL}) mode by calling {@link #geo2car} if
      in {@link #GEODETIC} mode.<br>
      <br>
      The statistics computed:
      <pre>
        Long Name        Short Name   Description            Units
        -----------------------------------------------------------
        RANGE            R            Range                  meters
        RANGE_RATE       RR           Range Rate             m/s
        DELTA_T          DELT         Path Delay (delta T)   sec
        DELTA_F          DELF         Doppler (delta F)      Hz
        SPREAD_FACTOR    SPRF         Spread Factor          --
        AZIMUTH_ANGLE    AZ           Azimuth Angle          deg
        ELEVATION_ANGLE  EL           Elevation Angle        deg

        Notes: 1. The short names match those used by X-Midas in DUAL5000.
               2. Range and Range Rate assume direct point-to-point
               3. Path delay computation assumes {@link #SPEED_OF_LIGHT}.
               4. Relativistic effects are assumed to be negligible and are
                  not factored in
               5. When RANGE=0.0:
                     SPREAD_FACTOR=Inf   (same as X-Midas)
                     AZIMUTH_ANGLE=0.0   (same as X-Midas)
                     ELEVATION_ANGLE=0.0 (X-Midas gives NaN)
               6. AZIMUTH_ANGLE and ELEVATION_ANGLE are given with respect to
                  the first state vector's reference frame
      </pre>
      <b>No Light-Time Correction: (<code>time == null</code>)</b><br>
      <br>
      The state vectors are assumed to have their time set as appropriate. This
      function explicitly does not compare the time values, on the assumption
      that differing time values may be intentional. This is particularly true
      in the situation where a moving object is compared to a fixed observer.
      The possibility of a fixed observer is also for this reason that
      {@link Position} is used for the input rather than {@link StateVector}.<br>
      <br>
      <b>With Light-Time Correction: (<code>time != null</code>)</b><br>
      <br>
      Any current time setting for the state vectors will be discarded and both
      will have their current time set to <code>time</code>. If the resulting values
      indicate a light-time delay outside of the given tolerance (<code>tol</code>)
      <i>[which is frequently the case]</i> the first state will then have its
      time adjusted <i>[moved back in time]</i> until the predicted light-time delay
      is equal to the actual delay returned (within the given tolerance). Note
      that this implicitly assumes that <code>a</code> represents the transmitter and
      <code>b</code> represents the receiver.<br>
      <br>
      <b>Illustration</b><br>
      <br>
      T - transmitter<br>
      R - receiver<br>
      Range Rate (velocity at which the transmitter and receiver are separating
      from each other).<br>
      <br>
      Note: Time is referenced to the receiver time. So without LTC, timeB would
      be used for both the receiver and transmitter state vector information
      (pos,vel,time)<br>
      <br>
      Case 1 - Range rate is negative (Transmitter and Receiver getting closer to each other)<br>
      <pre>
      T(a) ---&gt; T(b)
       \) elevation angle
        \
         \
          \
           \
            \
             \
              \ t(delta) = Light Time Delay
               \
                \
                 \
                  \
                   \
                    \
                     \
                      \
                       R(b)
      </pre>
      - With LTC, the transmit time "timeA" is adjusted so that timeA + Light_Time_Delay
        = timeB (within tolerance)<br>
      - The actual Light Time Delay will be a little greater than the initial T(b) to R(b) estimate<br>
      <br>
      Case 2 - Range rate is positive (Transmitter and Receiver are getting futher away from each other)
      - The actual Light Time Delay will be a little less than the initial T(b) to R(b) estimate
      (figure not shown)<br>
      <br>
      @param a          The first  state vector (i.e. the transmitter).
      @param b          The second state vector (i.e. the receiver).
      @param rf         The RF to use for Doppler computation.
      @param tbl        The table to put the results in (null = create new table).
                        If a table is provided, entries will be added to it, but
                        the table will not otherwise be cleared (this permits the
                        inclusion of additional pre-computed statistics, though
                        at the risk of them being over-written).
      @param shortNames Include the short names in the table? (The long names are
                        always included.)
      @param time       The current time to use for <code>b</code>. Set this to
                        <code>null</code> to skip any light-time corrections (see
                        above).
      @param tol        The tolerance for any light-time corrections in seconds.
                        Currently 1 ps is the minimum supported and any values
                        smaller than this will be increased to 1 ps (to avoid an
                        infinite loop due to floating-point rounding errors).
                        All values less than 0.0 are reserved; with the following
                        defined:
                        <pre>
                          -1  - Use default tolerance (1 us).
                          -2  - Disable light-time correction, but still set the
                                time for the two state vectors to <code>time</code>.
                        </pre>
      @return <code>tbl</code> or a new table if <code>tbl</code> was null.
      @since NeXtMidas 3.3.2
   */
  public static Table computeMeasurements (Position a, Position b, double rf,
                                           Table tbl, boolean shortNames,
                                           Time time, double tol) {
    if (tbl == null) tbl  = new Table();
    if (tol ==   -1) tol  = 0.000001;        // default value (corrected 3.5.2 Bug 2866, 2871)
    if (tol !=   -2) tol  = max(tol, 1e-12); // prevent tol from being too close to 0.0 (corrected 3.5.2 Bug 2866, 2871)

    Time   timeA = (time == null)? null     : new Time(time);
    Time   timeB = (time == null)? null     : time;
    double dpx;
    double dpy;
    double dpz;
    double range;
    double delay;
    double lastStep = 0; // Only 0 on first pass

    // Receiver is the reference for Time, get receiver position at current Time
    // (receive time)
    if (time != null) {
      b.setCurrentTime(timeB);  // Since 3.5.3 Bug 2866 - set outside loop since not updated
    }

    while (true) {
      // Get Transmitter position for this transmit Time.
      // 1) If LTC correction is NOT enabled, the position at receive time will
      //    be used for "transmit time" and the light time delay ignored.
      // 2) If LTC correction is ENABLED, the code will adjust this time backwards
      //    to the proper transmit time to match this receive time (within tol)
      //    (i.e. timeA + light_time_delay = timeB, where timeA is transmit time
      //    and timeB is receive time)
      if (time != null) {
        a.setCurrentTime(timeA);  //only keep updating transmission time - Since 3.5.3
      }

      if (a.referenceFrame == GEODETIC) a.geo2car();
      if (b.referenceFrame == GEODETIC) b.geo2car();

      dpx   = b.getX()  - a.getX(); // difference in position
      dpy   = b.getY()  - a.getY();
      dpz   = b.getZ()  - a.getZ();
      range = sqrt(dpx*dpx + dpy*dpy + dpz*dpz);
      delay = range*SPEED_OF_LIGHT_INV;

      if ((time == null) || (tol == -2)) break; // no light-time correction

      double dt  = timeB.diff(timeA);
      double err = dt - delay;

      if ((err >= -tol) && (err <= +tol)) break; // within tolerance  (corrected 3.5.2 Bug 2866, 2871)

      double step;
      // For the first step estimate the full light time delay.  This is a good
      // initial estimate as the total step needed could be greater if the
      // transmitter is getting closer or smaller if the transmitter is getting
      // further away                       (Since NeXtMidas 3.5.3 - Bug 2866)
      if (lastStep == 0){
        step = err;
      }
      // For future steps, if needed, take a more gradual approach
      else if (err > 0) {
        step = max(err/2, tol/2);

        if ((lastStep < 0) && (step > -lastStep/2)) {
          // Went too far last time, so make sure we get closer
          step = -lastStep/2;
        }
      }
      else {
        step = min(err/2, -tol/2);

        if ((lastStep > 0) && (step < -lastStep/2)) {
          // Went too far last time, so make sure we get closer
          step = -lastStep/2;
        }
      }
      lastStep = step;
      timeA.addSec(step);
    }

    double dvx        = b.getVX() - a.getVX();  // difference in velocity
    double dvy        = b.getVY() - a.getVY();
    double dvz        = b.getVZ() - a.getVZ();
    double rangeRate  = (range == 0)? sqrt(dvx*dvx + dvy*dvy + dvz*dvz)
                                    :     (dpx*dvx + dpy*dvy + dpz*dvz)/range;
    double doppler    = rangeRate*rf*SPEED_OF_LIGHT_INV;
    double spreadFact = FOUR_PI_INV/(range*range);
    double azimuth    = atan2(dpy,dpx) * RAD2DEG;
    double elevation  = (range == 0)? 0.0 : (asin(dpz/range) * RAD2DEG);

    tbl.put("RANGE",           range);
    tbl.put("RANGE_RATE",      rangeRate);
    tbl.put("DELTA_T",         delay);
    tbl.put("DELTA_F",         doppler);
    tbl.put("SPREAD_FACTOR",   spreadFact);
    tbl.put("AZIMUTH_ANGLE",   azimuth);
    tbl.put("ELEVATION_ANGLE", elevation);

    if (shortNames) {
      tbl.put("R",    range);
      tbl.put("RR",   rangeRate);
      tbl.put("DELT", delay);
      tbl.put("DELF", doppler);
      tbl.put("SPRF", spreadFact);
      tbl.put("AZ",   azimuth);
      tbl.put("EL",   elevation);
    }
    return tbl;
  }

  /** Adds a keyword value to a map with type="D:" and given value and units. */
  private void putWithUnits (Map<String,Object> kwds, String key, double val, int units) {
    putWithUnits(kwds, 'D', key, new Data(val), units);
  }

  /** Adds a keyword value to a map with given type, value, and units. */
  private void putWithUnits (Map<String,Object> kwds, char type, String key, Data val, int units) {
    kwds.put(type+":"+key,          val);
    kwds.put(    "B:"+key+".UNITS", new Data(units));
  }

  /** Helper to get the current time, if it is null use "TIME" value.
   *  If the value for "TIME" is less than a year, add start time and "TIME" values to get the J1950 time.
   *  @param Table e the table that contains TIME or CURRENTTIME
   *  @return the CURRENTTIME if available, otherwise TIME
   */
  private Time getCurrentTime (Table e) {
    Time time = Time.toTime(e.get("CURRENTTIME"));
    if (time == null) {
      return convertTimeToCurrentTime(Time.toTime(e.get("TIME")));
    }
    return convertTimeToCurrentTime(time);
  }

  /** This function will assume that a Time object that lasts less than one year is a time since start. Then it will
   *  convert it to a current time since 1950 based on the time at the start of the file.
   *  @param t The time object to convert
   *  @return The current time
   */
  private Time convertTimeToCurrentTime (Time t) {
    if (t.getSec() < Time.SECONDS_PER_YEAR) {// The time is since the start so we need to add it to the start time to get the current time
      return new Time(t.getSec() + getStartTime().getSec());
    }
    else {
      return new Time(t.getSec());
    }
  }
}
