package nxm.sys.lib;

import static nxm.sys.inc.Units.CARTESIAN;
import static nxm.sys.inc.Units.CYLINDRIC;
import static nxm.sys.inc.Units.ELLIPSOID;
import static nxm.sys.inc.Units.GEODETIC;
import static nxm.sys.inc.Units.SPHERICAL;

import java.util.Arrays;

import nxm.sys.inc.Constants;
import nxm.sys.inc.InternalUseOnly;
import nxm.sys.inc.Tablizable;
import nxm.sys.inc.Units;

/** A class to handle reference frame and coordinate system transformations. <br>
    <br>
    Reference Frames:
    <pre>
      {@link #NONE}     - <i>No reference frame specified.</i>
      {@link #ECR}      - <b>Earth-centered rotational (ECR)</b>, which has x and y in the equatorial plane at
                 0 and 90 degrees east longitude, respectively, and z north along the
                 earth's rotational axis. This is the same as the Earth-Centered, Earth-Fixed (ECEF or ECF)
                 Cartesian coordinate system.
      {@link #ECI}      - <b>Earth-centered inertial (ECI)</b>, which has x toward the vernal equinox, y 90
                 degrees east of x, and z north along the earth's rotational axis.
      {@link #TOP}      - <b>Modified topocentric</b>, with x due east in the local horizon plane, y
                 north in the horizon plane, and z perpendicular to the horizon plane
                 in the upward direction.
      {@link #TOPO}     - <b>Topocentric</b>, which has x due south in the local horizon plane, y
                 east in the horizon plane, and z perpendicular to the horizon plane
                 in the upward direction.
      TOPOCENT - <i>Same as TOPO</i>
    </pre>
    Coordinate Systems (SI units):
    <pre>
      {@link Units#CARTESIAN}  - { x, y, z } in meters
      {@link Units#SPHERICAL}  - { r, a, b }
      {@link Units#CYLINDRIC}  - <i>Not Supported in Transform</i>
      {@link Units#ELLIPSOID}  - <i>Not Supported in Transform</i>
      {@link Units#GEODETIC}   - { alt, lat, lon } alt in meters above sea level, lat in deg, lon in deg

      ALL (AltLatLon) - GEODETIC parameters in { alt, lat, lon } order (used in files/Transform)
      LLA (LonLatAlt) - GEODETIC parameters in { lon, lat, alt } order (used in PLOT)
    </pre>

    @author Jeff Schoen / Neon Ngo
 */
public class Transform extends Object implements Constants {
  private static final double KNOTS2MS   = NM2METERS / 3600.0; // knots to m/s
  private static final double GFORCE2MS2 = 9.80665;            // 1g to m/s^2

  /** List of Reference Frame names. */
  public static final String referenceFrameList = "None,ECR,ECI,TOP,TOPO";

  /** Reference Frame NONE (see {@link Transform} for details. */
  public static final int NONE=1;

  /** Reference Frame ECR (see {@link Transform} for details. */
  public static final int ECR=2;

  /** Reference Frame ECI (see {@link Transform} for details. */
  public static final int ECI=3;

  /** Reference Frame TOP (see {@link Transform} for details. */
  public static final int TOP=4;

  /** Reference Frame TOPO or TOPOCENT (see {@link Transform} for details. */
  public static final int TOPO=5;

  /** List of Data Modes. */
  public static final String dataModeList = "Pos,PosVel,PosVelAcc";

  /** Data Mode: Position (POS) only. */
  public static final int POS=1;

  /** Data Mode: Position (POS) and Velocity (VEL). */
  public static final int POSVEL=2;

  /** Data Mode: Position (POS), Velocity (VEL) and Acceleration (ACC). */
  public static final int POSVELACC=3;

  // Internal use only: Common ECR transforms since they are constant (do not let users access!).
  private static final Transform Transform_ECR_CARTESIAN = new Transform(ECR, Units.CARTESIAN);
  private static final Transform Transform_ECR_GEODETIC  = new Transform(ECR, Units.GEODETIC);

  private       int    referenceFrame   = NONE;
  private       int    coordinateSystem = Units.CARTESIAN;
  private       int    unitsPos         = Units.DISTANCE_M;
  private       int    unitsVel         = Units.SPEED_M_S;
  private       int    unitsAcc         = Units.ACCELERATION_M_S2;
  private final Time   epoch            = new Time();
  private       double lat,lon,alt;
  private       double azim,elev,roll;

  private static final double INVERSE_SQRT_2 = 1 / Math.sqrt(2);
  private static final double INVERSE_EER = 1/EARTH_EQUATORIAL_RADIUS;
  private static final double EARTH_EQUATORIAL_RADIUS_SQUARED = EARTH_EQUATORIAL_RADIUS * EARTH_EQUATORIAL_RADIUS;
  private static final double EARTH_ECCENTRICITY_SQUARED = EARTH_ECCENTRICITY * EARTH_ECCENTRICITY;
  /** this is the same as (1 - EARTH_FLATENING_FACTOR) */
  private static final double POLAR_FLATTENING_FACTOR = Math.sqrt(1.0-EARTH_ECCENTRICITY_SQUARED);
  private static final double INVERSE_POLAR_FLATENING_FACTOR = 1 / POLAR_FLATTENING_FACTOR;
  private static final double EARTH_EQUATORIAL_RADIUS_PLUS = EARTH_EQUATORIAL_RADIUS + 10000;

  /** Use legacy clipAtEarth extend to horizon section */
  private static boolean useLegacyConeCode = false;
  /** Use Andrew's spherical earth based extend to horizon for points that are well off the horizon - same as patch */
  private static boolean useSphericalConeCode = false;
  /** Debug and test mode that allows visual comparisons of footprints created by different algorithms */
  private static boolean compareConeCode = false;

  /** Flag for using default cone code */
  @InternalUseOnly
  public static int CONE_DEFAULT = 0x01;
  /** Flag for using legacy cone code */
  @InternalUseOnly
  public static int CONE_LEGACY = 0x02;
  /** Flag for using sphere cone code - matches patch*/
  @InternalUseOnly
  public static int CONE_SPHERE = 0x04;
  static String compareConeList = "Default,Legacy,Sphere";
  static int coneCodeComparisons = 0x03; // default is default and legacy

  /** enable additional print statements related to horizon at altitude debug */
  private static boolean horizonAltDebug = false;

  /** Constructs a new instance, setting the referenceFrame/coordinateSystem.
      @param refFrame ECR, ECI or NONE. TOP or TOPO can be used, but only if
                      setting the lon,lat,alt (and azim,elev,roll).
      @param coordSys Coordinate system
   */
  public Transform (int refFrame, int coordSys) {
    referenceFrame   = refFrame;
    coordinateSystem = coordSys;
  }

  /** Constructs a new instance with reference frame {@link #NONE} and a {@link Units#SCALAR}
      coordinate system.
   */
  public Transform () {
    this(NONE, Units.SCALAR); // <-- Matches NeXtMidas 3.2.0 and earlier.
  }

  /** Constructs a new instance, setting the referenceFrame/coordinateSystem based a Type 5000
      file (see {@link #fromDataFile(DataFile)}).
      @param df The file.
   */
  public Transform (DataFile df) {
    this();
    fromDataFile(df);
  }

  @Override
  public boolean equals (Object obj) { // Since NeXtMidas 3.3.0
    if (!(obj instanceof Transform)) return false;
    Transform that = (Transform)obj;

    return (this.referenceFrame   == that.referenceFrame)
        && (this.coordinateSystem == that.coordinateSystem)
        && (this.unitsPos         == that.unitsPos)
        && (this.unitsVel         == that.unitsVel)
        && (this.unitsAcc         == that.unitsAcc)
        && (this.epoch.equals(that.epoch))
        && (this.lat              == that.lat)
        && (this.lon              == that.lon)
        && (this.alt              == that.alt)
        && (this.azim             == that.azim)
        && (this.elev             == that.elev)
        && (this.roll             == that.roll);
  }

  @Override
  public int hashCode () { // Since NeXtMidas 3.3.0
    return (this.referenceFrame        )
         ^ (this.coordinateSystem <<  4)
         ^ (this.unitsPos         <<  8)
         ^ (this.unitsVel         << 16)
         ^ (this.unitsAcc         << 24)
         ^ (this.epoch.hashCode()      )
         ^ Double.valueOf(this.lat ).hashCode()
         ^ Double.valueOf(this.lon ).hashCode()
         ^ Double.valueOf(this.alt ).hashCode()
         ^ Double.valueOf(this.azim).hashCode()
         ^ Double.valueOf(this.elev).hashCode()
         ^ Double.valueOf(this.roll).hashCode();
  }

  /** Describes the transform. */
  @Override
  public String toString() {
    String str = "Transform Ref="+getReferenceFrame()
               + " CoordSys="+getCoordinateSystem()
               + " PosUnits="+getPosUnits()
               + " VelUnits="+getVelUnits()
               + " AccUnits="+getAccUnits();
    switch (referenceFrame) {
      case TOP: str += " alt="+alt+" lat="+lat+" lon="+lon; break;
    }
    return str;
  }

  /** Gets the reference frame.
      @return The reference frame name.
      @since NeXtMidas 2.5.3
   */
  public String getReferenceFrame () {
    return Parser.get(referenceFrameList,referenceFrame);
  }

  /** Gets the coordinate system.
      @return The coordinate system name.
      @since NeXtMidas 2.5.3
   */
  public String getCoordinateSystem () {
    return Parser.get(Units.compTypeList,coordinateSystem);
  }

  /** Gets the units used for the POS. Note that within {@link Transform} only the
      <code>fsu2fsu(..)</code> functions use this value, all of the other
      functions assume base-SI units. <br>
      <br>
      When the {@link #getCoordinateSystem()} is {@link Units#GEODETIC} the POS
      units describe the units for the altitude component (e.g. {@link Units#DISTANCE_M}
      or {@link Units#ALTITUDE_M}) since the latitude and longitude components are
      automatically assumed to be in degrees.
      @return units used for the POS
      @since NeXtMidas 3.3.0
   */
  public String getPosUnits () {
    return DataFile.getUnitsVarName(unitsPos);
  }

  /** Gets the units used for the VEL. Note that within {@link Transform} only the
      <code>fsu2fsu(..)</code> functions use this value, all of the other
      functions assume base-SI units.
      @return units used for the VEL
      @since NeXtMidas 3.3.0
   */
  public String getVelUnits () {
    return DataFile.getUnitsVarName(unitsVel);
  }

  /** Gets the units used for the ACC. Note that within {@link Transform} only the
      <code>fsu2fsu(..)</code> functions use this value, all of the other
      functions assume base-SI units.
      @return units used for the ACC
      @since NeXtMidas 3.3.0
   */
  public String getAccUnits () {
    return DataFile.getUnitsVarName(unitsAcc);
  }

  /** Set referenceFrame/coordinateSystem based on the QuadWords of a Type 5000 file. The coordinate
      system is set based on that used for the <code>POS</code> component (if available), otherwise it
      is not set. <br>
      <br>
      Prior to NeXtMidas 3.3.0 this function would "fail fast" when the reference frame was invalid
      (i.e. not one of the ones defined in the BLUE 1.0 ICD), creating a scenario where legal coordinate
      system and/or units definitions were ignored. This caused a number of down-the-line issues for
      situations where users were using {@link Units#GEODETIC} coordinates for a POS (no VEL, ACC) but
      had a file that neglected to set the reference frame. In this situation the file was "technically
      illegal" but there was nothing preventing the values from being read correctly (since the reference
      frame was irrelevant in this situation). Since NeXtMidas 3.3.0, this function explicitly allows
      files of this form (without warning) and simply sets the reference frame to ECR.
      @param df The file.
      @return true if able to load the reference frame from the file, false otherwise.
   */
  public boolean fromDataFile (DataFile df) {
    // JC4: I think we should add a check here that returns false if df is not a Type 5000 file.

    // Set referenceFrame
    //   Prior to 3.3.0 the "else" case simply returned false. This caused a strange
    //   situation where a file with an invalid reference frame (e.g. "") but legal
    //   coordinateSystem and units did not get the coordinateSystem or units loaded
    //   properly.
    String frame = df.getReferenceFrame();
         if (frame.startsWith("ECR"))  referenceFrame = ECR;
    else if (frame.startsWith("ECI"))  referenceFrame = ECI;
    else if (frame.startsWith("TOPO")) referenceFrame = TOPO;
    else if (frame.startsWith("TOP"))  referenceFrame = TOP;
    else                               referenceFrame = NONE;

    // Load QuadWords
    alt        = df.getQuadword(DataFile.Q_ALT);
    lat        = df.getQuadword(DataFile.Q_LAT);
    lon        = df.getQuadword(DataFile.Q_LON);
    azim       = df.getQuadword(DataFile.Q_AZIM);
    elev       = df.getQuadword(DataFile.Q_ELEV);
    roll       = df.getQuadword(DataFile.Q_ROLL);
    //epoch_year = df.getQuadword(DataFile.Q_YEAR);
    //epoch_sec  = df.getQuadword(DataFile.Q_SEC);
    //epoch_gha  = df.getQuadword(DataFile.Q_GHA);
    epoch.fromQuadwords(df);

    // Set default units
    unitsPos = (df.getUnits() != Units.NONE)? df.getUnits() : Units.DISTANCE_M;
    unitsVel = Units.SPEED_M_S;
    unitsAcc = Units.ACCELERATION_M_S2;

    // Set coordinateSystem and units
    int iPos = df.findRec("POS");
    int iVel = df.findRec("VEL");
    int iAcc = df.findRec("ACC");
    if (iPos >= 0) {
      coordinateSystem = df.getCompType(iPos);
      unitsPos         = df.getCompUnits(iPos);
    }
    if (iVel >= 0) unitsVel = df.getCompUnits(iVel);
    if (iAcc >= 0) unitsAcc = df.getCompUnits(iAcc);

    if (referenceFrame == NONE) {
      if (frame.isEmpty() && (coordinateSystem == Units.GEODETIC)
                          && (iPos >= 0) && (iVel < 0) && (iAcc < 0)) {
        // This is a strange special-case where the reference frame was not given,
        // which is technically illegal under the BLUE 1.0 ICD, but since all we
        // have is POS and since POS is in GEODETIC, we don't actually need any
        // additional information. As such, we will just flag it as ECR and go
        // on without any warning.
        referenceFrame = ECR;
      }
    }

    return (referenceFrame != NONE);
  }

