package nxm.sys.test.junit.lib;

import static nxm.sys.inc.Constants.EARTH_EQUATORIAL_RADIUS;

import java.util.Arrays;

import nxm.sys.inc.Constants;
import nxm.sys.inc.Units;
import nxm.sys.lib.Convert;
import nxm.sys.lib.GeodeticUtil;
import nxm.sys.lib.Midas;
import nxm.sys.lib.Parser;
import nxm.sys.lib.Position;
import nxm.sys.lib.TMatrix;
import nxm.sys.lib.Time;
import nxm.sys.lib.Transform;
import nxm.sys.lib.Vector3D;
import nxm.sys.test.junit.NMTestCase;

/** This will test the Transform class.

    @author  Neon Ngo
 */
public final class TransformTest extends NMTestCase {
  static boolean debug = false;

  /** public constructor to allow running this test case directly with JUnit. */
  public TransformTest (String methodName) { this(methodName, null); }

  /** This constructor is required by NeXtMidas Unit Testing framework.
      @param methodName The name of the method being called.
      @param midas      The midas context to use when running the tests.
   */
  public TransformTest (String methodName, Midas midas) {
    super(methodName, midas);
  }

  // for turning on/off debugging via 'DEBUG on nxm.sys.test.junit.lib.TransformTest'
  public static boolean getDebug () { return debug; }
  public static void setDebug (boolean val) { debug = val; }

  //////////////////////////////////////////////////////////////////////////////

  // Not tested - Deprecated
  public void testGeo2ecr () { } // see testGeo2car()
  public void testEcr2geo () { } // see testCar2geo()

  public void testClipAtEarth () {
    boolean setToLegacyCode = Transform.getUseLegacyConeCode();
    int div = 60; // number of divisions (points for each cone ring

    /* Test new Transform.clipAtEarth code against original Transform.clipAtEarth code
     * Scenario #1: cone does not intersect with the Earth
     * Scenario #2: cone partially intersects with the Earth
     * Scenario #3: cone completely intersects with the Earth
     * Note: legacy code gives inaccurate results when then cone does not
     * intersect the earth (code accurate enough for plots)
     */
    //                            lonP  latP        altP lonQ  latQ    altQ   angle
    double[][] testClipAtEarth = {{ 20,   15,      3.7E7,  20,   50,      0,  14.75},  // Scenario #1
                                  {  0,   90,        3E7,   0,   90,      0,     14},
                                  {  0,  -90,        3E7,   0,  -90,      0,     14},
                                  {  0,    0,        3E7,   0,    0,      0,     14},
                                  {  0,    0,        3E7,  60,    0,      0,     22},
                                  {  0,    0,        3E7,  45,   45,      0,     22},
                                  { 20,  -20,        3E7,   0,   30,      0,     22},
                                  {  0,   45,        3E6,   0,   90,      0,      4},  // Scenario #2
                                  {  0,   45,        3E6,   0,   90,      0,     14},
                                  {  0,   60,        3E7,   0,   90,      0,     14},
                                  {  0,  -45,        3E6,   0,  -90,      0,      4},
                                  {  0,  -45,        3E6,   0,  -90,      0,     14},
                                  {  0,  -60,        3E7,   0,  -90,      0,     14},
                                  {  0,    0,        3E7,  60,    0,      0,     10},
                                  {  0,    0,        3E7,  45,   45,      0,     10},
                                  { 20,  -20,        3E7,   0,   30,      0,     10},
                                  {  0,  -30,      3.7E7,   0,    0,      0,     13},
                                  { 16, -3.9, 3.481502E7,  67, 34.3, 0.1997, 0.6332},
                                  {  0,    0,      3.7E6,   0,    0,      0,      8}}; // Scenario #3

    // Whether or not each cone should touch the Earth
    boolean[] noEarthIntersects = {true,true,true,true,true,true,true,     // Scenario #1
        false,false,false,false,false,false,false,false,false,false,false, // Scenario #2
        false};                                                            // Scenario #3

    // Check scenario with default code
    Transform.setUseLegacyConeCode(false);
    boolean[] shouldPass = {true,true,true,true,true,true,true, // Scenario #1
        true,true,true,true,true,true,true,true,true,true,true, // Scenario #2
        true};                                                  // Scenario #3
    checkPoints(testClipAtEarth,noEarthIntersects,div, shouldPass, shouldPass, "default code: ");

    // Check scenario with legacy code
    Transform.setUseLegacyConeCode(true);
    shouldPass = new boolean[] {false,false,false,false,false,false,false, // Scenario #1
        false,false,false,false,false,false,false,false,false,false,false, // Scenario #2
        true};                                                             // Scenario #3
    checkPoints(testClipAtEarth,noEarthIntersects,div, shouldPass, shouldPass, "legacy code: ");

    // Restore setting - note we are not accounting for some of the temporary alternatives
    Transform.setUseLegacyConeCode(setToLegacyCode);
  }

