package nxm.sys.test.junit.lib;

import nxm.sys.lib.Convert;
import nxm.sys.lib.DataFile;
import nxm.sys.lib.Keywords;
import nxm.sys.lib.Midas;
import nxm.sys.lib.Position;
import nxm.sys.lib.Shell;
import nxm.sys.lib.StateVector;
import nxm.sys.lib.Table;
import nxm.sys.lib.Transform;
import nxm.sys.test.junit.NMTestCase;

/** This will test the Position class.
    @author  J. Czechowski IV
 */
public final class PositionTest extends NMTestCase {
  // CARTESIAN values computed using X-Midas primitive:
  //        program TRANSFORM
  //          implicit none
  //          include 'primitive.inc'
  //          real*8 pos(3)
  //
  //          call M$CHECKIN
  //          type*,'M$GEO_TO_CAR'
  //          call GEO_TO_CAR(0d0,  0d0,  0d0)
  //          call GEO_TO_CAR(0d0,  5d0,  0d0)
  //          call GEO_TO_CAR(0d0,  0d0,  5d0)
  //          call GEO_TO_CAR(0d0,  5d0,  5d0)
  //          call GEO_TO_CAR(5d0,  0d0,  0d0)
  //          call GEO_TO_CAR(0d0, 30d0, 30d0)
  //          call GEO_TO_CAR(0d0, 30d0,-30d0)
  //          call GEO_TO_CAR(0d0,-30d0, 30d0)
  //          call GEO_TO_CAR(0d0,-30d0,-30d0)
  //
  //          call M$RETURN
  //        end
  //
  //        subroutine GEO_TO_CAR (alt, lat, lon)
  //          implicit none
  //          include 'primitive.inc'
  //          real*8 alt,lat,lon
  //          real*8 pos(3)
  //
  //          pos(1) = alt
  //          pos(2) = lat
  //          pos(3) = lon
  //
  //          call M$GEO_TO_CAR(pos,1,1)
  //
  //          type*,'all=',alt,lat,lon
  //          type*,'pos=',pos
  //        return
  //        end
  //                                         |---- GEODETIC ----|------------------------ CARTESIAN ------------------------|--------------- ALWAYS 0 ---------------|
  //                                           alt,  lat,  lon,                  x,                  y,                  z,   vx,   vy,   vz,   ax,   ay,   az,  hdg
  private static final double[][] TESTS = { {  0.0,  0.0,  0.0,  6378137.000000001, 0.0000000000000000, 0.0000000000000000,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0 },
                                            {  0.0,  5.0,  0.0,  6354027.820562059, 0.0000000000000000,  552183.9600277715,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0 },
                                            {  0.0,  0.0,  5.0,  6353866.263102791,  555891.2675813202, 0.0000000000000000,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0 },
                                            {  0.0,  5.0,  5.0,  6329848.826371371,  553790.0141403698,  552183.9600277715,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0 },
                                            {  5.0,  0.0,  0.0,  6378142.000000000, 0.0000000000000000, 0.0000000000000000,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0 },
                                            {  0.0, 30.0, 30.0,  4787610.688267581,  2764128.319646417,  3170373.735383647,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0 },
                                            {  0.0, 30.0,-30.0,  4787610.688267581, -2764128.319646417,  3170373.735383647,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0 },
                                            {  0.0,-30.0, 30.0,  4787610.688267581,  2764128.319646417, -3170373.735383647,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0 },
                                            {  0.0,-30.0,-30.0,  4787610.688267581, -2764128.319646417, -3170373.735383647,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0 },
                                          };

  //                                              lon1  lat1  lon2  lat2  def    hdg
  private static final double[][] HDG_TESTS = { { 0.0,  0.0,  0.0,  5.0, -1.0,   0.0 }, // north
                                                { 0.0,  0.0,  5.0,  5.0, -1.0,  45.0 }, // north-east
                                                { 0.0,  0.0,  5.0,  0.0, -1.0,  90.0 }, // east
                                                { 0.0,  0.0,  5.0, -5.0, -1.0, 135.0 }, // south-east
                                                { 0.0,  0.0,  0.0, -5.0, -1.0, 180.0 }, // south
                                                { 0.0,  0.0, -5.0, -5.0, -1.0, 225.0 }, // south-west
                                                { 0.0,  0.0, -5.0,  0.0, -1.0, 270.0 }, // west
                                                { 0.0,  0.0, -5.0,  5.0, -1.0, 315.0 }, // north-west
                                                { 0.0,  0.0,  0.0,  0.0, -1.0,  -1.0 }, // stationary
                                              };