  /** Set quadwords of a Type 5000 file to match the referenceFrame defined here.
      Note that this does NOT set the coordinate system or units!
      @param df The file.
      @since NeXtMidas 3.3.0
   */
  public void toDataFile (DataFile df) {
    if (df.getTypeCodeClass() != 5) {
      throw new MidasException("Expected a Type 5000 file.");
    }

    // Set QuadWords
    if (referenceFrame == ECI) {
      df.setReferenceFrame("ECI");
      df.setQuadword(DataFile.Q_ALT,  0.0); // not used
      df.setQuadword(DataFile.Q_LAT,  0.0); // not used
      df.setQuadword(DataFile.Q_LON,  0.0); // not used
      df.setQuadword(DataFile.Q_AZIM, 0.0); // not used
      df.setQuadword(DataFile.Q_ELEV, 0.0); // not used
      df.setQuadword(DataFile.Q_ROLL, 0.0); // not used
      df.setQuadword(/* unused */  7, 0.0); // not used
      df.setQuadword(/* unused */  8, 0.0); // not used
      df.setQuadword(DataFile.Q_YEAR, epoch.getYear());
      df.setQuadword(DataFile.Q_SEC,  epoch.getSoY());
      df.setQuadword(DataFile.Q_GHA,  epoch.getGHA());
    }
    else if (referenceFrame == ECR) {
      df.setReferenceFrame("ECR");
      df.setQuadword(DataFile.Q_ALT,  0.0); // not used
      df.setQuadword(DataFile.Q_LAT,  0.0); // not used
      df.setQuadword(DataFile.Q_LON,  0.0); // not used
      df.setQuadword(DataFile.Q_AZIM, 0.0); // not used
      df.setQuadword(DataFile.Q_ELEV, 0.0); // not used
      df.setQuadword(DataFile.Q_ROLL, 0.0); // not used
      df.setQuadword(/* unused */  7, 0.0); // not used
      df.setQuadword(/* unused */  8, 0.0); // not used
      df.setQuadword(DataFile.Q_YEAR, 0.0); // not used
      df.setQuadword(DataFile.Q_SEC,  0.0); // not used
      df.setQuadword(DataFile.Q_GHA,  0.0); // not used
    }
    else if ((referenceFrame == TOP) || (referenceFrame == TOPO)) {
      if (referenceFrame == TOP) df.setReferenceFrame("TOP");
      else                       df.setReferenceFrame("TOPOCENT");
      df.setQuadword(DataFile.Q_ALT,  alt );
      df.setQuadword(DataFile.Q_LAT,  lat );
      df.setQuadword(DataFile.Q_LON,  lon );
      df.setQuadword(DataFile.Q_AZIM, azim);
      df.setQuadword(DataFile.Q_ELEV, elev);
      df.setQuadword(DataFile.Q_ROLL, roll);
      df.setQuadword(/* unused */  7, 0.0); // not used
      df.setQuadword(/* unused */  8, 0.0); // not used
      df.setQuadword(DataFile.Q_YEAR, 0.0); // not used
      df.setQuadword(DataFile.Q_SEC,  0.0); // not used
      df.setQuadword(DataFile.Q_GHA,  0.0); // not used
    }
    else {
      throw new MidasException("Unknown reference frame: "+referenceFrame);
    }
  }

  /** Converts an arbitrary object to a transform. Supported objects:
      <pre>
        null       - Returns null
        Transform  - Returns object passed in
        DataFile   - Returns 'new Transform((DataFile)obj)'
        Number     - New transform set to match {@link #NONE}, {@link #ECR},
                     {@link #ECI}, {@link #TOP} or {@link #TOPO} same as if
                     given the string form.
        Table      - **TBD**
        "NONE"     - New transform set to NONE
        "ECR"      - New transform set to ECR (epoch=0)
        "ECI"      - New transform set to ECI (epoch=0)
        "TOP"      - New transform set to TOP (epoch=0,lat=0,lon=0,alt=0)
        "TOPCENT"  - New transform set to TOPOCENT (epoch=0,lat=0,lon=0,alt=0,
                                                    azim=0,elev=0,roll=0)
        [other]    - IllegalArgumentException
      </pre>
      @param obj The object to convert
      @return The applicable transform.
      @since NeXtMidas 3.3.0
   */
  public static Transform toTransform (Object obj) {
    if (obj == null             ) return null;
    if (obj instanceof Transform) return (Transform)obj;
    if (obj instanceof DataFile ) return new Transform((DataFile)obj);

    if (obj instanceof Number) {
      switch (((Number)obj).intValue()) {
        case NONE: return new Transform(NONE, Units.CARTESIAN);
        case ECR:  return new Transform(ECR,  Units.CARTESIAN);
        case ECI:  return new Transform(ECI,  Units.CARTESIAN);
        case TOP:  return new Transform(TOP,  Units.CARTESIAN);
        case TOPO: return new Transform(TOPO, Units.CARTESIAN);
      }
    }
    else if (obj instanceof Tablizable) {
      // This is TBD
    }
    else {
      obj = obj.toString();
      if ("NONE".equals(obj)      ) return new Transform(NONE, Units.CARTESIAN);
      if ("ECR".equals(obj)       ) return new Transform(ECR,  Units.CARTESIAN);
      if ("ECI".equals(obj)       ) return new Transform(ECI,  Units.CARTESIAN);
      if ("TOP".equals(obj)       ) return new Transform(TOP,  Units.CARTESIAN);
      if ("TOPOCENT".equals(obj)  ) return new Transform(TOPO, Units.CARTESIAN);
    }
    throw new IllegalArgumentException("Could not convert "+obj+" to Transform");
  }

  /** Sets the transform in the {@link #TOP} reference frame and {@link Units#CARTESIAN} coordinate
      system. This is the same as <code>setTOP(alt,lat,lon,0,0,0)</code>.
      @param alt Altitude.
      @param lat Latitude (deg).
      @param lon Longitude (deg).
   */
  public final void setTOP (double alt, double lat, double lon) {
    setTOP(alt,lat,lon,0,0,0);
  }

  /** Sets the transform in the {@link #TOP} reference frame and {@link Units#CARTESIAN} coordinate
      system.
      @param alt  Altitude.
      @param lat  Latitude (deg).
      @param lon  Longitude (deg).
      @param azim The azimuth.
      @param elev The elevation.
      @param roll The roll.
   */
  public void setTOP (double alt, double lat, double lon,
                      double azim, double elev, double roll) {
    referenceFrame   = TOP;
    coordinateSystem = Units.CARTESIAN;
    this.alt = alt;
    this.lat = lat;
    this.lon = lon;
    this.azim = azim;
    this.elev = elev;
    this.roll = roll;
  }

  /** Changes reference frame to {@link #ECR} and coordinate system to
      {@link Units#CARTESIAN}. <br>
      <br>
      Prior to NeXtMidas 3.3.0 this function neglected to set the coordinate
      system resulting in undesirable behavior when users did:
      <pre>
        Transform t = new Transform(); // defaults to SCALAR not CARTESIAN or GEODETIC
        t.setECR();
      </pre>
   */
  public void setECR () {
    referenceFrame   = ECR;
    coordinateSystem = Units.CARTESIAN;
  }

  /** Changes reference frame to {@link #ECI} and coordinate system to
      {@link Units#CARTESIAN}.
      @param epoch The ECI epoch to use.
      @since NeXtMidas 3.3.0
   */
  public void setECI (Time epoch) {
    referenceFrame   = ECI;
    coordinateSystem = Units.CARTESIAN;
    setEpoch(epoch);
  }

  /** Sets the epoch and (indirectly) the GHA.
      @param epoch desired epoch
      @since NeXtMidas 3.3.0
   */
  public void setEpoch (Time epoch) {
    this.epoch.fromTime(epoch);
  }

  /** Convert a position from {@link Units#GEODETIC} to {@link Units#CARTESIAN}. No return value
      since the internal parameters of the input position are changed as a result of the conversion.
      @param p The position to convert.
   */
  public static void geo2car (Position p) {
    double f1,f2,rlat,rlon,slat,clat,slon,clon;
    f1 = 1-EARTH_FLATTENING_FACTOR;
    f2 = (2-EARTH_FLATTENING_FACTOR)*EARTH_FLATTENING_FACTOR;
    rlat = Math.atan(Math.tan(p.lat*DEG2RAD)*f1*f1);
    rlon = p.lon*DEG2RAD;
    slat = Math.sin(rlat);
    clat = Math.cos(rlat);
    slon = Math.sin(rlon);
    clon = Math.cos(rlon);
    p.r = (EARTH_EQUATORIAL_RADIUS*f1)/Math.sqrt(1-f2*clat*clat) + p.alt;
    p.x  = p.r*clat*clon;
    p.y  = p.r*clat*slon;
    p.z  = p.r*slat;

    // Take into account velocity north and east
    if (p instanceof StateVector) {
      StateVector sv = (StateVector) p;
      double[] v = new double[9];
      v[0] = sv.x;
      v[1] = sv.y;
      v[2] = sv.z;
      v[3] = sv.ve;
      v[4] = sv.vn;
      v[5] = sv.vu;
      v[6] = sv.ae;
      v[7] = sv.an;
      v[8] = sv.au;

      Transform t =  new Transform();
      t.setTOP(p.alt, p.lat, p.lon);
      top2ecef(v, POSVELACC, 1, t);

      sv.vx = v[3];
      sv.vy = v[4];
      sv.vz = v[5];
      sv.ax = v[6];
      sv.ay = v[7];
      sv.az = v[8];
    }
  }

  /** Converts a position from {@link Units#CARTESIAN} to {@link Units#GEODETIC}. No return value
      since the internal parameters of the input position are changed as a result of the conversion.
      @param p The position to convert.
   */
  public static void car2geo (Position p) {
    double f1,f2,a,ca;
    f1 = 1-EARTH_FLATTENING_FACTOR;
    f2 = (2-EARTH_FLATTENING_FACTOR)*EARTH_FLATTENING_FACTOR;
    p.r = Math.sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
    a = Math.asin( p.z/p.r );
    ca = Math.cos(a);
    p.alt = p.r-(EARTH_EQUATORIAL_RADIUS*f1)/Math.sqrt(1-f2*ca*ca);
    p.lat = RAD2DEG * Math.atan(Math.tan(a)/(f1*f1));
    p.lon = RAD2DEG * Math.atan2(p.y,p.x);

    // Take into account velocity north and east
    if (p instanceof StateVector) {
      StateVector sv = (StateVector) p;
      double[] v = new double[9];
      v[0] = sv.x;
      v[1] = sv.y;
      v[2] = sv.z;
      v[3] = sv.vx;
      v[4] = sv.vy;
      v[5] = sv.vz;
      v[6] = sv.ax;
      v[7] = sv.ay;
      v[8] = sv.az;

      Transform t =  new Transform();
      t.setTOP(p.alt, p.lat, p.lon);
      ecef2top(v, POSVELACC, 1, t);

      sv.ve = v[3];
      sv.vn = v[4];
      sv.vu = v[5];
      sv.ae = v[6];
      sv.an = v[7];
      sv.au = v[8];
    }
  }

  /** Calculate the great arc distance between two positions. Note that this method calls
      {@link #geo2car(Position)} on both <code>p</code> and <code>q</code> prior to computing the distance.
      @param p The 1st position must initially be in {@link Units#GEODETIC} mode.
      @param q The 2nd position must initially be in {@link Units#GEODETIC} mode.
      @return The great arc distance in meters.
   */
  public static double greatArc (Position p, Position q) {
    geo2car(p);
    geo2car(q);
    double r = 0.0;
    double r2 = p.r*q.r;
    if (r2>1e-20) r = (p.x*q.x+p.y*q.y+p.z*q.z)/r2;
    r = Math.min(1.0,Math.max(-1.0,r));
    r = Math.acos(r) * EARTH_EQUATORIAL_RADIUS;
    return r;
  }

  /** Convert data from {@link Units#SPHERICAL} to {@link Units#CARTESIAN} coordinate system. There
      is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
   */
  public static void sph2car (double[] v, int mpva, int nrec) {
    double ca,cb,sa,sb,r,a,b,rr,aa,bb,ri,rrr,aaa,bbb,rri;
    int isk = 3*mpva;
    for (int i=0,n=0; n<nrec; n++,i+=isk) {
      r = v[i+0];
      a = v[i+1]*DEG2RAD;
      b = v[i+2]*DEG2RAD;
      ca = Math.cos(a);  sa = Math.sin(a);
      cb = Math.cos(b);  sb = Math.sin(b);
      v[i+0] = r*cb*ca;
      v[i+1] = r*cb*sa;
      v[i+2] = r*sb;
      if (mpva<=1) continue;
      rr = v[i+3];
      aa = v[i+4]*DEG2RAD;
      bb = v[i+5]*DEG2RAD;
      ri = rr/r;
      v[i+3] = v[i+0]*ri - v[i+2]*bb*ca - v[i+1]*aa;
      v[i+4] = v[i+1]*ri - v[i+2]*bb*sa + v[i+0]*aa;
      v[i+5] = v[i+2]*ri + r*bb*cb;
      if (mpva<=2) continue;
      rrr = v[i+6];
      aaa = v[i+7]*DEG2RAD;
      bbb = v[i+8]*DEG2RAD;
      rri = rrr/r;
      v[i+6] = (v[i+3]-v[i+0]*ri)*ri + v[i+0]*rri - (v[i+5]*bb + v[i+2]*bbb)*ca
                        + v[i+2]*bb*aa*sa - v[i+1]*aaa - v[i+4]*aa;
      v[i+7] = (v[i+4]-v[i+1]*ri)*ri + v[i+1]*rri - (v[i+5]*bb + v[i+2]*bbb)*sa
                        - v[i+2]*bb*aa*ca - v[i+0]*aaa + v[i+3]*aa;
      v[i+8] = (v[i+5]-v[i+2]*ri)*ri + v[i+1]*rri + (rr*bb+r*bbb)*ca + v[i+2]*bb*bb;
    }
  }

  /** Convert data from {@link Units#GEODETIC} to ECR {@link Units#CARTESIAN} coordinate system. There
      is no return value since the values are changed within the array.
      @param v    Array of position vectors (alt, lat, lon)
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
   */
  public static void geo2car (double[] v, int mpva, int nrec) {
    double ca,cb,sa,sb,r,a,b,ee,t;
    ee = EARTH_ECCENTRICITY*EARTH_ECCENTRICITY;
    int isk = 3*mpva;
    for (int i=0,n=0; n<nrec; n++,i+=isk) {
      r = v[i+0];
      a = v[i+1]*DEG2RAD;
      b = v[i+2]*DEG2RAD;
      ca = Math.cos(a);  sa = Math.sin(a);
      cb = Math.cos(b);  sb = Math.sin(b);
      t = EARTH_EQUATORIAL_RADIUS/Math.sqrt(1-ee*sa*sa);
      v[i+0] = (r+t)*cb*ca;
      v[i+1] = (r+t)*ca*sb;
      v[i+2] = (r+(1-ee)*t)*sa;
      if (mpva<=1) continue;
    }
  }

  /** Lat/Lon to Cartesian conversion. Assumes an altitude of 0.
   *
   *  @param v     Array of (lat, lon) values
   *  @param ioff  offset into array of first pair
   *  @param npts  number of points
   *  @return array of Cartesian (ECEF) values
   */
  public static double[] lonlat2car (double[] v, int ioff, int npts) {
    double[] car = new double[npts*3];
    double ca,cb,sa,sb,r,a,b,ee,t;
    ee = EARTH_ECCENTRICITY*EARTH_ECCENTRICITY;
    for (int i=ioff,j=0,n=0; n<npts; n++, i+=2) {
      r = 0;
      a = v[i+1]*DEG2RAD;
      b = v[i]*DEG2RAD;
      ca = Math.cos(a);  sa = Math.sin(a);
      cb = Math.cos(b);  sb = Math.sin(b);
      t = EARTH_EQUATORIAL_RADIUS/Math.sqrt(1-ee*sa*sa);
      car[j++] = (r+t)*cb*ca;
      car[j++] = (r+t)*ca*sb;
      car[j++] = (r+(1-ee)*t)*sa;
    }
    return car;
  }