  private void checkPoints (double[][] testClipAtEarth, boolean[] noEarthIntersects, int div,
      boolean[] geosurfacePass, boolean[] aboveSurfacePass, String testInfo) {
    for (int i = 0; i < testClipAtEarth.length; i++) {
      Position p = Position.fromGeo(testClipAtEarth[i][2],testClipAtEarth[i][1],testClipAtEarth[i][0]); // POS
      Position q = Position.fromGeo(testClipAtEarth[i][5],testClipAtEarth[i][4],testClipAtEarth[i][3]); // POSP
      double[] viewer = {p.getX(), p.getY(),p.getZ()};
      double[] coneClipAtQ = GeodeticUtil.computeConeLLA(p, q, testClipAtEarth[i][6], div, 0, 0);
      double[] coneClipAtGeosurface = GeodeticUtil.computeConeLLA(p, q, testClipAtEarth[i][6], div, 0, 1,true);
      if (Transform.getUseLegacyConeCode()) {
        // zero out altitude on legacy version
        for (int j=2; j < coneClipAtGeosurface.length; j +=3) {
          coneClipAtQ[j] = 0;
        }
      }
      double[] coneOutFromEarth = GeodeticUtil.computeConeLLA(p, q, testClipAtEarth[i][6], div, 0, 1,false);
      double[] coneOutFromEarthLLA = Arrays.copyOf(coneOutFromEarth, coneOutFromEarth.length);

      int nPts = div+1;
      Transform.all2lla(coneClipAtQ,nPts); // really doing lla2all - same operation
      Transform.geo2car(coneClipAtQ, Transform.POS, nPts);
      Transform.all2lla(coneClipAtGeosurface,nPts); // really doing lla2all - same operation
      Transform.geo2car(coneClipAtGeosurface, Transform.POS, nPts);
      Transform.all2lla(coneOutFromEarth,nPts); // really doing lla2all - same operation
      Transform.geo2car(coneOutFromEarth, Transform.POS, nPts);

      String geosurfacePtsErrorMessage = "";
      boolean validConeExtensionPts = true;
      boolean nonIntersectingConesFullyChecked = true;
      for (int j=0; j < coneClipAtQ.length; j+=3) {
        double[] thisClipAtQ = {coneClipAtQ[j+0],coneClipAtQ[j+1],coneClipAtQ[j+2]};
        double[] thisClipAtGeosurface = {coneClipAtGeosurface[j+0],coneClipAtGeosurface[j+1],coneClipAtGeosurface[j+2]};
        double[] thisOutFromEarth = {coneOutFromEarth[j+0],coneOutFromEarth[j+1],coneOutFromEarth[j+2]};

        if (coneOutFromEarthLLA[j+2] > 0.1) {
          geosurfacePtsErrorMessage += validateHorizonPoint(viewer,thisClipAtQ, thisClipAtGeosurface, 1e-11,(j/3));
          validConeExtensionPts &= validateAltitudePoint(viewer,thisClipAtGeosurface, thisOutFromEarth, 1e-11,(j/3));
        }
        else if (noEarthIntersects[i]) {
          System.out.println(testInfo+"Cone "+i+" has ray (point "+(j/3 +1)+") that seems to be an earth intersect");
          nonIntersectingConesFullyChecked = false;
        }
      }
      boolean validGeoSurfacePoint = geosurfacePtsErrorMessage.isEmpty();
      if (geosurfacePass[i]) {
        assertTrue(testInfo+"Successful geosurface trim (for appropriate cone points) "+i+geosurfacePtsErrorMessage,validGeoSurfacePoint);
      }
      else {
        assertFalse(testInfo+"Successful geosurface trim (for appropriate cone points) "+i+geosurfacePtsErrorMessage,validGeoSurfacePoint);
      }
      if (aboveSurfacePass[i]) {
        assertTrue(testInfo+"Successful cone extension above geosurface pt (for appropriate cone points) "+i,validConeExtensionPts);
      }
      if (noEarthIntersects[i]) assertTrue(testInfo+"non-earth-intersecting cone was fully checked "+i,nonIntersectingConesFullyChecked);
    }
  }