  //                                              lon1  lat1  lon2  lat2  def    hdg
  private static final double[][] HDG_PRECISE_TESTS =
                                                { {45.0, 45.0, 45.0, 50.0, -1.0, 0.0},
                                                {-155.0, 0.0,-165.0,10.0,-1.0, 315.438 },
                                                {-175.0, 0.0, 175.0,10.0,-1.0, 315.438 },
                                                { 0.0,  0.0,  0.0,  5.0, -1.0,   0.0 }, // north
                                                { 0.0,  0.0,  5.0,  5.0, -1.0,  44.890 }, // north-east
                                                { 0.0,  0.0,  5.0,  0.0, -1.0,  90.0 }, // east
                                                { 0.0,  0.0,  5.0, -5.0, -1.0, 135.109 }, // south-east
                                                { 0.0,  0.0,  0.0, -5.0, -1.0, 180.0 }, // south
                                                { 0.0,  0.0, -5.0, -5.0, -1.0, 224.890 }, // south-west
                                                { 0.0,  0.0, -5.0,  0.0, -1.0, 270.0 }, // west
                                                { 0.0,  0.0, -5.0,  5.0, -1.0, 315.109 }, // north-west
                                                { 0.0,  0.0,  0.0,  0.0, -1.0,  -1.0 }, // stationary
                                                { 118.250,  34.050,  139.750,  35.667, -1.0,  78.63 }, // example: LA (34.0500, 118.2500) to Tokyo (35.667, 139.750)
                                                { 77.033,  38.883,  0.083,  51.533, -1.0,  310.703 } // example: Wash D.C. (38.8833, 77.0333) to London, UK (51.533, 0.0833)
                                                };
  //                                              lon1  lat1  lon2  lat2  def    hdg  ve  vn  vu
  private static final double[][] HDG_GEODETIC_TESTS =
                                                { {45.0, 45.0, 45.0, 50.0, -1.0, 32.222, 5.0, 5.0, 2.0 },
                                                {-155.0, 0.0,-165.0,10.0,-1.0, 63.259, 10.0, 5.0, 2.0  },
                                                {-175.0, 0.0, 175.0,10.0,-1.0, 71.323, 15.0, 5.0, 2.0  },
                                                { 0.0,  0.0,  0.0,  5.0, -1.0,  75.651, 20.0, 5.0, 2.0  },
                                                { 0.0,  0.0,  5.0,  5.0, -1.0,  78.304, 25.0, 5.0, 2.0  },
                                                { 0.0,  0.0,  5.0,  0.0, -1.0,  26.302, 5.0, 10.0, 2.0  },
                                                { 0.0,  0.0,  5.0, -5.0, -1.0, 18.0181, 5.0, 15.0, 2.0  },
                                                { 0.0,  0.0,  0.0, -5.0, -1.0, 13.466, 5.0, 20.0, 2.0  },
                                                { 0.0,  0.0, -5.0, -5.0, -1.0, 10.586, 5.0, 25.0, 2.0  },
                                                { 0.0,  0.0, -5.0,  0.0, -1.0, 44.561, 10.0, 10.0, 2.0  },
                                                { 0.0,  0.0, -5.0,  5.0, -1.0, 224.007, -15.0, -15.0, -2.0  },
                                                { 0.0,  0.0,  0.0,  0.0, -1.0,  223.219, -20.0, -20.0, -2.0  },
                                                { 118.250,  34.050,  139.750,  35.667, -1.0, 25.800, 25.0, 25.0, 2.0  },
                                                { 77.033,  38.883,  0.083,  51.533, -1.0,  22.528, 25.0, 25.0, 2.0  }
                                                };