  /** <b>Not implemented yet:</b>
      Convert data from {@link Units#CYLINDRIC} to ECR {@link Units#CARTESIAN} coordinate system. There
      is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
   */
  public static void cyl2car (double[] v, int mpva, int nrec) {
    Shell.warning("Transform.cyl2car is not yet coded");
  }

  /** <b>Not implemented yet:</b>
      Convert data from {@link Units#ELLIPSOID} to ECR {@link Units#CARTESIAN} coordinate system. There
      is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
   */
  public static void ell2car (double[] v, int mpva, int nrec) {
    Shell.warning("Transform.ell2car is not yet coded");
  }

  /** Convert {@link Units#CARTESIAN} data from {@link #ECR} (ECEF) to {@link #ECI} reference frame.
      There is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param gha   Greenwich hour angle vector (in radians)
      @param nrec The number of records to convert (1..N).
      @since NeXtMidas 2.7.0
   */
  public static void ecr2eci (double[] v, int mpva, int nrec, double[] gha) {
    eci2ecef(v, mpva, nrec, gha, false); // Since NeXtMidas 3.5.3 renamed method
  }

  /** Convert {@link Units#CARTESIAN} data from {@link #ECR} (ECEF) to {@link #ECI} reference frame.
      There is no return value since the values are changed within the array.
      This is the same as {@link #ecr2eci(double[], int, int, double[])}.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param gha   Greenwich hour angle vector (in radians)
      @param nrec The number of records to convert (1..N).
      @since NeXtMidas 2.7.0
   */
  public static void ecef2eci (double[] v, int mpva, int nrec, double[] gha) {
    eci2ecef(v, mpva, nrec, gha, false); // Since NeXtMidas 3.5.3 use renamed method
  }

  /** Convert {@link Units#CARTESIAN} data from {@link #ECI} to {@link #TOP}.
      There is no return value since the values are changed within the array. <br>
      <br>
      <i>This conversion is currently implemented as a call to {@link #eci2ecr} followed
      by a call to {@link #ecr2top}, but may be optimized in the future.</i>
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
      @param from {@link Transform} specifying the reference frame parameters for the input values.
      @param to   {@link Transform} specifying the reference frame parameters for the output values.
      @param gha  Greenwich hour angle vector (in radians)
      @since NeXtMidas 3.3.0
   */
  public static void eci2top (double[] v, int mpva, int nrec, Transform from, Transform to, double[] gha) {
    eci2ecef(v, mpva, nrec, gha);  // Since NeXtMidas 3.5.3 use renamed method
    ecef2top(v, mpva, nrec, to);  // Since NeXtMidas 3.5.3 use renamed method
  }

  /** Convert {@link Units#CARTESIAN} data from {@link #ECI} to {@link #ECR} (ECEF) reference
      frame. There is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param gha   Greenwich hour angle vector (in radians)
      @param nrec The number of records to convert (1..N).
      @since NeXtMidas 2.7.0
   */
  public static void eci2ecr (double[] v, int mpva, int nrec, double[] gha) {
    eci2ecef(v, mpva, nrec, gha, true);  // Since 3.5.3 use new method name "ecef"
  }

  /** Convert {@link Units#CARTESIAN} data from {@link #ECI} to {@link #ECR} (ECEF) reference
      frame. There is no return value since the values are changed within the array.
      This is the same as {@link #eci2ecr(double[], int, int, double[])}.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param gha   Greenwich hour angle vector (in radians)
      @param nrec The number of records to convert (1..N).
      @since NeXtMidas 2.7.0
   */
  public static void eci2ecef (double[] v, int mpva, int nrec, double[] gha) {
    eci2ecef(v, mpva, nrec, gha, true);  // Since 3.5.3 use new method name "ecef"
  }

  /** Convert {@link Units#CARTESIAN} data from/to {@link #ECI} to/from {@link #ECR} (ECEF)
      reference frame. There is no return value since the values are changed within the array.
      Note, this conversion does not account for the earth's polar motion.
      @param v     Array of position vectors.
      @param mpva  Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec  The number of records to convert (1..N).
      @param gha   Greenwich hour angle vector (in radians)
      @param toECEF true if ECI to ECEF (ECR), false for ECEF to ECI
      @since NeXtMidas 2.7.0, Added java port of M$ECI_TO_ECR in type5000.for
      @since NeXtMidas 3.5.3 method name changed to "ecef" (from "ecr")
   */
  static void eci2ecef (double[] v, int mpva, int nrec, double[] gha, boolean toECEF) {
    int dir = (toECEF)? 1 : -1; // 1 for ECI to ECEF, -1 for ECEF to ECI
    int isk = 3*mpva;
    double eav,eavt,eav2,x,y,xx,yy,xxx,yyy,ca,sa;
    eav  = dir*EARTH_ANGULAR_VELOCITY;
    eavt = eav+eav;
    eav2 = eav*eav;
    for (int i=0,n=0; n<nrec; n++,i+=isk) {
      sa = Math.sin(gha[n]);
      ca = Math.cos(gha[n]);
      if (dir<0) sa = -sa;
      x      = v[i+0];
      y      = v[i+1];
      v[i+0] =  ca*x + sa*y;
      v[i+1] = -sa*x + ca*y;
      if (mpva<=1) continue;
      xx = v[i+3];
      yy = v[i+4];
      v[i+3] =  ca*xx + sa*yy + eav*(-sa*x+ca*y);
      v[i+4] = -sa*xx + ca*yy + eav*(-ca*x-sa*y);
      if (mpva<=2) continue;
      xxx = v[i+6];
      yyy = v[i+7];
      v[i+6] =  ca*xxx + sa*yyy + eavt*(-sa*xx+ca*yy) + eav2*(-ca*x-sa*y);
      v[i+7] = -sa*xxx + ca*yyy + eavt*(-ca*xx-sa*yy) + eav2*(+sa*x-ca*y);
    }
  }

  /** Convert {@link Units#CARTESIAN} data from {@link #TOP} to {@link #ECI}.
      There is no return value since the values are changed within the array. <br>
      <br>
      <i>This conversion is currently implemented as a call to {@link #top2ecr} followed
      by a call to {@link #ecr2eci}, but may be optimized in the future.</i>
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
      @param from {@link Transform} specifying the reference frame parameters for the input values.
      @param to   {@link Transform} specifying the reference frame parameters for the output values.
      @param gha  Greenwich hour angle vector (in radians)
      @since NeXtMidas 3.3.0
   */
  public static void top2eci (double[] v, int mpva, int nrec, Transform from, Transform to, double[] gha) {
    top2ecef(v, mpva, nrec, from);  // Since 3.5.3 use renamed method
    ecr2eci(v, mpva, nrec, gha);    // Since 3.5.3 use renamed method
  }

  /** Convert {@link Units#CARTESIAN} data from {@link #TOP} to {@link #ECR} reference frame. There
      is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
      @param from {@link Transform} specifying the reference frame parameters for the input values.
      @since NeXtMidas 3.5.3 method name changed to "ecef" (from "ecr")
   */
  public static void top2ecef (double[] v, int mpva, int nrec, Transform from) {
    TMatrix tm = top2ecrTMatrix(from);
    matrixVectorMultiply (tm,v,mpva,nrec);
  }

  /** Alias for top2ecef (double[] v, int mpva, int nrec, Transform from)
      @param v     see linked method for docs
      @param mpva  see linked method for docs
      @param nrec  see linked method for docs
      @param from  see linked method for docs
      @see #top2ecef (double[] v, int mpva, int nrec, Transform from)
   */
  public static void top2ecr (double[] v, int mpva, int nrec, Transform from) {
    top2ecef(v,mpva,nrec,from);
  }

  /** Convert {@link Units#CARTESIAN} data between two {@link #TOP} reference frames with different
      reference frame parameters. There is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
      @param from {@link Transform} specifying the reference frame parameters for the input values.
      @param to   {@link Transform} specifying the reference frame parameters for the output values.
   */
  public static void top2top (double[] v, int mpva, int nrec, Transform from, Transform to) {
    TMatrix tm = top2ecrTMatrix(from);
    TMatrix tn = top2ecrTMatrix(to);
    tn.transpose();
    tn.multiply(tm);
    matrixVectorMultiply (tn,v,mpva,nrec);
  }

  /** Convert {@link Units#CARTESIAN} data from {@link #ECR} to {@link #TOP} reference frame. There
      is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
      @param to   {@link Transform} specifying the reference frame parameters for the output values.
      @since NeXtMidas 3.5.3 method name changed to "ecef" (from "ecr")
   */
  public static void ecef2top (double[] v, int mpva, int nrec, Transform to) {
    TMatrix tm = top2ecrTMatrix(to);
    tm.transpose();
    matrixVectorMultiply (tm,v,mpva,nrec);
  }

  /** Alias for ecef2top (double[] v, int mpva, int nrec, Transform to)
      @param v     see linked method for docs
      @param mpva  see linked method for docs
      @param nrec  see linked method for docs
      @param to    see linked method for docs
      @see #ecef2top (double[] v, int mpva, int nrec, Transform to)
   */
  public static void ecr2top (double[] v, int mpva, int nrec, Transform to) {
    ecef2top(v,mpva,nrec,to);
  }

  /** Convert data from {@link Units#CARTESIAN} to {@link Units#SPHERICAL} coordinate system. There
      is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
   */
  public static void car2sph (double[] v, int mpva, int nrec) {
    double x,y,z,xx,yy,zz,xxx,yyy,zzz,r,ri,si,si2;
    int isk = 3*mpva;
    for (int i=0,n=0; n<nrec; n++,i+=isk) {
      // position
      if (v[i]==0 && v[i+1]==0 && v[i+2]==0) continue; //fixed in 3.9.3
      x = v[i+0];
      y = v[i+1];
      z = v[i+2];
      r = Math.sqrt(x*x+y*y+z*z);
      ri = 1.0/r;
      v[i+0] = r;
      v[i+1] = Math.atan2(y,x)*RAD2DEG;
      v[i+2] = Math.asin(z*ri)*RAD2DEG;
      if (mpva<=1) continue;
      // acceleration
      xx = v[i+3];
      yy = v[i+4];
      zz = v[i+5];
      si2 = Math.sqrt(x*x+y*y);
      si = Math.sqrt(si2);
      v[i+3] = (x*xx+y*yy+z*zz)*ri;
      v[i+4] = (x*yy-xx*y)*si2*RAD2DEG;
      v[i+5] = (zz-z*v[i+3]*ri)*si*RAD2DEG;
      if (mpva<=2) continue;
      // velocity
      xxx = v[i+6];
      yyy = v[i+7];
      zzz = v[i+8];
      v[i+6] = (xx*xx+yy*yy+zz*zz + x*xxx+y*yyy+z*zzz - v[i+3]*v[i+3])*ri;
      v[i+7] = ((x*yyy-xxx*y)*RAD2DEG-2*v[i+4]*(x*xx+y*yy))*si2;
      v[i+8] = (zzz-zz*v[i+3]*ri-z*(v[i+6]*ri-(v[i+3]*ri)*(v[i+3]*ri)))
                     *si*RAD2DEG - v[i+5]*(x*xx+y*yy)*si2;
    }
  }

  /** @deprecated Since NeXtMidas 3.3.0: This function was never implemented and
      has been superceded by {@link #fsu2fsu(double[],int,int,Transform,Transform,double[])}.
      @param v     N/A
      @param nrec  N/A
      @param mpva  N/A
      @param from  N/A
      @param to    N/A
   */
  @Deprecated
  public static void fsu2fsu (double[] v, int mpva, int nrec, Transform from, Transform to) {
    Shell.warning("Transform.fsu2fsu is not yet coded");
  }

  /** Convert reference frame, coordinate system, and units between two different transforms.
      This is a variant of {@link #fsu2fsu(double[],int,int,Transform,Transform,double[])}
      that is intended for use when there is a single record to transform.
      @param v     Array of position vectors.
      @param mpva  Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param from  {@link Transform} specifying the reference frame parameters for the input values.
      @param to    {@link Transform} specifying the reference frame parameters for the output values.
      @param epoch The epoch defining the Greenwich hour angle vector (in radians) to use (see
                   {@link Time#getGHA()}. Required for any conversions involving ECI, can be omitted
                   otherwise.
   */
  public static void fsu2fsu (double[] v, int mpva, Transform from, Transform to, Time epoch) {
    fsu2fsu(v, mpva, 1, from, to, null, epoch);
  }

  /** Convert reference frame, coordinate system, and units between two different transforms. There
      is no return value since the values are changed within the array. This function does not
      support ALL possible conversions, only those most common involving transforms.
      <pre>
        Supported Units:
          {@link Units#DISTANCE_M}    {@link Units#SPEED_M_S}    {@link Units#ACCELERATION_M_S2}
          {@link Units#DISTANCE_FT}   {@link Units#SPEED_FT_S}   {@link Units#ACCEL_FT_S2}
          {@link Units#DISTANCE_NMI}  {@link Units#SPEED_NMI_S}  {@link Units#ACCEL_NMI_S2}
                                      {@link Units#SPEED_KNOTS}  {@link Units#ACCEL_KNOTS_S}

        Supported Reference Frames:
          {@link Units#CARTESIAN}  {@link Units#SPHERICAL}  {@link Units#CYLINDRIC}
          {@link Units#ELLIPSOID}  {@link Units#GEODETIC}

        Supported Coordinate Systems:
          "ECR"  "ECI"  "TOP"  "TOPOCENT" (or "TOPO")
      </pre>
      The algorithm used:
      <pre>
        1. Convert from input units to base-SI units
        2. Convert from input reference frame to {@link Units#CARTESIAN}
        3. Convert reference frame
        4. Convert from {@link Units#CARTESIAN} to output reference frame
        5. Convert from base-SI units to output units
      </pre>
      This function is specifically optimized to return immediately (with no changes to the values)
      if either transform is null or if the two transforms are equal.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
      @param from {@link Transform} specifying the reference frame parameters for the input values.
      @param to   {@link Transform} specifying the reference frame parameters for the output values.
      @param gha  Greenwich hour angle vector (in radians). Required for any conversions involving
                  ECI, can be omitted otherwise. Length of array must match <code>nrec</code>.
   */
  public static void fsu2fsu (double[] v, int mpva, int nrec,
                              Transform from, Transform to, double[] gha) {
    fsu2fsu(v, mpva, nrec, from, to, gha, null);
  }