  /** Validates points that have been extended to the horizon (trimmed at the geosurface)
   *  Checks that it is indeed a horizon point and that it is the corect one.
   *
   *  @param viewer              viewer position as double with x,y,z
   *  @param ptClipAtQ           point clipped at plane defined by target location
   *  @param ptTrimAtGeosurface  point extended and trimmed to geosurface
   *  @param tolerance           tolerance for tests
   *  @param ptNum               point index
   *  @return error message or empty string if point is valid
   */
  private String validateHorizonPoint (double[] viewer, double[] ptClipAtQ, double[] ptTrimAtGeosurface, double tolerance, int ptNum) {
    // compute proximity to surface of ellipsoid
    String errorMsg = "";
    double[] horizonScaled = Vector3D.multByScalar(ptTrimAtGeosurface, 1/EARTH_EQUATORIAL_RADIUS);
    horizonScaled[2] /= (1-Constants.EARTH_FLATTENING_FACTOR);

    double solution = Vector3D.mag(horizonScaled) - 1;
    boolean onSurface = solution < tolerance;

    if (!onSurface) errorMsg += "\n   Solution is not on surface of ellipsoid (pt:"+ptNum+")";

    // compute gradient at point on ellipsoid
    double[] horizonScaled2 = Vector3D.multByScalar(horizonScaled, 1/EARTH_EQUATORIAL_RADIUS);
    horizonScaled2[2] /= (1-Constants.EARTH_FLATTENING_FACTOR);

    double[] gHat = Vector3D.multByScalar(horizonScaled2, 2);

    // confirm tangency (gradient vector is perpendicular to vector connecting
    // viewer with surface point
    double[] vhHat = Vector3D.subtract(ptTrimAtGeosurface, viewer);
    vhHat = Vector3D.norm(vhHat);

    solution = Vector3D.dotProduct(gHat, vhHat);
    boolean tangentToSurface = solution < tolerance;

    if (!tangentToSurface) errorMsg += "\n   Solution is not a point of tangency for the ellipsoid (pt:"+ptNum+")";

    double vcMag = Vector3D.mag(viewer);
    double[] vcHat = Vector3D.norm(viewer, vcMag);

    double[] vpHat = Vector3D.subtract(ptClipAtQ, viewer);
    vpHat = Vector3D.norm(vpHat);

    // Verify that horizon point is in the same plane as point p
    // plane intersects earth's center
    double[] z = Vector3D.crossProduct(vpHat, vcHat);
    z = Vector3D.norm(z);

    solution = Vector3D.dotProduct(vhHat, z);
    boolean isInSamePlane = solution < tolerance;

    if (!isInSamePlane) errorMsg += "\n   Solution is not in the same plane as input point ptClipAtQ (pt:"+ptNum+") solution:"+solution;

    // Verify the the horizon point is the closest solution to ptClipAtQ
    double phAngle = Math.abs(Math.acos(Vector3D.dotProduct(vhHat, vpHat)));
    double rhAngle = Math.abs(Math.acos(Vector3D.dotProduct(vhHat, vcHat)));
    boolean correctHorizon = phAngle < rhAngle;

    if (!correctHorizon) errorMsg += "\n   Solution is not the horizon point closest to point ptClipAtQ (pt:"+ptNum+")";

    return errorMsg;
  }

  /** Validates that a point extended to the horizon, but not clipped to the geosurface is correct
   *  The point out from the earth should have the same lat, lon as the geosurface (horizon) point,
   *  but its altitude should keep it in line with the orignal conic
   *
   *  @param viewer              viewer position as double with x,y,z
   *  @param ptTrimAtGeosurface  point extended and trimmed to geosurface
   *  @param ptOutFromEarth      point extended along orignal conic
   *  @param tolerance           tolerance for tests
   *  @param ptNum               point index
   *  @return true if valid
   */
  private boolean validateAltitudePoint (double[] viewer, double[] ptTrimAtGeosurface, double[] ptOutFromEarth, double tolerance, int ptNum) {
    double horizonScaled[] = Vector3D.multByScalar(ptTrimAtGeosurface,
        1/(EARTH_EQUATORIAL_RADIUS*EARTH_EQUATORIAL_RADIUS));
    horizonScaled[2] /= (1-Constants.EARTH_FLATTENING_FACTOR);

    double[] hHat = Vector3D.norm(horizonScaled);

    // Confirm altitude is directly above horizon point (local altitude)
    double coHat[] = Vector3D.subtract(ptOutFromEarth, ptTrimAtGeosurface);
    double rhat_mag = Vector3D.mag(coHat);
    if (rhat_mag < tolerance) return true; // cone right at surface of earth
    coHat = Vector3D.norm(coHat,rhat_mag);

    double solution = Vector3D.dotProduct(hHat, coHat) - 1;
    boolean normalToSurface = solution < tolerance;

    if (!normalToSurface) {
      getMidas().warning("Altitude solution is not directly above specified ground point: tolerance="+tolerance+" solution:"+solution);
      return false;
    }

    return true;
  }


  /** todo: get correct data for this test */
  public void xtestGreatArc () {
    //                          latitude                      longitude
    double[][] values = {
                          { Convert.dms2deg("00'00'00"), Convert.dms2deg("00'00'00"),
                            Convert.dms2deg("90'00'00"), Convert.dms2deg("00'00'000"),
                            10018754.1714 },
                          { Convert.dms2deg("00'00'00"), Convert.dms2deg("00'00'00"),
                            Convert.dms2deg("00'00'00"), Convert.dms2deg("90'00'000"),
                            10018754.1714 },
                          { Convert.dms2deg("00'00'00"), Convert.dms2deg("00'00'00"),
                            Convert.dms2deg("00'00'00"), Convert.dms2deg("180'00'000"),
                            20037508.3428 },
                          { Convert.dms2deg("36'07'02"), Convert.dms2deg("-86'40'02"), // BNA
                            Convert.dms2deg("33'56'04"), Convert.dms2deg("-118'24'0"), // LAX
                            2890100.6800 }, // 2887259.0 }, // 2887 km
                          { Convert.dms2deg("40'38'00"), Convert.dms2deg("-73'47'00"), // JFX
                            Convert.dms2deg("33'56'04"), Convert.dms2deg("-118'24'0"), // LAX
                            2144000.0 }, // 2144 km
                        };

    // final double TOLERANCE_DELTA = 0.0001; // Tolerance for rounding errors (~10 m)
    final double TOLERANCE_DELTA = 10; // Tolerance for rounding errors (~10 m)

    for (int ii = 0; ii < values.length; ii++) {
      double[] testcase = values[ii];
      double refAnswer  = testcase[4];
      Position p = new Position();
      Position q = new Position();
      p.setGeo(0, testcase[0], testcase[1]);
      q.setGeo(0, testcase[2], testcase[3]);
      double result = Transform.greatArc(p, q);
      if (debug) System.out.println("p=" + p + "  q=" + q+ " great arc dist(m)="+ result);
      assertEquals("greatArc - latitude " +ii,  refAnswer, result, TOLERANCE_DELTA);
    }
  }