    //                                              lon1  lat1  lon2  lat2  def    hdg
  private static final double[][] HDG_GREAT_ARC_TESTS =
                                                { {45.0, 45.0, 45.0, 50.0, -1.0, 0.0},
                                                {-155.0, 0.0,-165.0,10.0,-1.0, 315.438 },
                                                {-175.0, 0.0, 175.0,10.0,-1.0, 315.438 },
                                                { 0.0,  0.0,  0.0,  5.0, -1.0, 0.0 }, // north
                                                { 0.0,  0.0,  5.0,  5.0, -1.0, 44.89 }, // north-east
                                                { 0.0,  0.0,  5.0,  0.0, -1.0, 90.0 }, // east
                                                { 0.0,  0.0,  5.0, -5.0, -1.0, 135.109 }, // south-east
                                                { 0.0,  0.0,  0.0, -5.0, -1.0, 180.0 }, // south
                                                { 0.0,  0.0, -5.0, -5.0, -1.0, 224.890 }, // south-west
                                                { 0.0,  0.0, -5.0,  0.0, -1.0, 270.0 }, // west
                                                { 0.0,  0.0, -5.0,  5.0, -1.0, 315.109 }, // north-west
                                                { 0.0,  0.0,  0.0,  0.0, -1.0,  0.0 }, // stationary
                                                { 118.250,  34.050,  139.750,  35.667, -1.0,  78.63 }, // example: LA (34.0500, 118.2500) to Tokyo (35.667, 139.750)
                                                { 77.033,  38.883,  0.083,  51.533, -1.0,  310.703 } // example: Wash D.C. (38.8833, 77.0333) to London, UK (51.533, 0.0833)
                                                };

  private static final double TOL = 0.001; // tolerance

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

  /** Creates a new instance of the class.
      @param methodName The name of the method being called.
      @param midas      The midas context testTo use when running the tests.
   */
  public PositionTest (String methodName, Midas midas) {
    super(methodName, midas);
  }

  private void check (String str, int i, Position pos) {
    checkMethods(str, i, pos);
    checkGetKey(str, pos);

    Position copy = pos.copy();
    checkMethods(str+" (copy)", i, copy);
    checkGetKey(str+" (copy)", copy);
  }

  private void checkMethods (String str, int i, Position pos) {
    assertEquals(str+" -> getAlt() for "+pos,     TESTS[i][ 0], pos.getAlt(), TOL);
    assertEquals(str+" -> getLat() for "+pos,     TESTS[i][ 1], pos.getLat(), TOL);
    assertEquals(str+" -> getLon() for "+pos,     TESTS[i][ 2], pos.getLon(), TOL);
    assertEquals(str+" -> getX() for "+pos,       TESTS[i][ 3], pos.getX(),   TOL);
    assertEquals(str+" -> getY() for "+pos,       TESTS[i][ 4], pos.getY(),   TOL);
    assertEquals(str+" -> getZ() for "+pos,       TESTS[i][ 5], pos.getZ(),   TOL);
    assertEquals(str+" -> getVX() for "+pos,      TESTS[i][ 6], pos.getVX());      // exactly 0.0 for now
    assertEquals(str+" -> getVY() for "+pos,      TESTS[i][ 7], pos.getVY());      // exactly 0.0 for now
    assertEquals(str+" -> getVZ() for "+pos,      TESTS[i][ 8], pos.getVZ());      // exactly 0.0 for now
    assertEquals(str+" -> getAX() for "+pos,      TESTS[i][ 9], pos.getAX());      // exactly 0.0 for now
    assertEquals(str+" -> getAY() for "+pos,      TESTS[i][10], pos.getAY());      // exactly 0.0 for now
    assertEquals(str+" -> getAZ() for "+pos,      TESTS[i][11], pos.getAZ());      // exactly 0.0 for now
    assertEquals(str+" -> getHeading() for "+pos, TESTS[i][12], pos.getHeading()); // exactly 0.0 for now
    assertEquals(str+" -> getHeadingGeodetic() for "+pos, TESTS[i][12], pos.getHeadingGeodetic()); // exactly 0.0 for now

    checkGetKey(str, pos);
  }