  /** Internal Use Only: This version takes GHA as array or single value. */
  @SuppressWarnings("fallthrough")
  private static void fsu2fsu (double[] v, int mpva, int nrec,
                               Transform from, Transform to,
                               double[] gha, Time epoch) {
    if ((from == null) || (to == null) || from.equals(to)) return; // nothing to do
    int fromRef = from.referenceFrame;
    int toRef   = to.referenceFrame;

    // CONVERT TO SI UNITS
    switch (mpva) {
      case POSVELACC: fixUnits(v, mpva, nrec, 6, from.unitsAcc, true); // FALLTHROUGH
      case POSVEL:    fixUnits(v, mpva, nrec, 3, from.unitsVel, true); // FALLTHROUGH
      case POS:       fixUnits(v, mpva, nrec, 0, from.unitsPos, true); // FALLTHROUGH
    }

    // CONVERT TO CARTESIAN
    switch (from.coordinateSystem) {
      case CARTESIAN: /* nothing to do */                    break;
      case SPHERICAL: sph2car(v, mpva, nrec); fromRef = ECR; break;
      //TBD: case CYLINDRIC: cyl2car(v, mpva, nrec); fromRef = ECR; break;
      //TBD: case ELLIPSOID: ell2car(v, mpva, nrec); fromRef = ECR; break;
      case GEODETIC:  geo2car(v, mpva, nrec); fromRef = ECR; break;
      default: throw new MidasException("Unsupported conversion "+from.getCoordinateSystem()
                                       +" to CARTESIAN to "+to.getCoordinateSystem());
    }

    // PREPARE FOR CONVERT FROM CARTESIAN
    switch (to.coordinateSystem) {
      case CARTESIAN: /* no change */ break;
      case SPHERICAL: toRef = ECR;    break;
      //TBD: case CYLINDRIC: toRef = ECR;    break;
      //TBD: case ELLIPSOID: toRef = ECR;    break;
      case GEODETIC:  toRef = ECR;    break;
      default: throw new MidasException("Unsupported conversion "+from.getCoordinateSystem()
                                       +" to CARTESIAN to "+to.getCoordinateSystem());
    }

    // CONVERT REFERENCE
    if ((fromRef != toRef) || (fromRef == TOP) || (fromRef == TOPO)) {
      // Only do this if converting between two reference frames or between two
      // TOP/TOPO references. We skip this for everything else since it is
      // possible to convert coordinate systems without a "valid" reference.
      if ((gha == null) && (epoch != null)) {
        gha = new double[] { epoch.getGHA() };
      }
      switch (fromRef + (toRef * 100)) {
        case ECR +(ECI *100): ecr2eci(v, mpva, nrec,           gha); break;
        case ECR +(TOP *100): ecef2top(v, mpva, nrec,       to     ); break;
        case ECR +(TOPO*100): ecef2top(v, mpva, nrec,       to     ); break;
        case ECI +(ECR *100): eci2ecr(v, mpva, nrec,           gha); break;
        case ECI +(TOP *100): eci2top(v, mpva, nrec, from, to, gha); break;
        case ECI +(TOPO*100): eci2top(v, mpva, nrec, from, to, gha); break;
        case TOP +(ECR *100): top2ecef(v, mpva, nrec, from         ); break;
        case TOP +(ECI *100): top2eci(v, mpva, nrec, from, to, gha); break;
        case TOP +(TOP *100): top2top(v, mpva, nrec, from, to     ); break;
        case TOP +(TOPO*100): top2top(v, mpva, nrec, from, to     ); break;
        case TOPO+(ECR *100): top2ecef(v, mpva, nrec, from         ); break;
        case TOPO+(ECI *100): top2eci(v, mpva, nrec, from, to, gha); break;
        case TOPO+(TOP *100): top2top(v, mpva, nrec, from, to     ); break;
        case TOPO+(TOPO*100): top2top(v, mpva, nrec, from, to     ); break;
        default: throw new MidasException("Unsupported conversion "+from.getReferenceFrame()
                                         +" to "+to.getReferenceFrame());
      }
    }

    // CONVERT FROM CARTESIAN
    //   Note: We intentionally omit "case CARTESIAN:" and "default:" as there
    //   is nothing further to do for those cases.
    switch (to.coordinateSystem) {
      case SPHERICAL: car2sph(v, mpva, nrec); break;
      case CYLINDRIC: car2cyl(v, mpva, nrec); break;
      case ELLIPSOID: car2ell(v, mpva, nrec); break;
      case GEODETIC:  car2geo(v, mpva, nrec); break;
    }

    // CONVERT FROM SI UNITS
    switch (mpva) {
      case POSVELACC: fixUnits(v, mpva, nrec, 6, to.unitsAcc, false); //FALLTHROUGH
      case POSVEL:    fixUnits(v, mpva, nrec, 3, to.unitsVel, false); //FALLTHROUGH
      case POS:       fixUnits(v, mpva, nrec, 0, to.unitsPos, false); //FALLTHROUGH
    }
  }

  /** <b>Internal use only:</b> Convert reference frame, coordinate system, and
      units between two different transforms. There is no return value since the
      values are changed within the table. The table is expected to have the
      following keys:
      <pre>
        POS=(x,y,x)   Position
        VEL=(x,y,x)   Velocity      <i>[Must be omitted if POS is omitted.]</i>
        ACC=(x,y,x)   Acceleration  <i>[Must be omitted if VEL is omitted.]</i>
      </pre>
      @param tbl  The table with the entries. The output will be as an updated
                  <code>double[]</code> entries in the table.
      @param from {@link Transform} specifying the reference frame parameters for the input values.
      @param to   {@link Transform} specifying the reference frame parameters for the output values.
      @param gha  Greenwich hour angle vector (in radians). Required for any conversions involving
                  ECI, can be omitted otherwise.
      @see #fsu2fsu(double[], int, int, Transform, Transform, double[])
      @since NeXtMidas 3.3.0
   */
  @InternalUseOnly
  @SuppressWarnings("fallthrough")
  public static void fsu2fsu (Table tbl, Transform from, Transform to, Double gha) {
    int      mpva = POSVELACC;
    double[] acc  = Convert.o2da(tbl.get("ACC"));  if (acc == null) mpva = POSVEL;
    double[] vel  = Convert.o2da(tbl.get("VEL"));  if (vel == null) mpva = POS;
    double[] pos  = Convert.o2da(tbl.get("POS"));  if (pos == null) return; // nothing to do
    double[] _gha = (gha == null)? null : new double[] { gha };
    double[] v    = new double[9];

    switch (mpva) {
      case POSVELACC: System.arraycopy(acc, 0, v, 6, 3); //FALLTHROUGH
      case POSVEL:    System.arraycopy(vel, 0, v, 3, 3); //FALLTHROUGH
      case POS:       System.arraycopy(pos, 0, v, 0, 3);
    }

    fsu2fsu(v, mpva, 1, from, to, _gha);

    switch (mpva) {
      case POSVELACC: System.arraycopy(v, 6, acc, 0, 3);  tbl.put("ACC", acc); //FALLTHROUGH
      case POSVEL:    System.arraycopy(v, 3, vel, 0, 3);  tbl.put("VEL", vel); //FALLTHROUGH
      case POS:       System.arraycopy(v, 0, pos, 0, 3);  tbl.put("POS", pos);
    }
  }

  /** Converts to/from SI units.
      @param v     The array.
      @param mpva  POS, POSVEL, or POSVELACC.
      @param nrec  The number of records.
      @param off   The offset (0=POS, 3=VEL, or 6=ACC).
      @param val   The value to multiply by.
      @param units The units to convert to/from.
      @param toSI  true=to SI units, false=from SI units
   */
  private static void fixUnits (double[] v, int mpva, int nrec, int off, int units, boolean toSI) {
    double mult = 0;
    switch (units) {
      // DIST/SPEED/ACCEL
      case Units.DISTANCE_M:        return; // nothing to do
      case Units.DISTANCE_FT:       mult = FEET2METERS; break;
      case Units.DISTANCE_NMI:      mult = NM2METERS;   break;
      case Units.SPEED_M_S:         return; // nothing to do
      case Units.SPEED_FT_S:        mult = FEET2METERS; break;
      case Units.SPEED_NMI_S:       mult = NM2METERS;   break;
      case Units.SPEED_KNOTS:       mult = KNOTS2MS;    break;
      case Units.ACCELERATION_M_S2: return; // nothing to do
      case Units.ACCEL_FT_S2:       mult = FEET2METERS; break;
      case Units.ACCEL_NMI_S2:      mult = NM2METERS;   break;
      case Units.ACCEL_KNOTS_S:     mult = KNOTS2MS;    break;
      case Units.ACCEL_G:           mult = GFORCE2MS2;  break;
      // OTHER
      case Units.ALTITUDE_M:        return; // nothing to do
      case Units.ALTITUDE_FT:       mult = FEET2METERS; break;
    }

    if (mult == 0) {
      String name = DataFile.getUnitsVarName(units);
      if (toSI) throw new MidasException("Can not convert "+name+" to SI units");
      else      throw new MidasException("Can not convert SI units to "+name);
    }

    if (!toSI) mult = 1/mult;

    for (int i = 0; i < nrec; i++) {
      v[off++] *= mult;
      v[off++] *= mult;
      v[off++] *= mult;
      off += (mpva-1)*3;
    }
  }

  /** Transform data for plotter coordinates (Lon,Lat,Alt) in EER.
      @param v    Array of position vectors.
      @param nrec The number of records to convert (1..N).
      @return Always returns <code>nrec</code>.
   */
  public int toLonLatAlt (double[] v, int nrec) {
    // TODO: Replace the code here with the following since the current implementation
    // lacks a LOT of error checking and will give bad results if things aren't
    // quite right.
    //    fsu2fsu(v, POS, nrec, this, Transform_ECR_GEODETIC, null, null);
    //    all2lla(v,nrec);  // plotter needs (Lon,Lat,Alt) not (Alt,Lat,Lon)
    //    return nrec;
    if (referenceFrame==TOP||referenceFrame==TOPO) top2ecef(v,POS,nrec,this);
    if (coordinateSystem==Units.CARTESIAN) car2geo(v,POS,nrec);
    all2lla(v,nrec);  // plotter needs (Lon,Lat,Alt) not (Alt,Lat,Lon)
    return nrec;
  }

  /** Transform data from plotter coordinates (Lon,Lat,Alt) in EER. This is the
      reverse of {@link #toLonLatAlt}.
      @param v    Array of position vectors.
      @param nrec The number of records to convert (1..N).
      @return Always returns <code>nrec</code>.
      @since NeXtMidas 3.3.0
   */
  public int fromLonLatAlt (double[] v, int nrec) {
    lla2all(v,nrec);
    fsu2fsu(v, POS, nrec, Transform_ECR_GEODETIC, this, null, null);
    return nrec;
  }

  /** Transform AltLatLon vector (as given in a BLUE file) to LonLatAlt vector for plotters.
      @param v    Array of position vectors.
      @param nrec The number of records to convert (1..N).
      @return Always returns <code>nrec</code>.
   */
  public static int all2lla (double[] v, int nrec) {
    double tmp;
    for (int n=0,i=0; n<nrec; n++,i+=3) {
      tmp=v[i+0]; v[i+0]=v[i+2]; v[i+2]=tmp;
    }
    return nrec;
  }

  /** Transform LonLatAlt vector to AltLatLon vector.
      @param v    Array of position vectors.
      @param nrec The number of records to convert (1..N).
      @return Always returns <code>nrec</code>.
   */
  private static int lla2all (double[] v, int nrec) {
    return all2lla(v, nrec); // <-- the math is the same (i.e. swap v[0] with v[2])
  }

  /** Clip at earth intersect lines from point to origin.
      @param v     The position vector (has a length that is a multiple of three
                   such that v[i+0]=x, v[i+1]=y, v[i+2]=z makes up a
                   single record/point).
      @param ioff  The offset into the position vector.
      @param nrec  The number of records/points to act on.
      @param vo    The reference point (has a length of three such that
                   v[0]=x, v[1]=y, v[2]=z makes up a record/point).
      @param mode  The clipping mode when clipping at the Earth's surface. <pre>
                   mode = 1 - Cone extends to horizon.
                   mode = 0 - Clip cone at a plane perpendicular to q.</pre>
      @return Always returns <code>nrec</code>.
   */
  public static int clipAtEarth (double[] v, int ioff, int nrec, double[] vo, int mode) {

    double xo,yo,zo,xd,yd,zd;       // origin and delta vectors
    double xe,ye,ze,xei,yei,zei;    // eccentricity vector and inverse
    double p,qA,qB,qC,qR,qS,qSR;    // quadratic components
    double poo,pod,pdd;             // inner products

    xe = EARTH_EQUATORIAL_RADIUS;
    ye = EARTH_EQUATORIAL_RADIUS;
    ze = xe*Math.sqrt(1.0-(EARTH_ECCENTRICITY*EARTH_ECCENTRICITY));
    xei = 1.0/xe;
    yei = 1.0/ye;
    zei = 1.0/ze;
    xo = xei*vo[0];
    yo = yei*vo[1];
    zo = zei*vo[2];
    for (int n=0,i=ioff; n<nrec; n++,i+=3) {
      xd = xei*v[i+0] - xo;
      yd = yei*v[i+1] - yo;
      zd = zei*v[i+2] - zo;
      pdd = xd*xd + yd*yd + zd*zd;
      pod = xo*xd + yo*yd + zo*zd;
      poo = xo*xo + yo*yo + zo*zo;
      qA = pdd;
      if (qA<1.0e-30) continue;
      qB = 2.0*pod;
      qC = poo - 1.0;
      qR = qB / (qA*2.0);
      qS = qC / qA;
      if (qR*qR > qS) {   // has 2 solutions
        qSR = Math.sqrt( qR*qR - qS );
        p = -qR - qSR;
        if (mode==1 || (p>0.0 && p<1.0)) {  // use it
          v[i+0] = (xo + p*xd) * xe;
          v[i+1] = (yo + p*yd) * ye;
          v[i+2] = (zo + p*zd) * ze;
        }
      } else if (mode==1) {
        qA = pod*pod - pdd;
        qB = 2.0*pod*(poo-1.0);
        qC = poo*(poo-1.0);
        qR = qB / (qA*2.0);
        qS = qC / qA;
        qSR = qR*qR - qS;
        if (qSR>0) qSR = Math.sqrt(qSR); else qSR=0;
        if (qR>0) p = -qR + qSR;
        else p = -qR - qSR;
        v[i+0] = (xo + p*xd) * xe;
        v[i+1] = (yo + p*yd) * ye;
        v[i+2] = (zo + p*zd) * ze;
      }
    }
    return nrec;
  }