  /** @since NeXtMidas 2.7.0 */
  public void testEci2ecef () { } // tested in testEci2ecr()
  public void testEcef2eci () { } // tested in testEci2ecr()
  public void testEcr2eci ()  { } // tested in testEci2ecr()
  public void testEci2ecr () {
    final double TOLERANCE_DELTA = 1.0E-6; // Tolerance for rounding errors
    final int   V_MODE2_LEN = 6;
    final double TIME_DELTA = 300; // in seconds from nxm.sys.dat.sv1.prm
    final double[] TESTDATA = {    // data from nxm.sys.dat.sv1.prm (ECR) in meters and meters/sec
      // POSITION(        x,                     y,                z),  VELOCITY(      vx,                vy,                vz)
      -3.477915901410128E7 , -2.387274376300576E7 , 10663.37983864173 , -6.909001163239793 , 12.93011670211473 , 267.8537283271007 ,
      -3.4781157688145764E7, -2.3868828302048326E7, 91010.5545165628  , -6.413511289591497 , 13.16975542507447 , 267.772731506623  ,
      -3.478300590829652E7 , -2.3864844033730097E7, 171314.2305845236 , -5.90611570161073  , 13.38874668497499 , 267.56374846385694,
      -3.478470023366146E7 , -2.3860797224657644E7, 251536.02429898112, -5.3876901350148785, 13.586614212979043, 267.2268701978891 ,
    };
    int nrec = TESTDATA.length/V_MODE2_LEN;
    double[] gha = new double[nrec]; // Greenwich hour angle (GHA) - in radians
    double[] v1  = new double[TESTDATA.length]; // for 1st ecr2eci conversion (ECI)
    double[] v2  = new double[TESTDATA.length]; // for     eci2ecr conversion (ECR)
    double[] v3  = new double[TESTDATA.length]; // for 2nd ecr2eci conversion (ECI)
    int mode;
    Time t = new Time("2008:09:26::00:00:00"); // Time instance to calculate GHA values using Sep 26, 2008 12:00AM

    if (debug) System.out.println("Date/Time of input data = ");
    // populate gha[] with time t + TIME_DELTA for number of test records that we have
    for (int ii=0; ii<nrec; ii++, t.addSec(TIME_DELTA)) {
      gha[ii] = t.getGHA();
      if (debug) System.out.println(" t"+ii+"="+t+"  => gha["+ii+"]="+gha[ii]);
    }
    if (debug) printArray("INPUT data in ECR (meters & meters/sec) = ", TESTDATA, V_MODE2_LEN);

    // test mode 2: Position + Velocity
    mode = 2;
    System.arraycopy(TESTDATA, 0, v1, 0, v1.length);
    Transform.ecr2eci(v1, mode, nrec, gha); // ECR to ECI => v1 is in ECI
    if (debug) printArray("ECR => ECI (v1) = ", v1, V_MODE2_LEN);

    System.arraycopy(v1, 0, v2, 0, v2.length);
    Transform.eci2ecr(v2, mode, nrec, gha); // ECI to ECR => v2 is in ECR
    if (debug) printArray("ECI => ECR (v2) = ", v2, V_MODE2_LEN);
    assertArrayEquals("ECI TO ECR conversion", TESTDATA, v2, TOLERANCE_DELTA);

    System.arraycopy(v2, 0, v3, 0, v3.length);
    Transform.ecr2eci(v3, mode, nrec, gha); // ECR to ECI => v3 is in ECI
    if (debug) printArray("ECR => ECI (v3) = ", v3, V_MODE2_LEN);
    assertArrayEquals("ECR TO ECI conversion", v1, v3, TOLERANCE_DELTA);
  }