  private void checkGetKey (String str, Position pos) {
    assertEquals(str+" -> getKey(ALT) for "+pos,   pos.getAlt(),  Convert.o2d(pos.getKey("ALT"  )) );
    assertEquals(str+" -> getKey(LAT) for "+pos,   pos.getLat(),  Convert.o2d(pos.getKey("LAT"  )) );
    assertEquals(str+" -> getKey(LON) for "+pos,   pos.getLon(),  Convert.o2d(pos.getKey("LON"  )) );
    assertEquals(str+" -> getKey(X) for "+pos,     pos.getX(),    Convert.o2d(pos.getKey("X"    )) );
    assertEquals(str+" -> getKey(Y) for "+pos,     pos.getY(),    Convert.o2d(pos.getKey("Y"    )) );
    assertEquals(str+" -> getKey(X) for "+pos,     pos.getZ(),    Convert.o2d(pos.getKey("Z"    )) );
    assertEquals(str+" -> getKey(VX) for "+pos,    pos.getVX(),   Convert.o2d(pos.getKey("VX"   )) );
    assertEquals(str+" -> getKey(VY) for "+pos,    pos.getVY(),   Convert.o2d(pos.getKey("VY"   )) );
    assertEquals(str+" -> getKey(VZ) for "+pos,    pos.getVZ(),   Convert.o2d(pos.getKey("VZ"   )) );
    assertEquals(str+" -> getKey(VE) for "+pos,    pos.getVE(),   Convert.o2d(pos.getKey("VE"   )) );
    assertEquals(str+" -> getKey(VN) for "+pos,    pos.getVN(),   Convert.o2d(pos.getKey("VN"   )) );
    assertEquals(str+" -> getKey(VU) for "+pos,    pos.getVU(),   Convert.o2d(pos.getKey("VU"   )) );
    assertEquals(str+" -> getKey(AX) for "+pos,    pos.getAX(),   Convert.o2d(pos.getKey("AX"   )) );
    assertEquals(str+" -> getKey(AY) for "+pos,    pos.getAY(),   Convert.o2d(pos.getKey("AY"   )) );
    assertEquals(str+" -> getKey(AZ) for "+pos,    pos.getAZ(),   Convert.o2d(pos.getKey("AZ"   )) );
    assertEquals(str+" -> getKey(AE) for "+pos,    pos.getAX(),   Convert.o2d(pos.getKey("AE"   )) );
    assertEquals(str+" -> getKey(AN) for "+pos,    pos.getAY(),   Convert.o2d(pos.getKey("AN"   )) );
    assertEquals(str+" -> getKey(AU) for "+pos,    pos.getAZ(),   Convert.o2d(pos.getKey("AU"   )) );
    assertEquals(str+" -> getKey(R) for "+pos,     pos.getR(),    Convert.o2d(pos.getKey("R"    )) );
    assertEquals(str+" -> getKey(T) for "+pos,     pos.getTime(), Convert.o2d(pos.getKey("T"    )) );
  }

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

  public void testCopy ()       { } // tested in testFromGeo
  public void testGetKey ()     { } // tested in testFromGeo
  public void testGetAlt ()     { } // tested in testFromGeo
  public void testGetLat ()     { } // tested in testFromGeo
  public void testGetLon ()     { } // tested in testFromGeo
  public void testGetX ()       { } // tested in testFromGeo
  public void testGetY ()       { } // tested in testFromGeo
  public void testGetZ ()       { } // tested in testFromGeo
  public void testGetVX ()      { } // tested in testFromGeo
  public void testGetVY ()      { } // tested in testFromGeo
  public void testGetVZ ()      { } // tested in testFromGeo
  public void testGetVE ()      { } // tested in testFromGeo
  public void testGetVN ()      { } // tested in testFromGeo
  public void testGetVU ()      { } // tested in testFromGeo
  public void testGetAX ()      { } // tested in testFromGeo
  public void testGetAY ()      { } // tested in testFromGeo
  public void testGetAZ ()      { } // tested in testFromGeo
  public void testGetAE ()      { } // tested in testFromGeo
  public void testGetAN ()      { } // tested in testFromGeo
  public void testGetAU ()      { } // tested in testFromGeo

  public void testFromGeo () {
    for (int i = 0; i < TESTS.length; i++) {
      Position pos = Position.fromGeo(TESTS[i][0], TESTS[i][1], TESTS[i][2]);
      check("fromGeo(double,double,double)", i, pos);
    }

    for (int i = 0; i < TESTS.length; i++) {
      Table tbl = new Table();
      tbl.put("ALT", TESTS[i][0]);
      tbl.put("LAT", TESTS[i][1]);
      tbl.put("LON", TESTS[i][2]);

      Position pos = Position.fromGeo(tbl);
      check("fromGeo(Table)", i, pos);
    }
  }