  /** Clip this cone at the earth.
   *  First see if this portion (ray) of the cone intersects the earth. If so, it will be trimmed there.
   *  - this is the case where the ray intersect with sphere equations generates 2 solutions
   *    (one for entering and the other exiting the earth)
   *  Second, if there are not two solutions, then this portion of the cone naturally extends out from the earth.
   *  -This last (or only) ring will either:
   *   1) be trimmed to the geosurface or
   *   2) be clipped at a point with the same lat and lon as the geosurface point,
   *     but with an altitude that is in line with the original conic
   *<pre>
   *  Earth is an oblate spheroid
   *
   *                                             e
   *                                                *
   *                                   . ...  f        p'
   *                               .             . *    *
   *                             .                 .  *   *
   *                           .                     .   *  *
   *                          .                       .     * *
   *                         .                         .        **
   *                         .            c------------n-----------*.viewer
   *                         .                         .        *
   *                          .                       .     *
   *                           .                     .   *
   *                             .                     p''
   *                               .             . *
   *                                   . ... .

   *   p'  (v[ioff],v[ioff+1],v[ioff+2]) - point that extended will not intersect the earth
   *   p'' (v[ioff],v[ioff+1],v[ioff+2]) - point that extended will intersect the earth
   *   c   (Center of Earth)             - point at center of earth
   *   f   (Footprint Point)             - point on horizon line that intersects with the Earth
   *   e   (Extended Along Conic)        - point extended along conic to have same lat, lon as 'f', but alt in line with conic
   *   n   (Nadir Point)                 - point on earth's surface between viewer and center of earth
   *</pre>
   *
   *  Below are the source equations for the ray intersect with sphere portion of this code:
   *<pre>
   *  A) The formula for 3D sphere is: (x-a)^2 + (y-b)^2 + (z-c)^2 = r^2
   *     with [a,b,c] being the center of the sphere and r its radius.
   *     The point [x,y,z] is on the sphere if it satisfies this equation
   *     In our case [a,b,c] is [0,0,0], resulting in x^2 + y^2 + z^2 = r^2
   *  B) The parametric equations for a ray are:
   *     X = xo + xd*t
   *     Y = yo + yd*t
   *     Z = zo + zd*t
   *  C) To find the intersection, we want to see what points on the ray are
   *     the same as the points on the sphere, so we substitute the ray equation
   *     into the sphere equation:
   *     (xo + xd*t - a)^2 + (yo + yd*t - b)^2 + (yo + yd*t - c)^2 = r^2
   *     In our case [a,b,c] is [0,0,0], resulting in:
   *     (xo + xd*t)^2 + (yo + yd*t)^2 + (yo + yd*t)^2 = r^2
   *     which expands to:
   *     (xd^2 + yd^2 + zd^2)                          *  t^2  +
   *     [2[xd*(xo - a) + yd*(yo - b) + zd*(zo - c)]]  *  t    +
   *     [(xo - a)^2 + (yo - b)^2 + (zo - c)^2) - r^2] *  1
   *     = 0
   *     In our case [a,b,c] is [0,0,0], resulting in:
   *     (xd^2 + yd^2 + zd^2)         *  t^2  +
   *     [2[xd*xo + yd*yo + zd*zo]]   *  t    +
   *     [(xo^2 + yo^2 + zo^2) - r^2] *  1
   *     = 0
   *  D) Notice that this is the quadratic equation in the form At^2 + Bt + C = 0, with:
   *     qA = (xd^2 + yd^2 + zd^2)
   *     qB = [2[xd*xo + yd*yo + zd*zo]]
   *     qC = [(xo^2 + yo^2 + zo^2) - r^2]
   *  E) We can apply the quadratic equation for an unknown variable
   *     t = (-B + sqrt(B^2 - 4AC)) / 2A
   *  F) The B^2 - 4AC portion is called the "discriminant".
   *     Depending on the value of the discriminant, we will get zero, one, or two solutions to this equation
   *    - If it is less than zero, the solution is an imaginary number and the ray and the sphere do not intersect in the real plane.
   *    - If it is equal to zero, then the ray intersects the sphere at exactly 1 point (it is exactly tangent to the sphere).
   *    - If it is greater than 0, then the ray intersects the sphere at exactly 2 points.
   *  G) If the discriminant indicates there's no solution, then you you're done! The ray doesn't intersect the sphere.
   *     If the discriminant indicates at least one solution, you can solve for t to determine the intersection point.
   *     the solutions are:
   *     t_1 = (-B + sqrt(B^2 - 4AC)) / 2A
   *     t_2 = (-B - sqrt(B^2 - 4AC)) / 2A
   *  H) The smaller solution is the point at which the ray first hits the sphere.
   *</pre>
   *
   *  @param v      The position vector (has a length that is a multiple of three
   *                such that v[i+0]=x, v[i+1]=y, v[i+2]=z makes up a
   *                single record/point).
   *  @param ioff   The offset into the position vector.
   *  @param nrec   The number of records/points to act on.
   *  @param vo     The reference point (has a length of three such that
   *                v[0]=x, v[1]=y, v[2]=z makes up a record/point).
   *  @param mode   The clipping mode when clipping at the Earth's surface. <pre>
   *                mode = 1 - Cone extends to horizon.
   *                mode = 0 - Clip cone at a plane perpendicular to q.</pre>
   *  @param trimToGeoSurface trim footprint to geosurface
   *  @param viewer Position describing the vertex of the cone
   *  @param angle  1/2 angle
   *  @return always returns <code>nrec</code>.
   *  @since NeXtMidas 4.0.1
    */
  public static int clipAtEarth (double[] v, int ioff, int nrec, double[] vo, int mode,
      boolean trimToGeoSurface,Position viewer, double angle) {

    boolean legacyOnlyMode = Transform.getUseLegacyConeCode(); // use only original Jeff code
    boolean preciseHorizonExtension = !legacyOnlyMode && !Transform.getUseSphericalConeCode();

    // declare variables used for scaled calculations
    double xo,yo,zo,xd,yd,zd;       // origin and delta vectors
    double xe,ye,ze,xei,yei,zei;    // eccentricity vector and inverse
    double p,qA,qB,qC,qR,qS,qSR;    // quadratic components
    double poo,pod,pdd;             // inner (dot) products

    // declare variables used when the cone does not intersect the earth and we
    // are using an alternate calculation for extending the point to the horizon
    double[] viewerXYZ = null;
    double distViewer2CenterEarth = 0; // only calculate if needed
    double[] vcHat = null; // normalized vector viewer to center of earth
    double horizonDistanceSpherical = 0;  // only calculate if needed
    // initialize these, only if we are using potentially using them
    if (!legacyOnlyMode) {
      viewerXYZ = new double[] {viewer.getX(),viewer.getY(),viewer.getZ()}; // viewer location as double[] {x,y,z}
      double distViewer2CenterEarthSquared = Vector3D.dotProduct(viewerXYZ, viewerXYZ);
      double distFromEarthSurface = distViewer2CenterEarthSquared - EARTH_EQUATORIAL_RADIUS_SQUARED;
      if (distFromEarthSurface > 0) {
        distViewer2CenterEarth = Math.sqrt(distViewer2CenterEarthSquared);
        vcHat = Vector3D.norm(viewerXYZ,distViewer2CenterEarth);
        horizonDistanceSpherical = Math.sqrt(distFromEarthSurface); // remove when spherical earth opt removed
      }
      else {
        // NXM-3979 New more precise code does not handle case where the viewer is below earth surface
        preciseHorizonExtension = false;
        legacyOnlyMode = true;
      }
    }

    // set up scaling so oblate earth is now represented as unit sphere
    xe = EARTH_EQUATORIAL_RADIUS;
    ye = EARTH_EQUATORIAL_RADIUS;
    ze = xe*Math.sqrt(1.0-EARTH_ECCENTRICITY_SQUARED);
    xei = 1.0/xe;
    yei = 1.0/ye;
    zei = 1.0/ze;
    xo = xei*vo[0];
    yo = yei*vo[1];
    zo = zei*vo[2];

    poo = xo*xo + yo*yo + zo*zo; // dot product of scaled viewer vector with itself

    for (int n=0,i=ioff; n<nrec; n++,i+=3) {
      // *** First check if the cone vector (ray) intersects the earth - see equations in method docs ***
      xd = xei*v[i+0] - xo; // get x, y, x of vector describing delta between viewer on current point
      yd = yei*v[i+1] - yo;
      zd = zei*v[i+2] - zo;
      pdd = xd*xd + yd*yd + zd*zd; // dot product of scaled delta vector with itself
      pod = xo*xd + yo*yd + zo*zd; // dot product of scaled viewer vector with delta vector
      qA = pdd;
      if (qA<1.0e-30) {  // we already have our point: t is (-B + Sqrt(B^2))/2A = 0/2A = 0
        continue;
      }
      qB = 2.0*pod;
      qC = poo - 1.0;
      qR = qB / (qA*2.0); // same as pod / pdd, i.e. pod / (distance between this point and reference)
      qS = qC / qA;
      // qR * qR is same as B^2/4A^2 and qS is C/A, so multiply both sides by 4A^2 & you get: B^2 > 4AC
      // i.e. discriminant (B^2 - 4AC) is positive, which means we have 2 solutions
      if (qR*qR > qS) {   // has 2 solutions
        // 2 solutions means ray goes through earth (entry and exit point)
        qSR = Math.sqrt( qR*qR - qS ); // same as Math.sqrt(B^2 - 4AC)/2A
        p = -qR - qSR; // -qR is -B/2A, so p = t_1 = (-B + sqrt(B^2 - 4AC)) / 2A
        if (mode==1 || (p>0.0 && p<1.0)) { // p is scaled distance from observer to first intersect (comparing it against radius)
          v[i+0] = (xo + p*xd) * xe;
          v[i+1] = (yo + p*yd) * ye;
          v[i+2] = (zo + p*zd) * ze;
        }
      } else if (mode==1){
        // ray needs to be clipped at horizon (either at geosurface or along original vector)
        boolean pointHasBeenExtended = false;
        if (preciseHorizonExtension) {
          // use more accurate calculation, that is default as of 4.0.1
          double[] pt2Clip = new double[] {v[i+0], v[i+1], v[i+2]};
          double[] extendedPoint = Transform.oblateEarthConeToHorizon(vcHat,distViewer2CenterEarth, viewerXYZ,
              pt2Clip,trimToGeoSurface);
          if (extendedPoint != null) { // this should never happen, but will if vcHat & pt2Clip vectors are colinear
            v[i+0] = extendedPoint[0];
            v[i+1] = extendedPoint[1];
            v[i+2] = extendedPoint[2];
            pointHasBeenExtended = true;
          }
        }
        if (!pointHasBeenExtended) {
          // use original scaled solution for finding the horizon point along the original vector (ray)
          // Note: Jeff mentioned that this solution "is probably the back side of the sphere. In the
          // case of at the edge of the earth I think we replace the conic intersect with the horizon line.
          // At that line the front and back solutions meet." This code in practice calculates a point along
          // the original trajectory that has very close to the same lat & lon as the matching horizon point.
          // I (jtm) still do not know what equations are being used.
          qA = pod*pod - pdd;
          qB = 2.0*pod*(poo-1.0);
          qC = poo*(poo-1.0);
          qR = qB / (qA*2.0);
          qS = qC / qA;
          qSR = qR*qR - qS;
          if (qSR>0) {
            qSR = Math.sqrt(qSR);
          } else {
            qSR=0;
          }
          // now qSR is sqrt(new discriminant)
          // p is proportion - distance of new location compared to distance of old location
          if (qR>0) p = -qR + qSR;
          else p = -qR - qSR;
          // now unscale all the values
          double clippedX =  (xo + p*xd) * xe;
          double clippedY =  (yo + p*yd) * ye;
          double clippedZ =  (zo + p*zd) * ze;
          double newRadius = Math.sqrt(clippedX*clippedX + clippedY*clippedY + clippedZ*clippedZ);
          if (legacyOnlyMode || (newRadius <= EARTH_EQUATORIAL_RADIUS_PLUS)) {
            // solution within this range seems accurate enough and is where earth's oblateness
            // weighs heavily on the solution (note: this is NOT trimmed to the geosurface)
            v[i+0] = (xo + p*xd) * xe;
            v[i+1] = (yo + p*yd) * ye;
            v[i+2] = (zo + p*zd) * ze;
          }
          else {
            // for points well off the surface of the earth, this alternate code that
            // does not take into account earth's oblateness is generating better data
            // (avoids cone footprint that covers dark side of earth)
            // Note: this is a faster implementation of the temporary patch
            double[] pt2Clip = new double[] {v[i+0], v[i+1], v[i+2]};
            double[] geosurfacePoint = Transform.sphericalEarthConeToHorizon(vcHat,1/distViewer2CenterEarth, viewerXYZ,
                pt2Clip,horizonDistanceSpherical,trimToGeoSurface);
            v[i+0] = geosurfacePoint[0];
            v[i+1] = geosurfacePoint[1];
            v[i+2] = geosurfacePoint[2];
          }
        }
      }
    }
    return nrec;
  }