  /** GEODETIC-to/from-CARTESIAN test data. Test data from:
      <pre>
        "Ephemeris/Station/Antenna Offset Documentation." NGA.
        Effective Date 04 Oct 2012.
        http://earth-info.nga.mil/GandG/sathtml/gpsdoc2012_10a.html
      </pre>
      Note that the GEODETIC data below is LAT/LON/ALT (not LON/LAT/ALT) and
      has LON on [0,360] not [-180,+180] -- which matches the reference
      document. This is immediately converted to the normal LON/LAT/ALT in
      the static block below.
   */
  private static final Object[] GPS_MONITOR_STATIONS = {
    // ID  NAME                      X (m)          Y (m)         Z (m)
    //                             VX (m/s)       VY (m/s)      VZ (m/s)
    //                               LAT           LON           ALT (m)
    85128, "AMC1", new double[] {-1248599.684, -4819441.064,  3976490.163,
                                       -.0146,        .0009,       -.0049},
                   new double[] { 38.80293817, 255.47540441,     1911.851},
    85129, "ASCN", new double[] { 6118523.949, -1572350.766,  -876463.917,
                                       -.0002,       -.0057,        .0110},
                   new double[] { -7.95132928, 345.58786988,      106.360},
    85130, "DGAR", new double[] { 1916196.835,  6029998.860,  -801737.158,
                                       -.0448,        .0176,        .0331},
                   new double[] { -7.26984187,  72.37092401,      -64.320},
    85131, "KWGL", new double[] {-6160884.079,  1339852.174,   960843.156,
                                        .0201,        .0663,        .0295},
                   new double[] {  8.72250183, 167.73052383,       39.703},
    85132, "HAWI", new double[] {-5511980.303, -2200246.813,  2329481.010,
                                       -.0098,        .0628,        .0320},
                   new double[] { 21.56149225, 201.76066735,      425.845},
    85143, "CAPE", new double[] {  918988.080, -5534552.969,  3023721.401,
                                       -.0126,        .0016,        .0011},
                   new double[] { 28.48373821, 279.42769508,      -23.996},
    85402, "AUSA", new double[] {-3939182.309,  3467075.393, -3613220.631,
                                       -.0409,        .0030,        .0485},
                   new double[] {-34.72899824, 138.64734614,       38.164},
    85403, "ARGA", new double[] { 2745499.115, -4483636.626, -3599054.557,
                                        .0045,       -.0079,        .0085},
                   new double[] {-34.57370121, 301.48070086,       48.778},
    85404, "ENGD", new double[] { 3981776.606,   -89239.026,  4965284.741,
                                       -.0127,        .0168,        .0101},
                   new double[] { 51.45374362, 358.71610986,      163.145},
    85405, "BAH1", new double[] { 3633910.113,  4425277.188,  2799862.558,
                                       -.0324,        .0096,        .0270},
                   new double[] { 26.20914157,  50.60814606,      -14.719},
    85406, "ECUA", new double[] { 1272867.335, -6252772.177,   -23801.764,
                                        .0067,        .0013,        .0108},
                   new double[] {  -.21515713, 281.50639199,     2922.590},
    85407, "USNO", new double[] { 1112158.872, -4842855.638,  3985497.095,
                                       -.0150,       -.0001,        .0024},
                   new double[] { 38.92056510, 282.93368419,       59.109},
    85410, "ALAS", new double[] {-2296298.554, -1484805.093,  5743080.084,
                                       -.0222,       -.0068,       -.0086},
                   new double[] { 64.68793940, 212.88703334,      176.608},
    85411, "NZLD", new double[] {-4780787.888,   436877.312, -4185259.659,
                                       -.0219,        .0127,        .0205},
                   new double[] {-41.27264845, 174.77870633,       47.996},
    85412, "SAFR", new double[] { 5066232.157,  2719227.066, -2754392.675,
                                       -.0012,        .0197,        .0168},
                   new double[] {-25.74634534,  28.22403810,     1416.439},
    85413, "SKOR", new double[] {-3067863.296,  4067641.022,  3824295.844,
                                       -.0263,       -.0091,       -.0094},
                   new double[] { 37.07756763, 127.02403337,       51.875},
    85414, "TAHI", new double[] {-5246404.037, -3077285.367, -1913839.323,
                                       -.0422,        .0515,        .0327},
                   new double[] {-17.57702921, 210.39381205,       99.936},
  };
  static {
    for (int i = 0; i < GPS_MONITOR_STATIONS.length; i+=4) {
      double[] latLonAlt = (double[])GPS_MONITOR_STATIONS[i+3];
      double[] lonLatAlt = { latLonAlt[1], latLonAlt[0], latLonAlt[2] };
      if (lonLatAlt[0] > 180) lonLatAlt[0] = lonLatAlt[0] - 360;
      GPS_MONITOR_STATIONS[i+3] = lonLatAlt;
    }
  }

  public void testCar2geo () {
    // Note that this only tests 3-value mode
    for (int i = 0; i < GPS_MONITOR_STATIONS.length; i+=4) {
      String   name     = ( String )GPS_MONITOR_STATIONS[i+1];
      double[] givenCAR = (double[])GPS_MONITOR_STATIONS[i+2];
      double[] givenGEO = (double[])GPS_MONITOR_STATIONS[i+3];
      double[] data     = Arrays.copyOf(givenCAR, 3);

      Transform.car2geo(data, Transform.POS, 1);
      Transform.all2lla(data, 1);

      assertArrayEquals("car2geo for "+name, givenGEO, data, 0.001);
    }
  }