  public void testSetGeo () {
    Position pos = new Position();
    for (int i = 0; i < TESTS.length; i++) {
      pos.setGeo(TESTS[i][0], TESTS[i][1], TESTS[i][2]);
      check("setGeo(..)", i, pos);
    }
  }

  public void testSetGeoALL () {
    Position pos = new Position();
    for (int i = 0; i < TESTS.length; i++) {
      double[] all = { TESTS[i][0], TESTS[i][1], TESTS[i][2] };
      pos.setGeoALL(all);
      check("setGeoALL(..)", i, pos);
    }
  }

  public void testSetGeoLLA () {
    Position pos = new Position();
    for (int i = 0; i < TESTS.length; i++) {
      double[] lla = { TESTS[i][2], TESTS[i][1], TESTS[i][0] };
      pos.setGeoLLA(lla);
      check("setGeoLLA(..)", i, pos);
    }
  }

  public void testSetAlt () {
    for (int i = 0; i < TESTS.length; i++) {
      Position pos = Position.fromGeo(-1, TESTS[i][1], TESTS[i][2]);
      pos.setAlt(TESTS[i][0]);
Transform.geo2car(pos);
      check("setAlt(..)", i, pos);
    }
  }

  public void testSetLat () {
    for (int i = 0; i < TESTS.length; i++) {
      Position pos = Position.fromGeo(TESTS[i][0], -1, TESTS[i][2]);
      pos.setLat(TESTS[i][1]);
Transform.geo2car(pos);
      check("setLat(..)", i, pos);
    }
  }

  public void testSetLon () {
    for (int i = 0; i < TESTS.length; i++) {
      Position pos = Position.fromGeo(TESTS[i][0], TESTS[i][1], -1);
      pos.setLon(TESTS[i][2]);
Transform.geo2car(pos);
      check("setLon(..)", i, pos);
    }
  }

  public void testFromAltLatLon () {
    for (int i = 0; i < TESTS.length; i++) {
      double[] array = { TESTS[i][0], TESTS[i][1], TESTS[i][2] };
      Position pos = Position.fromAltLatLon(array);
      check("fromAltLatLon(..)", i, pos);
    }
  }

  public void testFromLonLatAlt () {
    for (int i = 0; i < TESTS.length; i++) {
      double[] array = { TESTS[i][2], TESTS[i][1], TESTS[i][0] };
      Position pos = Position.fromLonLatAlt(array);
      check("fromLonLatAlt(..)", i, pos);
    }
  }

  /*
   Test the original (rhumb line) behavior for getHeading.
   IMPORTANT: Latitude and longitude are reversed in the test matrix.
   */
  public void testGetHeading () {
    // Need to use StateVector since a stationary position has no heading
    for (int i = 0; i < HDG_TESTS.length; i++) {
      Position p = Position.fromGeo(0.0, HDG_TESTS[i][1], HDG_TESTS[i][0]);
      Position q = Position.fromGeo(0.0, HDG_TESTS[i][3], HDG_TESTS[i][2]);

      StateVector s = new StateVector();
      s.setCar(p.getX(), p.getY(), p.getZ());
      s.setVel(q.getX()-p.getX(), q.getY()-p.getY(), q.getZ()-p.getZ());
      s.setAcc(0.0, 0.0, 0.0);

      double hdg = HDG_TESTS[i][5];
      if (hdg < 0) hdg = 0.0; // stationary

      assertEquals("getHeading() for "+s, hdg, s.getHeading(), TOL);
    }
  }