  /** Horizon to Horizon at Altitude<br>
   *  Uses horizon points and viewer to determine what can be viewed at a
   *  specified altitude and creates that horizon line.
   *  NOTE: Known issue with at least horizon lines that directly cross the
   *  North Pole!
   *  <pre>
   *  Earth is an oblate spheroid. Earth + Altitude is a different oblate
   *  spheroid.
   *
   *  Big picture: We want p2 which is the horizon pt extended
   *  to an altitude - basically what is visible to the observer given the
   *  target is at a given altitude. Step 1 is to convert to scale everything
   *  to the scale needed to make Earth + Altitude a Unit Sphere.
   *
   *                                   aaa+(p2)
   *                                a  .... *a
   *                            a  .           +(horizon pt)
   *                          a  .            *  .* a
   *                        a  .             *     . *a
   *                       a  .             *        .  +(p1)
   *                      a  .             *          .  a *
   *                      a  .            c***********.**a****observer
   *                      a  .                        .  a
   *                       a  .                      .  a
   *                        a  .                    .  a
   *                          a  .                 .  a
   *                            a  .           .  a
   *                                a  .....  a
   **                                  aaaaa
   *</pre>
   *
   *  Below are the source equations for the ray intersect with sphere portion of this code:
   *<pre>
   *  A) The formula for 3D sphere is: (x-a)^2 + (y-b)^2 + (z-c)^2 = r^2
   *     with [a,b,c] being the center of the sphere and r its radius.
   *     The point [x,y,z] is on the sphere if it satisfies this equation
   *     In our case [a,b,c] is [0,0,0], resulting in x^2 + y^2 + z^2 = r^2
   *  B) The parametric equations for a ray are:
   *     X = xo + xd*t
   *     Y = yo + yd*t
   *     Z = zo + zd*t
   *  C) To find the intersection, we want to see what points on the ray are
   *     the same as the points on the sphere, so we substitute the ray equation
   *     into the sphere equation:
   *     (xo + xd*t - a)^2 + (yo + yd*t - b)^2 + (yo + yd*t - c)^2 = r^2
   *     In our case [a,b,c] is [0,0,0], resulting in:
   *     (xo + xd*t)^2 + (yo + yd*t)^2 + (yo + yd*t)^2 = r^2
   *     which expands to:
   *     (xd^2 + yd^2 + zd^2)                          *  t^2  +
   *     [2[xd*(xo - a) + yd*(yo - b) + zd*(zo - c)]]  *  t    +
   *     [(xo - a)^2 + (yo - b)^2 + (zo - c)^2) - r^2] *  1
   *     = 0
   *     In our case [a,b,c] is [0,0,0], resulting in:
   *     (xd^2 + yd^2 + zd^2)         *  t^2  +
   *     [2[xd*xo + yd*yo + zd*zo]]   *  t    +
   *     [(xo^2 + yo^2 + zo^2) - r^2] *  1
   *     = 0
   *  D) Notice that this is the quadratic equation in the form At^2 + Bt + C = 0, with:
   *     qA = (xd^2 + yd^2 + zd^2) = 1 because we have {xd,yd,zd} as a unit vector
   *     qB = [2[xd*xo + yd*yo + zd*zo]]
   *     qC = [(xo^2 + yo^2 + zo^2) - r^2]
   *  E) We can apply the quadratic equation for an unknown variable
   *     t = (-B + sqrt(B^2 - 4AC)) / 2A
   *  F) The B^2 - 4AC portion is called the "discriminant".
   *     Depending on the value of the discriminant, we will get zero, one, or
   *     two solutions to this equation
   *    - If it is less than zero, the solution is an imaginary number and the
   *      ray and the sphere do not intersect in the real plane.
   *    - If it is equal to zero, then the ray intersects the sphere at exactly
   *      1 point (it is exactly tangent to the sphere).
   *    - If it is greater than 0, then the ray intersects the sphere at exactly
   *      2 points.
   *    - For our case here, we should always have 2 intersects, except when the
   *      provided horizon point lacks accuracy.
   *  G) We want the second intersect (p2), as p1 occurs when hitting the altitude bubble before reaching the horizon point.
   *     p2 is the expanded horizon showing what is visible at this altitude.
   *     p1 = (-B + sqrt(B^2 - 4AC)) / 2A
   *     p2 = (-B - sqrt(B^2 - 4AC)) / 2A
   *     Note: qA==A, qB==B, qC==C
   *     Since qA==1, this simplifies to:
   *     p1 = (-B + sqrt(B^2 - 4C)) / 2
   *     p2 = (-B - sqrt(B^2 - 4C)) / 2
   *</pre>
   *  @param hznPts horizon points in ECEF Cartesian (x,y,z) array
   *  @param ioff   offset into array for first point
   *  @param npts   number of points
   *  @param vo     Observer (Viewer) Position
   *  @param alt    altitude to base new horizon line on
   *  @return array with ECEF Cartesian horizon at altitude points
   *  @since NeXtMidas 4.1.4
   */
  @InternalUseOnly("Prototype version - use at your own risk")
  public static double[] hzn2AltHzn (double[] hznPts, int ioff, int npts, Position vo, double alt) {
    if (horizonAltDebug) System.out.println("Using more traditional ray intersect sphere math hzn2AltHzn method alttitude="+alt);

    double xo,yo,zo,xd,yd,zd;    // origin and delta vectors
    double xe,ye,ze,xei,yei,zei; // eccentricity vector and inverse
    double qB,qC,qR,qSR;         // quadratic components
    double poo,pod;              // inner products
    double[] hznAtAltPts = new double[npts*3];

    xe = EARTH_EQUATORIAL_RADIUS + alt;
    ye = EARTH_EQUATORIAL_RADIUS + alt;
    ze = xe*Math.sqrt(1.0-(EARTH_ECCENTRICITY*EARTH_ECCENTRICITY)) + alt;
    xei = 1.0/xe;
    yei = 1.0/ye;
    zei = 1.0/ze;
    xo = xei*vo.x; // viewer should be being passed in correctly, checked agains clipAtEarth
    yo = yei*vo.y;
    zo = zei*vo.z;
    double[] viewerVec = new double[] {xo, yo, zo}; // A = origin of the ray

    for (int n=0,i=ioff; n<npts; n++,i+=3) {
      xd = xei*hznPts[i+0] - xo; // to - from
      yd = yei*hznPts[i+1] - yo;
      zd = zei*hznPts[i+2] - zo;
      double[] scaledHzn = {xei*hznPts[i+0],yei*hznPts[i+1],zei*hznPts[i+2]};
      double[] h2oVec = new double[] {xd, yd, zd};
      double h2oMag = Vector3D.mag(h2oVec);
      double[] h2oUnitVec = Vector3D.norm(h2oVec); // B direction of ray that is a unit vector
      pod = Vector3D.dotProduct(h2oUnitVec, viewerVec); // dot(B,A)
      poo = Vector3D.dotProduct(viewerVec, viewerVec); // dot(A,A)

      qB = 2.0*pod; // b = 2 * dot(B,A)
      qC = poo - 1.0; // c = dot(A,A) - r^2
      qR = qB / 2.0;

      if (qR*qR > qC) {   // has 2 solutions
        qSR = Math.sqrt(qR*qR - qC);
        double p1 = -qR - qSR;
        double p2 = -qR + qSR;
        if (horizonAltDebug) System.out.println("p1:"+p1+" p2:"+p2+" h2oMag:"+h2oMag);
        // -qR is like tc and qSR like t1c
        // with inexact horizon points we may have 2 intersects
        // even at altitude 0, which should not happen onn a real
        // horizon point, as that would have just one tangential
        // intersect with the earth's surface.
        // If the distance to the original horizon point is truely
        // beyond the first intersect, then this is operating as planned
        // and we want the second intersect with the altitude bubble around
        // the earth (p2)
        if (p1 + 0.001 < h2oMag)  {
          double pnow = (ioff==0) ? p2 : p2;
          hznAtAltPts[i+0] = (xo + pnow*h2oUnitVec[0]) * xe;
          hznAtAltPts[i+1] = (yo + pnow*h2oUnitVec[1]) * ye;
          hznAtAltPts[i+2] = (zo + pnow*h2oUnitVec[2]) * ze;
          if (horizonAltDebug) {
            System.out.println("scaledHorizon:"+Arrays.toString(scaledHzn));
            System.out.println("scaledHorizonAtAlt x="+(xo + p2*h2oUnitVec[0])+" y="+(yo + p2*h2oUnitVec[1])+" z="+(zo + p2*h2oUnitVec[2]));
            Position hznPos = new Position();
            hznPos.setCar(hznPts[i+0], hznPts[i+1], hznPts[i+2]);
            Position hznAtAlt = new Position();
            hznAtAlt.setCar(hznAtAltPts[i+0], hznAtAltPts[i+1], hznAtAltPts[i+2]);
            System.out.println("Position hzn:"+hznPos.toString(1));
            System.out.println("Position hznAtAlt:"+hznAtAlt.toString(1));
          }
        }
      }
      else {
        hznAtAltPts[i+0] = hznPts[i+0];
        hznAtAltPts[i+1] = hznPts[i+1];
        hznAtAltPts[i+2] = hznPts[i+2];
      }
    }
    return hznAtAltPts;
  }


  /** Convert horizon points to horizon at altitude points - based on more visual math.
   *<pre>
   *  Earth is an oblate spheroid. Earth + Altitude is a different oblate
   *  spheroid.
   *
   * [diagram 1] - big picture we want p2 which is the horizon pt extended
   * to an altitude - basically what is visible to the observer given the
   * target is at a given altitude. Step 1 is to convert to scale everything
   * to the scale needed to make Earth + Altitude a Unit Sphere.
   *
   *                                   aaa+(p2)
   *                                a  .... *a
   *                            a  .           +(horizon pt)
   *                          a  .            *  .* a
   *                        a  .             *     . *a
   *                       a  .             *        .  +(p1)
   *                      a  .             *          .  a *
   *                      a  .            c***********.**a****observer
   *                      a  .                        .  a
   *                       a  .                      .  a
   *                        a  .                    .  a
   *                          a  .                 .  a
   *                            a  .           .  a
   *                                a  .....  a
   **                                  aaaaa
   *
   * [diagram 2] In our scaled model, get the distance from the
   *  observer to the 90 degree intersection point that is along
   *  a vector from the center of the earth (A) and then distance
   *  from that intersect to the center of the earth (B)
   *  1) tc = L * Direction  (L is hypotenuse C)
   *     tc (a.k.a A) is the dot product between L and Direction (of the
   *     vector between observer and horizon point)
   *  2) d (a.k.a. B) = Math.sqrt(tc^2 - L^2)  formula: B^2 = Math.sqrt(C^2 - A^2)
   *
   *
   *                               +(horizon pt)
   *                              *
   *                             *
   *                        /   I
   *                       /   *90 *    tc (observer-to-I - a.k.a. A)
   *                      d   *       *
   *                     /   *          +(p1)
   *                    /   *              *
   *                   /   c******************observer
   *                       ----C (a.k.a. L)---
   *                       (observer-to-center)
   *
   *[diagram 3] Now distance from center of earth to the 90 degree
   * intersect (i.e. B), get the distance from the 90 degree intersect to p1
   * A' = Math.sqrt(1 - B^2)
   *
   *                                +(horizon pt)
   *                               *
   *                              *
   *                         /   I
   *                        /   *90 *    A' (p1-to-I)
   *                       d   *       *
   *                      /   *          +(p1)
   *                     /   *        *
   *                    /   *   *  C' = 1 = radius of our unit sphere
   *                   /   c
   *
   * [diagram 4] p2 is the same distance away from the 90 degree intersect
   * point as p1, so p2 = Observer + Direction * (A + A')
   *
   *                    +(p2)      +(horizon pt)
   *                       *
   *           (I-to-p2) A'   *
   *                         /   I
   *                        /   *90 *    A' (p1-to-I)
   *                       d   *       *
   *                      /   *          +(p1)
   *                     /   *        *
   *                    /   *   *  C' = 1 = radius of our unit sphere
   *                   /   c
   *
   *
   *
   *   observer - viewer (will be scaled based on alt bubble being unit sphere
   *   h        - one of the horizon points based on the observer/viewer (scaled)
   *   c        - point at center of earth
   *   a        - point at given alt above earth (scaled to make this unit sphere)
   *   .        - point along surface of earth (new scaling will result in this
   *               being oblate, but different ratios than ECEF
   *   p1       - first intersect with alt bubble (the one we want to ignore)
   *   p2       - second intersect which is observer horizon at the specified
   *               altitude (in our scale)
   *   ******   - lines
   *               [diagram 1]
   *               1) from observer/viewer to center of earth
   *               2) from center of earth horizon point
   *               3) from observer through t1 (on alt bubble), through horizon
   *                  point, and then to t2 (on alt bubble and the point we want)
   *               [diagram 2]
   *               1) from observer/viewer to center of earth
   *               2) from center of earth horizon point
   *               3) from observer through t1 (on alt bubble) to 90 degree
   *                  intersection point with line from center of earth to horizon
   *   [diagram 2]
   *   I        - 90 degree intersect point
   *   B        - distance from center of earth to 90 degree intersect point (I)
   *              (in scaled version)
   *   A        - distance from observer to  90 degree intersect point
   *   C        - distance from observer to center of earth
   *   where A^2 + B^2 = C^2
   *   and B = Math.sqrt(A^2 - C^2)
   *   [diagram 3]
   *   A'       - is also the distance from the 90 degree intersect (I) to t2
   *   let's call:
   *     OI the distance from the observer to the 90 degrees intersect
   *       OI = Ob2Hzn
   *   p2 = Observer + (direction * (OI + A))
   *</pre>
   *  based on Ray-Sphere-Intersection.html on kyleholiday.com/blog/tutorials/math/2013/12/24/
   *
   *  @param horizonPtsECEF horizon points in ECEF Cartesian double[]
   *  @param ioff           integer offset
   *  @param npts           number of Points
   *  @param alt            altitude
   *  @param observer       Position describing the viewer
   *  @return the horizon points in ECEF cartesian x,y,z
   *  @since NeXtMidas 4.1.4
    */
  @InternalUseOnly("Prototype version - use at your own risk")
  public static double[] horizon2HorizonAtAlt (double[] horizonPtsECEF, int ioff, int npts, double alt, Position observer) {

    if (horizonAltDebug) System.out.println("Using more visual math horizon2HorizonAtAlt method altitude="+alt);
    double[] horizonAtAlt = new double[npts*3];

    // declare variables used for scaled calculations
    double xo,yo,zo; // observer position x,y,z as scaled values
    double xdOb2Hzn,ydOb2Hzn,zdOb2Hzn; // observer to horizon point delta in scaled system
    double xOfAlt,yOfAlt,zOfAlt; // ECEF values of altitude at each axis
    double xOfAltInv,yOfAltInv,zOfAltInv;    // inverse ECEF values of altitude at each axis
    double ob2Ctr2,tc,pDotOb2HznVector;             // inner (dot) products

    // set up scaling so oblate earth + altitude is now represented as unit sphere
    xOfAlt = EARTH_EQUATORIAL_RADIUS + alt;
    yOfAlt = EARTH_EQUATORIAL_RADIUS + alt;
    zOfAlt = EARTH_EQUATORIAL_RADIUS*Math.sqrt(1.0-EARTH_ECCENTRICITY_SQUARED) + alt;
    xOfAltInv = 1.0/xOfAlt;
    yOfAltInv = 1.0/yOfAlt;
    zOfAltInv = 1.0/zOfAlt;

    // get viewer location in this new scaled "universe"
    xo = xOfAltInv*observer.x;
    yo = yOfAltInv*observer.y;
    zo = zOfAltInv*observer.z;
    double[] ob2ctrVec = new double[] {-xo,-yo,-zo}; // L

    ob2Ctr2 = xo*xo + yo*yo + zo*zo; // poo L^2 dot product (i.e. dist observer to center earth squared C^2 )

    for (int i=ioff,n=0; n < npts; i+=3,n++) {
      // scale the horizon points according to altitude sphere being unit sphere
      double xh = xOfAltInv*horizonPtsECEF[i+0];
      double yh = yOfAltInv*horizonPtsECEF[i+1];
      double zh = zOfAltInv*horizonPtsECEF[i+2];
      double[] scaledHznPts = new double[] {xh,yh,zh};
      // get direction vector components describing delta between observer/viewer and current horizon point
      xdOb2Hzn = xh - xo; // xd // to - from
      ydOb2Hzn = yh - yo; // yd
      zdOb2Hzn = zh - zo; // zd
      double[] dOb2Hzn = new double[] {xdOb2Hzn,ydOb2Hzn,zdOb2Hzn}; // effectively {l,m,n} slope vector
      double ob2HzDist = Vector3D.mag(dOb2Hzn);
      double[] direction = Vector3D.norm(dOb2Hzn);
      if (horizonAltDebug) {
        System.out.println(" ---- point "+n+" ---------------------");
        System.out.println("xo:"+xo+" xh:"+xh+" yo:"+yo+" yh:"+yh+" zo:"+zo+" zh:"+zh+" observer.z:"+observer.z+" observer.alt:"+observer.alt);
        System.out.println("ob2HzDist:"+ob2HzDist);
        System.out.println("direction unit vector:"+Arrays.toString(direction));
      }

      // dot product of scaled delta vector with itself
      pDotOb2HznVector = xdOb2Hzn*xdOb2Hzn + ydOb2Hzn*ydOb2Hzn + zdOb2Hzn*zdOb2Hzn; // pdd = qA
      if (pDotOb2HznVector<1.0e-30) {  // we already have our point: t is (-B + Sqrt(B^2))/2A = 0/2A = 0
        if (horizonAltDebug) System.out.println("1 intersect -> use original horizon pt");
        horizonAtAlt[i]   = horizonPtsECEF[i+0];
        horizonAtAlt[i+1] = horizonPtsECEF[i+1];
        horizonAtAlt[i+2] = horizonPtsECEF[i+2];
      }
      tc = Vector3D.dotProduct(ob2ctrVec, direction); // pod
      // B = Math.sqrt(A^2 - C^2)
      double o2h = Vector3D.dotProduct(dOb2Hzn, direction);
      double d = Math.sqrt(Math.abs(ob2Ctr2 - tc*tc));  // ?? d = Math.sqrt(tc^2 - L^2) (where d is distance from center to 90 deg intersect)
      if (horizonAltDebug) {
        System.out.println("tc:"+tc+" ob2Ctr:"+Math.sqrt(ob2Ctr2)+" ob2Ctr2:"+ob2Ctr2+" - tc*tc:"+(tc*tc));
        System.out.println("o2h:"+o2h);
        System.out.println("d:"+d+" d*d:"+(d*d));
      }

      if (d < 1) {
        double t1c = Math.sqrt(1-d*d); // t1c = Math.sqrt(radius^2 - d^2)
        double t2 = tc + t1c;
        // t2 = ob2HzDist; // temp for debug - gives correct points
        // p2 = observer + direction * t2
        double[] o2p2 = new double[] {direction[0]*t2,direction[1]*t2,direction[2]*t2}; // direction * t2
        double p2[] = new double[] {xo+o2p2[0], yo+o2p2[1], zo+o2p2[2]};

        // unscale back to ECEF Cartesian
        horizonAtAlt[i]   = p2[0]*xOfAlt;
        horizonAtAlt[i+1] = p2[1]*yOfAlt;
        horizonAtAlt[i+2] = p2[2]*zOfAlt;

        if (horizonAltDebug) {

          double t1 = tc - t1c;
          double[] o2p1 = new double[] {direction[0]*t1,direction[1]*t1,direction[2]*t1}; // direction * t1
          double p1[] = new double[] {xo+o2p1[0], yo+o2p1[1], zo+o2p1[2]};
          System.out.println("t1c:"+t1c+" t2:"+t2+" t1:"+t1);
          System.out.println("o2p2:"+Arrays.toString(o2p2));
          System.out.println("direction:"+Arrays.toString(direction));
          System.out.println("p1:"+Arrays.toString(p1));
          System.out.println("scaledHznPts:"+Arrays.toString(scaledHznPts));
          System.out.println("scaledHznAtAlt p2:"+Arrays.toString(p2));
          Position hznPos = new Position();
          hznPos.setCar(horizonPtsECEF[i+0], horizonPtsECEF[i+1], horizonPtsECEF[i+2]);
          Position hznAtAlt = new Position();
          hznAtAlt.setCar(horizonAtAlt[i+0], horizonAtAlt[i+1], horizonAtAlt[i+2]);
          System.out.println("Position hzn:"+hznPos.toString(1));
          System.out.println("Position hznAtAlt:"+hznAtAlt.toString(1));
        }
      }
      else {
        horizonAtAlt[i]   = horizonPtsECEF[i+0];
        horizonAtAlt[i+1] = horizonPtsECEF[i+1];
        horizonAtAlt[i+2] = horizonPtsECEF[i+2];
      }
    }
    return horizonAtAlt;
  }