  public void testGeo2car () {
    // Note that this only tests 3-value mode
    for (int i = 0; i < GPS_MONITOR_STATIONS.length; i+=4) {
      String   name     = ( String )GPS_MONITOR_STATIONS[i+1];
      double[] givenCAR = (double[])GPS_MONITOR_STATIONS[i+2];
      double[] givenGEO = (double[])GPS_MONITOR_STATIONS[i+3];
      double[] expected = Arrays.copyOf(givenCAR, 3);
      double[] data     = Arrays.copyOf(givenGEO, 3);

      Transform.all2lla(data, 1);
      Transform.geo2car(data, Transform.POS, 1);

      assertArrayEquals("geo2car for "+name, expected, data, 0.001);
    }
  }

  public void testAll2lla () {
    double[] data3 = { 0,1,2, 3,4,5, 6,7,8 };
    double[] exp3  = { 2,1,0, 5,4,3, 8,7,6 };
    double[] data2 = Arrays.copyOf(data3, 6);
    double[] exp2  = Arrays.copyOf(exp3,  6);
    double[] data1 = Arrays.copyOf(data3, 3);
    double[] exp1  = Arrays.copyOf(exp3,  3);

    Transform.all2lla(data1, 1);
    Transform.all2lla(data2, 2);
    Transform.all2lla(data3, 3);

    assertArrayEquals("all2lla for 1 record", exp1, data1);
    assertArrayEquals("all2lla for 2 record", exp2, data2);
    assertArrayEquals("all2lla for 3 record", exp3, data3);
  }

  /** used for debugging contents of arrays */
  static void printArray (String prepend, double[] arr, int recLen) {
    StringBuilder sb = new StringBuilder(arr.length*recLen);
    sb.append(prepend).append("\n");
    for (int ii=0; ii<arr.length; ii+=recLen) {
      for (int jj=0; jj<recLen; jj++) { sb.append(arr[ii+jj]).append(" "); }
      sb.append("\n");
    }
    System.out.println(sb.toString());
  }

  public void testTransformIntInt() {
    subtest_Transform_int_int(Transform.ECR, Units.GEODETIC);
    subtest_Transform_int_int(Transform.ECR, Units.CARTESIAN);
  }
  private void subtest_Transform_int_int(final int r, final int c) {
    final Transform t = new Transform(r, c);
    assertEquals("transform.getReferenceFrame()", Parser.get(Transform.referenceFrameList, r), t.getReferenceFrame());
    assertEquals("transform.getCoordinateSystem()", Parser.get(Units.compTypeList, c), t.getCoordinateSystem());
  }

  public void testEcrGeo2ecrCarAzEq() {
    final double EPSILON = 5.0e-9;
    final double PRIM    = Transform.POLAR_RADIUS_IN_METERS;
    final double N45     = PRIM * Math.toRadians( 45); // spherical distance from the North Pole to 45deg North latitude
    final double EQU     = PRIM * Math.toRadians( 90); // spherical distance from the North Pole to the equator
    final double S85     = PRIM * Math.toRadians(175); // spherical distance from the North Pole to 85deg South latitude
    final double S85_C30 = S85 * Math.cos(Math.toRadians(30)); // spherical distance from the North Pole to 85deg South latitude, times cos(30)
    final double S85_C45 = S85 * Math.cos(Math.toRadians(45)); // spherical distance from the North Pole to 85deg South latitude, times cos(45)
    final double S85_C60 = S85 / 2.0; // spherical distance from the North Pole to 85deg South latitude, times cos(60)
    final double[] givenEG = {
      0,  90,    0, // North Pole
      0,  90, 12.3, // North Pole with (an ignored) longitude
      0,  45, -180, // 45deg North at 180deg West
      0,  45,  -90, // 45deg North at 90deg West
      0,  45,    0, // 45deg North at Prime Meridian
      0,   0, -180, // Equator at 180deg West
      0,   0,  -90, // Equator at 90deg West
      0,   0,    0, // Equator at Prime Meridian
      0,   0,   90, // Equator at 90deg East
      0,   0,  180, // Equator at 180deg East
      0, -85,   45, // 85deg South at 45deg East
      0, -85,   60, // 85deg South at 60deg East
      0, -85,  150, // 85deg South at 150deg East
      PRIM, -85, 150, // a location with (an ignored) altitude
    };
    final double[] actualECAZ = Transform.ecrGeo2ecrCarAzEq(givenEG);
    final double[] expectedECAZ = {
         0,    0, PRIM,
         0,    0, PRIM,
      -N45,    0, PRIM,
         0, -N45, PRIM,
       N45,    0, PRIM,
      -EQU,    0, PRIM,
         0, -EQU, PRIM,
       EQU,    0, PRIM,
         0,  EQU, PRIM,
      -EQU,    0, PRIM,
       S85_C45, S85_C45, PRIM,
       S85_C60, S85_C30, PRIM,
      -S85_C30, S85_C60, PRIM,
      -S85_C30, S85_C60, PRIM,
    };
    assertArrayEquals("actualECAZ[]", expectedECAZ, actualECAZ, EPSILON);
  }