  /**
    Tests the setting of a geodetic heading (speed & bearing) and then calculating
    the expected initial bearing along the great arc circle.
    In this test, the Velocity East/North/Up are set independently.
    Therefore, the Position q is essentially not used.
    IMPORTANT: Latitude and longitude are reversed in the test matrix.
    @since NeXtMidas 2.9.2
   */
  public void testGetHeadingGeodetic () {
    // Need to use StateVector since a stationary position has no heading
    for (int i = 0; i < HDG_GEODETIC_TESTS.length; i++) {
      Position p = Position.fromGeo(0.0, HDG_GEODETIC_TESTS[i][1], HDG_GEODETIC_TESTS[i][0]);
      Position q = Position.fromGeo(0.0, HDG_GEODETIC_TESTS[i][3], HDG_GEODETIC_TESTS[i][2]);

      StateVector s = new StateVector();
      s.setGeo(p.getAlt(), p.getLat(), p.getLon());
      s.setVel(q.getX()-p.getX(), q.getY()-p.getY(), q.getZ()-p.getZ());
      s.setVelGeo(HDG_GEODETIC_TESTS[i][6], HDG_GEODETIC_TESTS[i][7], HDG_GEODETIC_TESTS[i][8]);
      s.setAcc(0.0, 0.0, 0.0);


      double hdg = HDG_GEODETIC_TESTS[i][5];
      if (hdg < 0) hdg = 0.0; // stationary

      assertEquals("getHeadingGeodetic() for "+s, hdg, s.getHeadingGeodetic(), TOL);
    }
  }

  public void testGetSpeed () {
    // Need to use StateVector since a stationary position has no speed
    for (double heading = 0.0; heading <= 360.0; heading += 5.0) {
      for (double speed = 0.0; speed <= 20.0; speed += 5.0) {
        StateVector s = new StateVector();
        s.setHeadingSpeed(heading, speed);

        if (speed > 0) {
          assertEquals("getHeading() for "+s+" heading="+heading+" speed="+speed, heading%360, s.getHeading(), TOL);
          assertEquals("getSpeed() for "+s+" heading="+heading+" speed="+speed,   speed,       s.getSpeed(),   TOL);
        }
        else {
          assertEquals("getHeading() for "+s+" heading="+heading+" speed="+speed, 0.0, s.getHeading()); // exactly 0.0 if stationary
          assertEquals("getSpeed() for "+s+" heading="+heading+" speed="+speed,   0.0, s.getSpeed());   // exactly 0.0 if stationary
        }
      }
    }
  }

  /*
   Test the (rhumb line) getHeading().
   IMPORTANT: Latitude and longitude are reversed in the test matrix.
   */
  public void testComputeHeading () {
    for (int i = 0; i < HDG_TESTS.length; i++) {
      String s = "Position.computeHeading("+HDG_TESTS[i][0]+", "+HDG_TESTS[i][1]+", "+HDG_TESTS[i][2]+", "+HDG_TESTS[i][3]+", "+HDG_TESTS[i][4]+")";
      double h = Position.computeHeading(HDG_TESTS[i][0], HDG_TESTS[i][1], HDG_TESTS[i][2], HDG_TESTS[i][3], HDG_TESTS[i][4]);
      assertEquals(s, HDG_TESTS[i][5], h, TOL);

      Position pos1 = Position.fromGeo(0.0, HDG_TESTS[i][1], HDG_TESTS[i][0]);
      Position pos2 = Position.fromGeo(0.0, HDG_TESTS[i][3], HDG_TESTS[i][2]);

      String str = "Position.computeHeading("+pos1+", "+pos2+", "+HDG_TESTS[i][4]+")";
      double hdg = Position.computeHeading(pos1, pos2, HDG_TESTS[i][4]);
      assertEquals(str, HDG_TESTS[i][5], hdg, TOL);
    }
  }

  /*
   Test the new (Haversine Formula, Initial Bearing) getHeading().
   IMPORTANT: Latitude and longitude are reversed in the test matrix.
   @since NeXtMidas 2.9.2
   */
  public void testComputeHeadingPrecise () {
    for (int i = 0; i < HDG_PRECISE_TESTS.length; i++) {
      String s = "Position.computeHeadingPrecise("+HDG_PRECISE_TESTS[i][0]+", "+HDG_PRECISE_TESTS[i][1]+", "+HDG_PRECISE_TESTS[i][2]+", "+HDG_PRECISE_TESTS[i][3]+", "+HDG_PRECISE_TESTS[i][4]+")";
      double h = Position.computeHeadingPrecise(HDG_PRECISE_TESTS[i][0], HDG_PRECISE_TESTS[i][1], HDG_PRECISE_TESTS[i][2], HDG_PRECISE_TESTS[i][3], HDG_PRECISE_TESTS[i][4]);
      assertEquals(s, HDG_PRECISE_TESTS[i][5], h, TOL);

      Position pos1 = Position.fromGeo(0.0, HDG_PRECISE_TESTS[i][1], HDG_PRECISE_TESTS[i][0]);
      Position pos2 = Position.fromGeo(0.0, HDG_PRECISE_TESTS[i][3], HDG_PRECISE_TESTS[i][2]);

      String str = "Position.computeHeadingPrecise("+pos1+", "+pos2+", "+HDG_PRECISE_TESTS[i][4]+")";
      double hdg = Position.computeHeadingPrecise(pos1, pos2, HDG_PRECISE_TESTS[i][4]);
      assertEquals(str, HDG_PRECISE_TESTS[i][5], hdg, TOL);
}
  }