  /** Spherical Earth based cone horizon point and cone extension point that
   *  gives equivalent results to the patch provided.
   *  IMPORTANT: This should only be used after the legacy code comes up
   *  with a horizon extension that give a point well off the earth.
   *  It is very efficient code, but it is not as accurate as oblate earth
   *  calculations.<br>
   *
   *  Note: this code is based on code contributed by Andrew McCall
   *
   *  @param vcHat                     normalized vector viewer to center of earth
   *  @param invDistViewer2Center      1 / (distance_from_viewer_to_center_of_earth)
   *  @param viewerXYZ                 X,Y,Z coordinates of viewer
   *  @param pt2Extend                 point on same plane as target that needs extending to the horizon
   *  @param distanceHorizonSpherical  distance to the horizon from viewer (assuming spherical earth)
   *  @param trimToGeosurface          true if result should be geosurface point,
   *                                   false if result should have altitude in line with original conic
   *  @return                       extended point X, Y, Z
   */
  private static double[] sphericalEarthConeToHorizon (double[] vcHat,double invDistViewer2Center,double[] viewerXYZ,double[] pt2Extend,
      double distanceHorizonSpherical, boolean trimToGeosurface) {
    double[] zhat = Vector3D.norm(Vector3D.crossProduct(pt2Extend, vcHat));
    double[] yhat = Vector3D.crossProduct(zhat, vcHat);
    double[] xhatXdistH = Vector3D.multByScalar(vcHat,distanceHorizonSpherical);
    double[] yhatXeer = Vector3D.multByScalar(yhat,EARTH_EQUATORIAL_RADIUS);

    double[] vectorViewer2Pt2Extend = Vector3D.multByScalar(Vector3D.add(xhatXdistH, yhatXeer),invDistViewer2Center);
    double[] vectorViewer2Result;
    if (trimToGeosurface) {
      // create vector describing viewer to horizon point (our result)
      vectorViewer2Result = Vector3D.multByScalar(vectorViewer2Pt2Extend,-distanceHorizonSpherical);
    }
    else {
      // create vector / reference frame point to extend to satellite
      // get unnormalized vector then normalize
      double[] phatNorm = Vector3D.norm(Vector3D.subtract(pt2Extend,viewerXYZ));
      // distance to horizon leverage point on plane satellite to horizon satellite to point to extend
      double distViewer2Result = distanceHorizonSpherical/Vector3D.dotProduct(vectorViewer2Pt2Extend, phatNorm);
      vectorViewer2Result = Vector3D.multByScalar(phatNorm,-distViewer2Result);
    }
    double result[] = Vector3D.add(viewerXYZ,vectorViewer2Result);
    return result;
  }

  /** When dealing with a viewer to cone point ray that will not intersect the earth
   *  1) Calculate the matching geosurface horizon point OR
   *  2) Calculate the point with the same lat, lon as the horizon point, but with
   *     an altitude such that the point is along the original ray
   *
   *  Note: this code is based on code contributed by Andrew McCall
   *
   *  @param vcHat      normalized vector from viewer to center of earth
   *  @param rmag       distance from viewer to center of earth
   *  @param viewerXYZ  viewer location (apex of cone)
   *  @param pt2Extend  point along cone surface (on plane with nadir point)
   *  @param trimToGeosurface  find the matching geosurface horizon point
   *  @return horizon point (either at geosurface or along original ray)
   */
  private static double[] oblateEarthConeToHorizon (double[] vcHat, double rmag, double[] viewerXYZ,
                           double[] pt2Extend, boolean trimToGeosurface) {

    // step 2 - calculate vector normal (perpendicular) to plane containing center of earth and the point
    double[] vNormPlaneCP = Vector3D.crossProduct(pt2Extend, vcHat);
    double vNormCPMag = Vector3D.mag(vNormPlaneCP);
    if (vNormCPMag < 1e-10) {
      // Input vectors pt2Extend and vcHat are colinear. Can not determine a single horizon point
      return null;
    }

    double[] ncpHat = Vector3D.multByScalar(vNormPlaneCP, 1/vNormCPMag);

    // step 3 - calculate additional vector in same plane as center of earth and point to extend
    double[] cpHat = Vector3D.crossProduct(ncpHat, vcHat);

    // Accounting for earth's oblateness...
    // The intersection of a plane with an oblate spheroid produces an ellipse
    // First, compute the rotation angle between this ellipse and the
    // <vcHat, cpHat, ncpHat> basis (a rotation about ncpHat)
    double[] vcHat_scaled = Vector3D.multByScalar(vcHat, INVERSE_EER); // x & y scaled by equatorial radius
    vcHat_scaled[2] *= INVERSE_POLAR_FLATENING_FACTOR; // z scaled by polar radius

    double[] cpHat_scaled = Vector3D.multByScalar(cpHat, INVERSE_EER);
    cpHat_scaled[2] *= INVERSE_POLAR_FLATENING_FACTOR;

    double num = 2 * Vector3D.dotProduct(vcHat_scaled, cpHat_scaled);
    double den = Vector3D.dotProduct(vcHat_scaled, vcHat_scaled) -
        Vector3D.dotProduct(cpHat_scaled, cpHat_scaled);

    // compute rotation angle needed to align semimajor (xhat) and semiminor (yhat)
    // with ellipse that is formed by plane slicing through center of earth
    // and horizon point
    double cos_o; // cosine of omega
    double sin_o; // sine of omega
    if (num == 0) {
      // omega = 0
      cos_o = 1;
      sin_o = 0;
    }
    else if (den == 0 ) {
      // omega = PI / 4
      cos_o = sin_o = INVERSE_SQRT_2;
    }
    else {
      double omega = 0.5 * Math.atan(num/den);
      cos_o = Math.cos(omega);
      sin_o = Math.sin(omega);
    }

    // Using the rotation angle, compute the new vcHat and cpHat basis vectors
    // Note: rotation is done about the nc axis
    double[] vcHat_rot = Vector3D.add(Vector3D.multByScalar(vcHat, cos_o),
                        Vector3D.multByScalar(cpHat, sin_o));
    double[] cpHat_rot = Vector3D.add(Vector3D.multByScalar(vcHat, -sin_o),
                        Vector3D.multByScalar(cpHat, cos_o));

    double[] vcHat_rot_scaled = Vector3D.multByScalar(vcHat_rot, INVERSE_EER);
    vcHat_rot_scaled[2] *= INVERSE_POLAR_FLATENING_FACTOR;

    double[] cpHat_rot_scaled = Vector3D.multByScalar(cpHat_rot, INVERSE_EER);
    cpHat_rot_scaled[2] *= INVERSE_POLAR_FLATENING_FACTOR;

    // Compute parameters for the ellipse in this basis (semimajor & semiminor axis)
    double semimajorSquared = 1 / Vector3D.dotProduct(vcHat_rot_scaled, vcHat_rot_scaled);
    double semiminorSquared = 1 / Vector3D.dotProduct(cpHat_rot_scaled, cpHat_rot_scaled);

    // Compute viewer location in new basis
    // Note: we have redefined the coordinate system to align with the ellipse
    //       now we transform the viewer location by rotating off x in the y direction.
    //       At this point we are working in 2D.
    double x_radius = rmag * cos_o;
    double y_radius = rmag * sin_o;

    // Compute slope of tangent line between viewer and earth horizon
    double xRadSquared_semimajorSquared = x_radius*x_radius - semimajorSquared;

    double m = (x_radius*y_radius -
        Math.sqrt(semimajorSquared*y_radius*y_radius + semiminorSquared*xRadSquared_semimajorSquared)) / xRadSquared_semimajorSquared;

    // Compute point of tangency on ellipse
    double cInverse = 1 / Math.sqrt(semimajorSquared*m*m + semiminorSquared);
    double point_x = -semimajorSquared*m * cInverse;
    double point_y = -semiminorSquared * cInverse;

    // Transform points back to ECI
    double[] geosurface = Vector3D.add(Vector3D.multByScalar(vcHat_rot, point_x),
                     Vector3D.multByScalar(cpHat_rot, point_y));

    if (trimToGeosurface) {
      return geosurface;
    }
    else {
      // normalized vector viewer to original point to extend
      double[] vpHat = Vector3D.norm(Vector3D.subtract(pt2Extend,viewerXYZ));
      // vector viewer to geosurface (horizon) point
      double[] vhHat = Vector3D.subtract(geosurface,viewerXYZ);
      // distance viewer to horizon point on geosurface
      double horizonDistance = Vector3D.mag(vhHat);
      // normalize the viewer to geosurface (horizon) vector
      vhHat = Vector3D.multByScalar(vhHat, 1/horizonDistance);
      // adjusted distance to account for angle
      double distance2HorizonPt = horizonDistance / Vector3D.dotProduct(vhHat, vpHat);
      // add vector viewer to off surface horizon point to position of viewer
      double[] offSurfaceHorizonPoint = Vector3D.add(Vector3D.multByScalar(vpHat, distance2HorizonPt), viewerXYZ);
      return offSurfaceHorizonPoint;
    }

  }

  /** Convert data from ECR {@link Units#CARTESIAN} to {@link Units#GEODETIC} coordinate system. There
      is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
   */
  public static void car2geo (double[] v, int mpva, int nrec) {
    double x,y,z,a,b,c,eer,ee,ee1,ee1i,r,t,f1,f2,sa,ca,h,da,dh;
    ee = EARTH_ECCENTRICITY*EARTH_ECCENTRICITY;
    ee1 = (1-ee); ee1i = 1/ee1;
    eer = EARTH_EQUATORIAL_RADIUS;
    int isk = 3*mpva;
    for (int i=0,n=0; n<nrec; n++,i+=isk) {
      x = v[i+0];
      y = v[i+1];
      z = v[i+2];
      if (x==0 && y==0) {
        v[i+0] = Math.abs(z) - eer*Math.sqrt(ee1);
        v[i+1] = (z<0)? -90 : 90;
        v[i+2] = 0;
        continue;
      }
      r = Math.sqrt(x*x+y*y+z*z);
      b = Math.sqrt(x*x+y*y);
      c = Math.asin(z/r);
      a = Math.atan( Math.tan(c)*ee1i );
      sa = Math.sin(a);
      h = r - eer/Math.sqrt(1+ee*sa*sa*ee1i);
      do {
        sa = Math.sin(a); ca = Math.cos(a);
        t = 1./Math.sqrt(1-ee*sa*sa);
        f1 = (h + eer*t)*ca - b;
        f2 = (h + eer*ee1*t)*sa - z;
        da = (-f1*sa+f2*ca)/(h+eer*ee1*t*t*t);
        dh = f1*ca+f2*sa;
        a -= da; h -= dh;
      } while (da>.0001 || dh>.0001);
      v[i+0] = h;
      v[i+1] = RAD2DEG*a;
      v[i+2] = RAD2DEG*Math.atan2(y,x);
      if (mpva<=1) continue;
    }
  }

  /** <b>Not implemented yet:</b>
      Convert data from {@link Units#ELLIPSOID} to {@link Units#CYLINDRIC} coordinate system. There
      is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
   */
  public static void car2cyl (double[] v, int mpva, int nrec) {
    Shell.warning("Transform.car2cyl is not yet coded");
  }

  /** <b>Not implemented yet:</b>
      Convert data from {@link Units#ELLIPSOID} to {@link Units#ELLIPSOID} coordinate system. There
      is no return value since the values are changed within the array.
      @param v    Array of position vectors.
      @param mpva Mode: {@link #POS}, {@link #POSVEL}, {@link #POSVELACC}
      @param nrec The number of records to convert (1..N).
   */
  public static void car2ell (double[] v, int mpva, int nrec) {
    Shell.warning("Transform.car2ell is not yet coded");
  }