  public void testEcrCarAlt2ecrCarTMatrix() {
    subtest_eca2ecTMatrix( 90,        0);
    subtest_eca2ecTMatrix( 34.567, -156.789);
    subtest_eca2ecTMatrix( 34.567,  -56.789);
    subtest_eca2ecTMatrix( 34.567,   56.789);
    subtest_eca2ecTMatrix( 34.567,  156.789);
    subtest_eca2ecTMatrix(  0,     -180);
    subtest_eca2ecTMatrix(  0,      -90);
    subtest_eca2ecTMatrix(  0,        0);
    subtest_eca2ecTMatrix(  0,       90);
    subtest_eca2ecTMatrix(  0,      180);
    subtest_eca2ecTMatrix(-56.789, -134.567);
    subtest_eca2ecTMatrix(-56.789,  -34.567);
    subtest_eca2ecTMatrix(-56.789,   34.567);
    subtest_eca2ecTMatrix(-56.789,  134.567);
    subtest_eca2ecTMatrix(-90,        0);
  }
  private void subtest_eca2ecTMatrix(final double lat, final double lon) {
    final double EPSILON = 5.0e-16;
    final TMatrix actual = Transform.ecrCarAlt2ecrCarTMatrix(lat, lon);

    final TMatrix expected = new TMatrix();
    expected.rotate(90.0-lat,TMatrix.ROLL);
    expected.rotate(90.0+lon,TMatrix.AZIM);

    assertArrayEquals("actual[]", toArray(expected), toArray(actual), EPSILON);
  }

  public void testEcrCar2ecrCarAltTMatrix() {
    subtest_ec2ecaTMatrix( 90,        0);
    subtest_ec2ecaTMatrix( 34.567, -156.789);
    subtest_ec2ecaTMatrix( 34.567,  -56.789);
    subtest_ec2ecaTMatrix( 34.567,   56.789);
    subtest_ec2ecaTMatrix( 34.567,  156.789);
    subtest_ec2ecaTMatrix(  0,     -180);
    subtest_ec2ecaTMatrix(  0,      -90);
    subtest_ec2ecaTMatrix(  0,        0);
    subtest_ec2ecaTMatrix(  0,       90);
    subtest_ec2ecaTMatrix(  0,      180);
    subtest_ec2ecaTMatrix(-56.789, -134.567);
    subtest_ec2ecaTMatrix(-56.789,  -34.567);
    subtest_ec2ecaTMatrix(-56.789,   34.567);
    subtest_ec2ecaTMatrix(-56.789,  134.567);
    subtest_ec2ecaTMatrix(-90,        0);
  }
  private void subtest_ec2ecaTMatrix(final double lat, final double lon) {
    final double EPSILON = 5.0e-16;
    final TMatrix actual = Transform.ecrCar2ecrCarAltTMatrix(lat, lon);

    final TMatrix expected = new TMatrix();
    expected.rotate(90.0-lat,TMatrix.ROLL);
    expected.rotate(90.0+lon,TMatrix.AZIM);
    expected.transpose();

    assertArrayEquals("actual[]", toArray(expected), toArray(actual), EPSILON);
  }