  /*
   Tests the initial bearing value of an object traveling from Position p to
   Position q along the great arc circle.
   @since NeXtMidas 2.9.2
   */
  public void testGreatArcInitialBearing () {
    for (int i = 0; i < HDG_GREAT_ARC_TESTS.length; i++) {

      Position pos1 = Position.fromGeo(0.0, HDG_GREAT_ARC_TESTS[i][1], HDG_GREAT_ARC_TESTS[i][0]);
      Position pos2 = Position.fromGeo(0.0, HDG_GREAT_ARC_TESTS[i][3], HDG_GREAT_ARC_TESTS[i][2]);

      String str = "Position.getGreatArcInitialBearing("+pos1+", "+pos2+", "+HDG_GREAT_ARC_TESTS[i][4]+")";
      double hdg = Position.getGreatArcInitialBearing(pos1, pos2);
      assertEquals(str, HDG_GREAT_ARC_TESTS[i][5], hdg, TOL);
       }
  }

  /** This test compares the performance of loading a SV from a file
   *  with thousands of SV keywords using the contributed efficient
   *  SV keyword code vs using the legacy SV keyword code.
   *
   *  @since NeXtMidas 4.1.3
   */
  public void testSetFileKeywordsEph () {
    // Get the original state of whether or not to use the efficient SV keyword code
    boolean origUseEfficientCode = Position.getUseEfficientSVKeywordCode();

    // Create test DataFile
    String tmpFile = "my_pos_test.tmp{AUX=RAM}";
    Midas M = Shell.getSharedMidasContext();
    Shell.runCommand(M, "headermod/create "+tmpFile);
    DataFile df = new DataFile(M,tmpFile);

    // Populate the DataFile with ephemeral keywords
    df.open();
    df.setKeyScope("EXT");
    Keywords kw = df.getKeywordsObject();

    int numEphKwdEntries = 2500;
    for (int i = 1; i<=numEphKwdEntries; i++) {
      double ephKwdInc = i*1E-8;
      kw.put("ETIM"+i,i);
      kw.put("POSX"+i,0.0111111+ephKwdInc);
      kw.put("POSY"+i,0.0211111+ephKwdInc);
      kw.put("POSZ"+i,0.0311111+ephKwdInc);
      kw.put("VELX"+i,0.0411111+ephKwdInc);
      kw.put("VELY"+i,0.0511111+ephKwdInc);
      kw.put("VELZ"+i,0.0611111+ephKwdInc);
      kw.put("ACCX"+i,0.0711111+ephKwdInc);
      kw.put("ACCY"+i,0.0811111+ephKwdInc);
      kw.put("ACCZ"+i,0.0911111+ephKwdInc);
    }

    df.update();
    df.close();

    // Run the ephemeral keyword test using the efficient SV keyword code and then with legacy code
    ChildPosition posEphTest = new ChildPosition();
    double timeEfficientCode = posEphTest.testSetFileKeywordsEph(df,true);
    double timeLegacyCode = posEphTest.testSetFileKeywordsEph(df,false);
    assertTrue("setFileKeywordsEph() quicker with efficient SV keyword code",timeEfficientCode + 1000 < timeLegacyCode);

    // Cleanup
    Position.setUseEfficientSVKeywordCode(origUseEfficientCode);
    df.erase(true);
  }

  private static final class ChildPosition extends Position {
    private double testSetFileKeywordsEph (DataFile df, boolean useEfficientSVKeywordCode) {
      setUseEfficientSVKeywordCode(useEfficientSVKeywordCode);
      double start = System.currentTimeMillis();
      setFileKeywordsEph(df, true);
      double end = System.currentTimeMillis();
      return end-start;
    }
  }
}