  /** Construct an AZIMUTHAL-EQUIDISTANT {@link Units#CARTESIAN} array from an ECEF (ECR) {@link Units#GEODETIC} array.

      Note: As written, a point exactly at nadir (the South Pole) will somewhat
            arbitrarily map to a point somewhere on a circle PI*POLAR_RADIUS_IN_METERS from the origin,
            depending only on the input value for longitude.
      @param   ecefGeo      Array of ECEF (ECR) {@link Units#GEODETIC} position vectors in {@link #POS} Mode.
      @return  Array of ECR {@link Units#CARTESIAN} AZIMUTHAL-EQUIDISTANT coordinates with ordering
               {x, y, POLAR_RADIUS_IN_METERS}.
      @since NeXtMidas 3.5.3   Bug 2873
   */
  public static double[] ecefGeo2ecefCarAzEq (final double[] ecefGeo) {
    final int LENGTH = (ecefGeo.length/3) * 3; // defensively round down if not given a multiple of 3
    final double[] ecrCarAzEq = new double[LENGTH];
    for (int ii=0; ii<LENGTH;) {
      final double dist = Math.toRadians(90.0-ecefGeo[ii+1]);
      final double azim = Math.toRadians(ecefGeo[ii+2]);
      ecrCarAzEq[ii++] = POLAR_RADIUS_IN_METERS*dist*Math.cos(azim); // x
      ecrCarAzEq[ii++] = POLAR_RADIUS_IN_METERS*dist*Math.sin(azim); // y
      ecrCarAzEq[ii++] = POLAR_RADIUS_IN_METERS; // z
     }
    return ecrCarAzEq;
  }

  /** Alias for ecefGeo2ecefCarAzEq(final double[] ecefGeo)
      @param ecrGeo see linked method for docs
      @return       see linked method for docs
      @see #ecefGeo2ecefCarAzEq(double[])
      @since NeXtMidas 3.5.3   Bug 2873
   */
  public static double[] ecrGeo2ecrCarAzEq (final double[] ecrGeo) {
    return ecefGeo2ecefCarAzEq(ecrGeo);
  }

  /** ECR/Cartesian/Alternate to ECR/Cartesian

      ECR/Cartesian/Alternate is identical to ECR/Cartesian except that the
      {x, y, z} ordinates are for an alternate Cartesian reference frame.  In
      this alternate frame of reference x, y, and z still have their conventional
      meanings, but the location of the North Pole is defined to be the center
      of a given "area of interest".  Simple spherical transforms are used to
      convert between "ecrCarAlt" and "ecrCar".

      @param lat latitude in ECR/Cartesian/Alternate coordinates
      @param lon longitude in ECR/Cartesian/Alternate coordinates
      @return ECR/Cartesian TMatrix
      @since NeXtMidas 3.5.3   Bug 2873
  */
  public static TMatrix ecefCarAlt2ecefCarTMatrix (final double lat, final double lon) {
    final TMatrix tm = new TMatrix();
    tm.a11 = -Math.sin(Math.toRadians(lon));
    tm.a21 =  Math.cos(Math.toRadians(lon));
    tm.a31 =  0;
    tm.a32 =  Math.cos(Math.toRadians(lat));
    tm.a33 =  Math.sin(Math.toRadians(lat));
    tm.a12 = -tm.a21*tm.a33;
    tm.a13 =  tm.a21*tm.a32;
    tm.a22 =  tm.a11*tm.a33;
    tm.a23 = -tm.a11*tm.a32;
    return tm;
  }

  /** Alias for ecefCarAlt2ecefCarTMatrix(final double lat, final double lon)
      @param lat  see linked method for docs
      @param lon  see linked method for docs
      @return     see linked method for docs
      @see #ecefCarAlt2ecefCarTMatrix(double, double)
      @since NeXtMidas 3.5.3   Bug 2873
   */
  public static TMatrix ecrCarAlt2ecrCarTMatrix (final double lat, final double lon) {
    return ecefCarAlt2ecefCarTMatrix(lat,lon);
  }

  /** ECR/Cartesian to ECR/Cartesian/Alternate

      ECR/Cartesian/Alternate is identical to ECR/Cartesian except that the
      {x, y, z} ordinates are for an alternate Cartesian reference frame.  In
      this alternate frame of reference x, y, and z still have their conventional
      meanings, but the location of the North Pole is defined to be the center
      of a given "area of interest".  Simple spherical transforms are used to
      convert between "ecrCar" and "ecrCarAlt".

      @param lat latitude in ECR/Cartesian coordinates
      @param lon longitude in ECR/Cartesian coordinates
      @return ECR/Cartesian/Alternate TMatrix
      @since NeXtMidas 3.5.3   Bug 2873
   */
  public static TMatrix ecefCar2ecefCarAltTMatrix (final double lat, final double lon) {
    final TMatrix tm = new TMatrix();
    tm.a11 = -Math.sin(Math.toRadians(lon));
    tm.a12 =  Math.cos(Math.toRadians(lon));
    tm.a13 =  0;
    tm.a23 =  Math.cos(Math.toRadians(lat));
    tm.a33 =  Math.sin(Math.toRadians(lat));
    tm.a21 = -tm.a12*tm.a33;
    tm.a22 =  tm.a11*tm.a33;
    tm.a31 =  tm.a12*tm.a23;
    tm.a32 = -tm.a11*tm.a23;
    return tm;
  }

  /** Alias for ecefCar2ecefCarAltTMatrix(final double lat, final double lon)
      @param lat see linked method for docs
      @param lon see linked method for docs
      @return    see linked method for docs
      @see #ecefCar2ecefCarAltTMatrix(double, double)
      @since NeXtMidas 3.5.3   Bug 2873
   */
  public static TMatrix ecrCar2ecrCarAltTMatrix (final double lat, final double lon) {
    return ecefCar2ecefCarAltTMatrix(lat,lon);
  }

  /** ECEF/Geodetic to ECEF/Geodetic (depending on the TMatrix passed in, this
      will convert either ecefGeo2ecefGeoAlt or ecefGeoAlt2ecefGeo)

      ECEF/Geodetic/Alternate is identical to ECEF/Geodetic except that the
      {alt, lat, lon} ordinates are for an alternate geodetic reference frame.
      In this alternate frame of reference latitude and longitude still have
      their conventional meanings and number ranges, but the location of the
      North Pole is defined to be the center of a given "area of interest.
      Simple spherical transforms are used to convert between "ecrGeo" and
      "ecrGeoAlt"

      @param tm     TMatrix
      @param ecefGeo ECEF/Geodetic or ECEF/Geodetic/Alternate
      @return ECEF/Geodetic/Alternate or ECEF/Geodetic depending on TMatrix
      @since NeXtMidas 3.5.3   Bug 2873
   */
  public static double[] ecefGeo2ecefGeo (final TMatrix tm, final double[] ecefGeo) {
    final int LENGTH = (ecefGeo.length/3) * 3; // defensively round down if not given a multiple of 3
    final double[] eg_ = new double[LENGTH];
    for (int i=0; i<LENGTH;) {
      // analogous to Transform.geo2car()
      final double alt  = ecefGeo[i];
      final double lat  = Math.toRadians(ecefGeo[i+1]); // lat
      final double lon  = Math.toRadians(ecefGeo[i+2]); // lon
      final double clat = Math.cos(lat);
      final double x    = Math.cos(lon)*clat;
      final double y    = Math.sin(lon)*clat;
      final double z    = Math.sin(lat);

      // analogous to Transform.matrixVectorMultA()
      final double x_ = x*tm.a11 + y*tm.a12 + z*tm.a13;
      final double y_ = x*tm.a21 + y*tm.a22 + z*tm.a23;
      final double z_ = x*tm.a31 + y*tm.a32 + z*tm.a33;

      // analogous to Transform.car2geo()
      eg_[i++] = alt; // alt
      if (z_>=1.0) { // preclude asin() domain errors
        eg_[i++] = 90.0; // lat
        eg_[i++] = 0.0; // lon
      } else if (z_<=-1.0) { // preclude asin() domain errors
        eg_[i++] = -90.0; // lat
        eg_[i++] = 0.0; // lon
      } else if (x_==0.0 && y_==0.0) { // preclude atan2() domain errors
        eg_[i++] = (z_>0.0) ? 90.0 : -90.0; // lat
        eg_[i++] = 0.0; // lon
      } else {
        eg_[i++] = Math.toDegrees(Math.asin(z_)); // lat
        eg_[i++] = Math.toDegrees(Math.atan2(y_,x_)); // lon
      }
    }
    return eg_;
  }

  /** Alias for ecefGeo2ecefGeo(final TMatrix tm, final double[] ecefGeo)
      @param tm       see linked method for docs
      @param ecrGeo   see linked method for doc
      @return    see linked method for docs
      @see #ecefGeo2ecefGeo(TMatrix, double[])
      @since NeXtMidas 3.5.3   Bug 2873
   */
  public static double[] ecrGeo2ecrGeo (final TMatrix tm, final double[] ecrGeo) {
    return ecefGeo2ecefGeo(tm, ecrGeo);
  }

  private static TMatrix top2ecrTMatrix (Transform t) {
    double t1,t2,slat,clat,slon,clon,x,y,z;
//JEFF    if (t.lat>90 || t.lat<-90) return null;
    slat = Math.sin(t.lat*DEG2RAD); clat = Math.cos(t.lat*DEG2RAD);
    slon = Math.sin(t.lon*DEG2RAD); clon = Math.cos(t.lon*DEG2RAD);
    t1 = EARTH_ECCENTRICITY*EARTH_ECCENTRICITY;
    t2 = EARTH_EQUATORIAL_RADIUS/Math.sqrt(1-t1*slat*slat);
    x = (t.alt+t2)*clat*clon;
    y = (t.alt+t2)*clat*slon;
    z = (t.alt+(1-t1)*t2)*slat;
    TMatrix tm = new TMatrix();
    tm.rotate(t.roll,TMatrix.ROLL);          // local ROLL
    tm.rotate(t.elev,TMatrix.ELEV);          // local ELEVation
    tm.rotate(t.azim,TMatrix.AZIM);          // local AZIMuth
    tm.rotate(90.0-t.lat,TMatrix.ROLL);      // topo lat roll
    tm.rotate(90.0+t.lon,TMatrix.AZIM);      // topo lon AZIMuth
    tm.offset(x,y,z);                        // topo origin
    return tm;
  }

  private static void matrixVectorMultiply (TMatrix tm, double[] v, int mpva, int nrec) {
    switch (mpva) {
      case 0:
        matrixVectorMultA(tm,v,0,3,nrec);
        break;
      case 1:     // POS
        matrixVectorMultAB(tm,v,0,3,nrec);
        break;
      case 2:     // POS,VEL
        matrixVectorMultAB(tm,v,0,6,nrec);
        matrixVectorMultA(tm,v,3,6,nrec);
        break;
      case 3:     // POS,VEL
        matrixVectorMultAB(tm,v,0,9,nrec);
        matrixVectorMultA(tm,v,3,9,nrec);
        matrixVectorMultA(tm,v,6,9,nrec);
        break;
    }
  }

  private static void matrixVectorMultA (TMatrix tm, double[] v, int ioff, int isk, int nrec) {
    double x,y,z; int i,n;
    for (i=ioff,n=0; n<nrec; i+=isk,n++) {
      x=v[i+0]; y=v[i+1]; z=v[i+2];
      v[i+0] = x*tm.a11 + y*tm.a12 + z*tm.a13;
      v[i+1] = x*tm.a21 + y*tm.a22 + z*tm.a23;
      v[i+2] = x*tm.a31 + y*tm.a32 + z*tm.a33;
    }
  }

  private static void matrixVectorMultAB (TMatrix tm, double[] v, int ioff, int isk, int nrec) {
    double x,y,z; int i,n;
    for (i=ioff,n=0; n<nrec; i+=isk,n++) {
      x=v[i+0]; y=v[i+1]; z=v[i+2];
      v[i+0] = x*tm.a11 + y*tm.a12 + z*tm.a13 + tm.b1;
      v[i+1] = x*tm.a21 + y*tm.a22 + z*tm.a23 + tm.b2;
      v[i+2] = x*tm.a31 + y*tm.a32 + z*tm.a33 + tm.b3;
    }
  }


  /** Use the combo Jeff's code and Andrew's spherical earth code
   *  Note: This option will go away. It is being left in
   *  during evaluation phase.
   *
   *  @param useSphereConeCode use the combo Jeff's code and Andrew's spherical earth code
   */
  @InternalUseOnly
  public static void setUseSphericalConeCode (boolean useSphereConeCode) {
    Transform.useSphericalConeCode = useSphereConeCode;
    if (useSphereConeCode) {
      Transform.useLegacyConeCode = false;
    }
  }

  /** Are we using the combo Jeff's code and Andrew's spherical earth code
   *  @return true if using the combo Jeff's code and Andrew's spherical earth code
   */
  @InternalUseOnly
  public static boolean getUseSphericalConeCode () {
    return Transform.useSphericalConeCode;
  }

  /** set if using legacy code
   *
   *  @param useLegacyConeCode true means use legacy code
   */
  public static void setUseLegacyConeCode (boolean useLegacyConeCode) {
    Transform.useLegacyConeCode = useLegacyConeCode;
    if (useLegacyConeCode) {
      Transform.useSphericalConeCode = false;
    }
  }

  /** Get if using legacy cone code
   *
   *  @return true if using legacy code
   */
  public static boolean getUseLegacyConeCode () {
    return Transform.useLegacyConeCode;
  }

  /** Debug mode that allows cone footprint implementations to be compared
   *
   *   @param compare true if we are in this debug mode
   */
  @InternalUseOnly
  public static void setCompareConeCode (boolean compare) {
    Transform.compareConeCode = compare;
  }

  /** Get if we are in a debug mode where the various implementations
   *  of cone footprint calculation are being compared
   *
   *  @return true if in this debug mode
   */
  @InternalUseOnly
  public static boolean getCompareConeCode () {
    return Transform.compareConeCode;
  }

  /**
   * Set the cone code comparisons to be enabled.
   * @param value This is a mask value of the cone comparison
   */
  @InternalUseOnly
  public static void setCompareConeList(String value) {
    setCompareConeList(Parser.mask(compareConeList, value, Transform.coneCodeComparisons));
  }

  /**
   * Set the cone code comparisons to be enabled.
   * @param value This is a mask value of the cone comparison
   */
  @InternalUseOnly
  public static void setCompareConeList(int value) {
    Transform.coneCodeComparisons = value;
  }

  /** Get the fields to be enabled.
   *  @return The cone code comparisons enabled
   */
  @InternalUseOnly
  public static String getCompareConeList() {
    return Parser.mask2s(compareConeList, Transform.coneCodeComparisons);
  }

  /** Used to support cone calculation comparison
   *  @param codeCode int cone code to check
   *  @return true if being compared
   */
  @InternalUseOnly
  public static boolean isOnCompareList (int codeCode) {
    return ((codeCode & coneCodeComparisons) != 0);
  }

  /** Set if horizon at altitude debug is enabled
   *  @param enabled true to enable
   */
  @InternalUseOnly
  public static void setHorizonAltDebug (boolean enabled) {
    Transform.horizonAltDebug = enabled;
  }

}