  public void testEcrGeo2ecrGeo() {
    final int DIV = 8 * 7;
    subtest_eg2eg(new double[]{0.0,  90.0,      0.0  },  8.3, DIV);
    subtest_eg2eg(new double[]{0.0,  89.999,    0.0  },  8.3, DIV);
    subtest_eg2eg(new double[]{0.0,  34.567, -156.789}, 18.3, DIV);
    subtest_eg2eg(new double[]{0.0,  34.567,  -56.789}, 18.3, DIV);
    subtest_eg2eg(new double[]{0.0,  34.567,   56.789}, 18.3, DIV);
    subtest_eg2eg(new double[]{0.0,  34.567,  156.789}, 18.3, DIV);
    subtest_eg2eg(new double[]{0.0,   0.0,   -180.0  },  8.3, DIV);
    subtest_eg2eg(new double[]{0.0,   0.0,    -90.0  }, 18.3, DIV);
    subtest_eg2eg(new double[]{0.0,   0.0,      0.0  }, 18.3, DIV);
    subtest_eg2eg(new double[]{0.0,   0.0,     90.0  }, 18.3, DIV);
    subtest_eg2eg(new double[]{0.0,   0.0,    180.0  },  8.3, DIV);
    subtest_eg2eg(new double[]{0.0, -56.789, -134.567}, 18.3, DIV);
    subtest_eg2eg(new double[]{0.0, -56.789,  -34.567}, 18.3, DIV);
    subtest_eg2eg(new double[]{0.0, -56.789,   34.567}, 18.3, DIV);
    subtest_eg2eg(new double[]{0.0, -56.789,  134.567}, 18.3, DIV);
    subtest_eg2eg(new double[]{0.0, -89.999,    0.0  },  8.3, DIV);
    subtest_eg2eg(new double[]{0.0, -90.0,      0.0  },  8.3, DIV);
  }
  private void subtest_eg2eg(final double[] centerEG, final double perimRadius, final int perimDiv) {
    final double EPSILON_EGA2EG     = 1.0e-8;
    final double EPSILON_EGA2EG2EGA = 3.0e-13;
    final TMatrix eca2ec = Transform.ecrCarAlt2ecrCarTMatrix(centerEG[1], centerEG[2]);
    final TMatrix ec2eca = Transform.ecrCar2ecrCarAltTMatrix(centerEG[1], centerEG[2]);

    final double[] perimEGA = new double[3*(perimDiv+1)]; {
      final double perimLatEGA = 90.0 - perimRadius;
      int ii=0;
      for (int jj=0; jj<perimDiv; ++jj) {
        double perimLonEGA = 90.0 - jj*360.0/perimDiv;
        if (perimLonEGA<-180.0) perimLonEGA+=360.0;
        if (perimLonEGA> 180.0) perimLonEGA-=360.0;
        perimEGA[ii++] = 0.0; // altitude
        perimEGA[ii++] = perimLatEGA; // latitude
        perimEGA[ii++] = perimLonEGA; // longitude
      }
      perimEGA[ii++] = 0.0; // altitude
      perimEGA[ii++] = perimLatEGA; // latitude
      perimEGA[ii]   = perimEGA[2]; // longitude
    }
    final double[] perimEGA_asEG       = Transform.ecrGeo2ecrGeo(eca2ec, perimEGA);
    final double[] perimEGA_asEG_asEGA = Transform.ecrGeo2ecrGeo(ec2eca, perimEGA_asEG);
    final double[] perimEG             = GeodeticUtil.fromLLToALL(GeodeticUtil.computeCircleLL(centerEG[2], centerEG[1], perimRadius, perimDiv));
/*
    System.out.println(centerEG[1]+":");
    System.out.println("           perimEGA[]="+toLonString(perimEGA));
    System.out.println("      perimEGA_asEG[]="+toLonString(perimEGA_asEG));
    System.out.println("perimEGA_asEG_asEGA[]="+toLonString(perimEGA_asEG_asEGA));
    System.out.println("            perimEG[]="+toLonString(perimEG));
*/
    assertArrayEqualsALL("perimEGA[] -> perimEGA_asEG[]", perimEG, perimEGA_asEG, EPSILON_EGA2EG);
    assertArrayEqualsALL("perimEGA[] -> perimEGA_asEG_asEGA[]", perimEGA, perimEGA_asEG_asEGA, EPSILON_EGA2EG2EGA);
  }
  private String toLonString(final double[] in) {
    final int numPoints =  in.length/3;
    final long[] out = new long[numPoints];
    for (int ii=0; ii<numPoints; ++ii)
      out[ii] = Math.round(in[3*ii+2]);
    return Arrays.toString(out);
  }

  private double[] toArray(final TMatrix tm) {
    final double[] array = {
      tm.a11, tm.a21, tm.a31,
      tm.a12, tm.a22, tm.a32,
      tm.a13, tm.a23, tm.a33,
      tm.b1,  tm.b2,  tm.b3
    };
    return array;
  }

  /**
   * Asserts that two double arrays, representing geodetic points in ALL
   * (altitude, latitude, longitude) order, are equal. If they are not, an
   * {@link AssertionError} is thrown with the given message. Special handling
   * is provided for longitudes which straddle the International Date Line.
   *
   * @param message the identifying message for the {@link AssertionError} (<code>null</code>
   * okay)
   * @param expecteds double array with expected values.
   * @param actuals double array with actual values
   * @param delta the maximum delta between <code>expecteds[i]</code> and
   * <code>actuals[i]</code> for which both numbers are still
   * considered equal.
   */
  static public void assertArrayEqualsALL(String message, double[] expecteds, double[] actuals, double delta) {
    final int length = actuals.length;
    assertEquals(message, expecteds.length, length); // the two arrays must be the same length
    assertEquals(message, 0, length%3); // the lengths must be a multiple of 3
    for (int ii=0; ii<length;) {
      // compare altitudes
      assertEquals(message+": value at index "+ii, expecteds[ii], actuals[ii], delta);
      ++ii;

      // compare latitudes
      assertEquals(message+": value at index "+ii, expecteds[ii], actuals[ii], delta);
      ++ii;

      // compare longitudes
      double ee = expecteds[ii];
      double aa = actuals[ii];
      if (
        doubleIsDifferent(ee, aa, delta)
        &&
        (
          (ee<aa && doubleIsDifferent(ee+360.0, aa, delta))
          ||
          (aa<ee && doubleIsDifferent(ee, aa+360.0, delta))
        )
      ) {
        assertEquals(message+": value at index "+ii, expecteds[ii], actuals[ii], delta);
      }
      ++ii;
    }
  }
  static private boolean doubleIsDifferent(double d1, double d2, double delta) {
    if (Double.compare(d1, d2) == 0) {
        return false;
    }
    if ((Math.abs(d1 - d2) <= delta)) {
        return false;
    }

    return true;
  }

}