package nxm.sys.test.junit.lib;

import java.util.ArrayList;
import java.util.Formatter;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import nxm.sys.inc.Units;
import nxm.sys.lib.Data;
import nxm.sys.lib.DataFile;
import nxm.sys.lib.Midas;
import nxm.sys.lib.Shell;
import nxm.sys.lib.StateVector;
import nxm.sys.lib.StateVector.InterpolationOrder;
import nxm.sys.lib.Table;
import nxm.sys.lib.Time;
import nxm.sys.lib.Transform;
import nxm.sys.test.junit.NMTestCase;

/** This will test the StateVector class.
    @author  J. Czechowski IV
 */
public final class StateVectorTest extends NMTestCase {
  private static final String[] AXIS = { "X", "Y", "Z" };
  private static final String[] DIM  = { "POS", "VEL", "ACC" };

  private final Midas M; // since 3.7.0
  private boolean isGradleBuild = false;

  /** public constructor to allow running this test case directly with JUnit. */
  public StateVectorTest (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 StateVectorTest (String methodName, Midas midas) {
    super(methodName, midas);
    M = (midas!=null) ? midas : Shell.getMidasContext();
    isGradleBuild = Shell.isGradleBuild();
  }

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

  public void testGetHeading ()     { } // tested in testSetHeadingSpeed
  public void testGetTime ()        { } // tested in testSetTime
  public void testGetCurrentTime () { } // tested in testSetTime
  public void testGetTimeCode ()    { } // tested in testSetTime
  public void testGetX ()           { } // tested in testSetTime
  public void testGetY ()           { } // tested in testSetTime
  public void testGetZ ()           { } // tested in testSetTime
  public void testGetVX ()          { } // tested in testSetTime
  public void testGetVY ()          { } // tested in testSetTime
  public void testGetVZ ()          { } // tested in testSetTime
  public void testGetAX ()          { } // tested in testSetTime
  public void testGetAY ()          { } // tested in testSetTime
  public void testGetAZ ()          { } // tested in testSetTime
  public void testCopy ()           { } // tested in testSetTime
  public void testSetFile ()        { } // tested in testSetTime
  public void testSetTimeCode ()    { } // tested in testSetTime
  public void testSetCurrentTime () { } // tested in testSetTime
  public void testToTable ()        { } // tested in testSetTime

  public void testProcess () {
    StateVector sv = new StateVector();
    sv.process(); // Bug 1406 - Errors in process() if file not set
  }

  public void testSetHeadingSpeed () {
    double tol = 0.001;
    //                       hdg, spd
    double[][] tests = { {   0.0, 1.0 },
                         {  45.0, 1.0 },
                         {  90.0, 1.0 },
                         { 135.0, 1.0 },
                         { 180.0, 1.0 },
                         { 225.0, 1.0 },
                         { 270.0, 1.0 },
                         { 315.0, 1.0 },
                         { 360.0, 1.0 },  // same as hdg = 0.0
                         {   0.0, 0.0 },  // not moving
                         {  90.0, 0.0 },  // not moving
                         { 180.0, 0.0 },  // not moving
                         { 270.0, 0.0 },  // not moving
                         { 360.0, 0.0 },  // not moving
                       };
    for (int i = 0; i < tests.length; i++) {
      String      str = "setHeadingSpeed("+tests[i][0]+", "+tests[i][1]+")";
      StateVector sv  = new StateVector();
      sv.setHeadingSpeed(tests[i][0], tests[i][1]);

      if (tests[i][1] > 0.0) {
        assertEquals(str+" -> getHeading()", tests[i][0]%360, sv.getHeading(), tol);
        assertEquals(str+" -> getSpeed()",   tests[i][1],     sv.getSpeed(),   tol);
      }
      else {
        assertEquals(str+" -> getHeading()", 0.0, sv.getHeading()); // exactly 0.0 if not moving
        assertEquals(str+" -> getSpeed()",   0.0, sv.getSpeed());   // exactly 0.0 if not moving
      }
    }

    StateVector sv = new StateVector();
    for (double deg = 0.0; deg < 360.0; deg+=0.125) {
      String str = "setHeadingSpeed("+deg+", "+500+")";
      sv.setGeo(1000, 20, 20);
      sv.setHeadingSpeed(deg, 500);

      assertEquals(str+" -> getHeading()", deg, sv.getHeading(), tol);
    }
  }

  /** Test heading set for a geodetic computation.
      @since NeXtMidas 2.9.2
   */
  public void testSetHeadingSpeedGeodetic () {
    double tol = 0.001;

    //                     hdg, speed, geodetic bearing
    double[][] tests =
                       { {   0.0, 0.0, 90.0 },
                         {  45.0, 1.0, 44.997 },
                         {  90.0, 1.0, 90.0 },
                         { 135.0, 1.0, 135.002 },
                         { 180.0, 1.0, 180.0 },
                         { 225.0, 1.0, 224.997 },
                         { 270.0, 1.0, 270.0 },
                         { 315.0, 1.0, 315.002 },
                         { 360.0, 1.0, 0.0 },  // same as hdg = 0.0
                         {   0.0, 0.0, 0.0 },  // not moving
                         {  90.0, 0.0, 0.0 },  // not moving
                         { 180.0, 0.0, 0.0 },  // not moving
                         { 270.0, 0.0, 0.0 },  // not moving
                         { 360.0, 0.0, 0.0 },  // not moving
                       };

     //                    hdg, speed, geodetic bearing
    double[][] geo_tests =
                       { {   0.0, 0.0, 0.0 },
                         {  45.0, 1.0, 34.232 },
                         {  90.0, 1.0, 83.974 },
                         { 135.0, 1.0, 132.9000 },
                         { 180.0, 1.0, 180.0 },
                         { 225.0, 1.0, 227.099 },
                         { 270.0, 1.0, 276.0250 },
                         { 315.0, 1.0, 325.767 },
                         { 360.0, 1.0, 0.0 },  // same as hdg = 0.0
                         {   0.0, 0.0, 0.0 },  // not moving
                         {  90.0, 0.0, 83.974 },
                         { 180.0, 0.0, 180.00 },
                         { 270.0, 0.0, 276.0250 },
                         { 360.0, 0.0, 0.0 },
                       };

    double test_alt = 1000.0;
    double test_lat = 20.0;
    double test_lon = 20.0;
    double test_low_speed = 34.3;    // Mach .1
    double test_high_speed = 340.29; // Mach 1.0

    for (int i = 0; i < tests.length; i++) {
      String      str = "setHeadingSpeedGeodetic("+tests[i][0]+", "+tests[i][1]+")";
      StateVector sv  = new StateVector();
      sv.setHeadingSpeedGeodetic(tests[i][0], tests[i][1]);

      if (tests[i][1] > 0.0) {
        assertEquals(str+" -> getHeadingGeodetic()", tests[i][2], sv.getHeadingGeodetic(), tol);
        assertEquals(str+" -> getHorizontalSpeed()", tests[i][1], sv.getHorizontalSpeed(), tol);
      }
      else {
        assertEquals(str+" -> getHeadingGeodetic()", 0.0, sv.getHeadingGeodetic()); // exactly 0.0 if not moving
        assertEquals(str+" -> getHorizontalSpeed()", 0.0, sv.getHorizontalSpeed()); // exactly 0.0 if not moving
      }
    }

    // Low-speed tests (Mach .1 or less)
    for (int i = 0; i < geo_tests.length; i++) {
      StateVector sv = new StateVector();
      String str = "setHeadingSpeedGeodetic("+geo_tests[i][0]+", "+test_low_speed+")";
      sv.setGeo(test_alt, test_lat, test_lon);
      sv.setHeadingSpeedGeodetic(geo_tests[i][0], test_low_speed);

      // At low speeds, degree and geodetic bearing are nearly the same
      assertEquals(str+" -> getHeadingGeodetic()", geo_tests[i][2], sv.getHeadingGeodetic(), tol);
    }

  }

  /** test StateVector.toKeywordsEph(ListOfTimes) (for BUG 2981).
   *  test position and velocity values for each time entry.
   *  NOTE: timeList increments by 600 seconds, while sv1.prm data file increments by 300 seconds.
   *  Also, check interpolated values from times in between actual data points.
   *  @since NeXtMidas 3.7.0
   */
  public void testKeywordsEph () {
    if (isGradleBuild) {
      M.warning("nxm.sys.junit.lib.StateVectorTest skipping testKeywordsEph() because fails in Gradle build");
      return;
    }
    final double tol = 1e-6; // tolerance for double checks
    Time currentTime = new Time("00:00:00");
    String      fname = urlDat+"sv1.prm{TIMECODE="+currentTime+"}";
    DataFile    df    = new DataFile(M, fname);
    StateVector sv    = new StateVector(M, fname);
    List<Time>  timeList = new ArrayList<>();

    for (int k=0; k<5; k++) {
      Time time = new Time(currentTime.addSec(600));
      timeList.add(time);
    }

    Map<String, Object> ephKW = sv.toKeywordsEph(timeList); // <-- API to test

    df.open();
    for (int jKW=1, iDF=2; jKW<=timeList.size(); jKW++, iDF+=2) {
      Table  exptbl = df.getDataTable(iDF);
      Data   expPos = exptbl.getData("POS");
      Data   expVel = exptbl.getData("VEL");

      assertEquals("ephKeyword D:ETIM"+jKW, df.getTimeAt(iDF), ephKW.get("D:ETIM"+jKW));
      assertEquals("ephKeyword D:POSX"+jKW, expPos.getD(0),    ephKW.get("D:POSX"+jKW));
      assertEquals("ephKeyword D:POSY"+jKW, expPos.getD(1),    ephKW.get("D:POSY"+jKW));
      assertEquals("ephKeyword D:POSZ"+jKW, expPos.getD(2),    ephKW.get("D:POSZ"+jKW));
      assertEquals("ephKeyword D:VELX"+jKW, expVel.getD(0),    ephKW.get("D:VELX"+jKW));
      assertEquals("ephKeyword D:VELY"+jKW, expVel.getD(1),    ephKW.get("D:VELY"+jKW));
      assertEquals("ephKeyword D:VELZ"+jKW, expVel.getD(2),    ephKW.get("D:VELZ"+jKW));
      // no acceleration in test file, hence zero checks below
      assertEquals("ephKeyword D:ACCX"+jKW,            0.0,    ephKW.get("D:ACCX"+jKW));
      assertEquals("ephKeyword D:ACCY"+jKW,            0.0,    ephKW.get("D:ACCY"+jKW));
      assertEquals("ephKeyword D:ACCZ"+jKW,            0.0,    ephKW.get("D:ACCZ"+jKW));
    }
    df.close();

    // test interpolation for times in between actual state vector points
    // The values in this first test case are just taken from the output of NeXtMidas 3.7.0 and are not ones we
    // actually computed by hand. If interpolation algorithm changes in StateVector.toKeywordsEph(timeList),
    // then the expected tested values should change below.
    List<Time> interpolateTimes = new ArrayList<>();
    interpolateTimes.add(new Time(100.0));
    interpolateTimes.add(new Time(333.3));
    interpolateTimes.add(new Time(999.9));
    double[][] expectedResults = { // MUST match size of interpolateTimes
    //  time,    x,                    y,                    z,                  velX,              velY,               velZ
        {100.0, -3.477984173309881E7, -2.3871446632876515E7, 37448.23383249337, -6.743837872023694, 13.009996276434643, 267.8267293869415 },
        {333.3, -3.47813703297789E7,  -2.3868389327589642E7, 99927.1099118731,  -6.357190379325632, 13.194063454923427, 267.74953438887593},
        {999.9, -3.478522973234439E7, -2.3859436855815474E7, 278225.087650736,  -5.211680027357109, 13.64532399370521,  267.0721512049311 },
    };

    Map<String,Object> interpolateEphKW = sv.toKeywordsEph(interpolateTimes); // <-- API to test

    int ii = 1; // result is one-based
    for (double[] expected : expectedResults) {
      assertEquals("interpolated ephKeyword D:ETIM"+ii, expected[0], (double)interpolateEphKW.get("D:ETIM"+ii));
      assertEquals("interpolated ephKeyword D:POSX"+ii, expected[1], (double)interpolateEphKW.get("D:POSX"+ii), tol);
      assertEquals("interpolated ephKeyword D:POSY"+ii, expected[2], (double)interpolateEphKW.get("D:POSY"+ii), tol);
      assertEquals("interpolated ephKeyword D:POSZ"+ii, expected[3], (double)interpolateEphKW.get("D:POSZ"+ii), tol);
      assertEquals("interpolated ephKeyword D:VELX"+ii, expected[4], (double)interpolateEphKW.get("D:VELX"+ii), tol);
      assertEquals("interpolated ephKeyword D:VELY"+ii, expected[5], (double)interpolateEphKW.get("D:VELY"+ii), tol);
      assertEquals("interpolated ephKeyword D:VELZ"+ii, expected[6], (double)interpolateEphKW.get("D:VELZ"+ii), tol);
      // no acceleration in test file, hence zero checks below
      assertEquals("interpolated ephKeyword D:ACCX"+ii,         0.0, (double)interpolateEphKW.get("D:ACCX"+ii));
      assertEquals("interpolated ephKeyword D:ACCY"+ii,         0.0, (double)interpolateEphKW.get("D:ACCY"+ii));
      assertEquals("interpolated ephKeyword D:ACCZ"+ii,         0.0, (double)interpolateEphKW.get("D:ACCZ"+ii));
      ii++;
    }

    _testWithCurrTime();
  }

  /** test written to check that the current times are set correctly */
  private void _testWithCurrTime () {
    // pretty much the same test just using current time for values
    Time currentTime = Time.currentTime();
    String fname = urlDat+"sv1.prm{TIMECODE="+currentTime+"}";
    DataFile df    = new DataFile(M, fname);
    StateVector sv    = new StateVector(M, fname);
    List<Time> timeList = new ArrayList<>();
    for (int k=0; k<5; k++) {
      Time time = new Time(currentTime.getSec()+((k+1) * 600));
      timeList.add(time);
    }
    Map<String, Object> ephKW = sv.toKeywordsEph(timeList); // <-- API to test
    df.open();

    for (int jKW=1, iDF=2; jKW<=timeList.size(); jKW++, iDF+=2) {
      Table  exptbl = df.getDataTable(iDF);
      Data   expPos = exptbl.getData("POS");
      Data   expVel = exptbl.getData("VEL");

      assertEquals("ephKeyword D:ETIM"+jKW, df.getTimeAt(iDF), ephKW.get("D:ETIM"+jKW));
      assertEquals("ephKeyword D:POSX"+jKW, expPos.getD(0),    ephKW.get("D:POSX"+jKW));
      assertEquals("ephKeyword D:POSY"+jKW, expPos.getD(1),    ephKW.get("D:POSY"+jKW));
      assertEquals("ephKeyword D:POSZ"+jKW, expPos.getD(2),    ephKW.get("D:POSZ"+jKW));
      assertEquals("ephKeyword D:VELX"+jKW, expVel.getD(0),    ephKW.get("D:VELX"+jKW));
      assertEquals("ephKeyword D:VELY"+jKW, expVel.getD(1),    ephKW.get("D:VELY"+jKW));
      assertEquals("ephKeyword D:VELZ"+jKW, expVel.getD(2),    ephKW.get("D:VELZ"+jKW));
      // no acceleration in test file, hence zero checks below
      assertEquals("ephKeyword D:ACCX"+jKW,            0.0,    ephKW.get("D:ACCX"+jKW));
      assertEquals("ephKeyword D:ACCY"+jKW,            0.0,    ephKW.get("D:ACCY"+jKW));
      assertEquals("ephKeyword D:ACCZ"+jKW,            0.0,    ephKW.get("D:ACCZ"+jKW));
    }

    df.close();

    List<Time> interpolateTimes = new ArrayList<>();
    interpolateTimes.add(new Time(100.0));
    interpolateTimes.add(new Time(333.3));
    interpolateTimes.add(new Time(999.9));

    double[] expectedTimes = new double[]{ currentTime.getSec() + 100.0, currentTime.getSec() + 333.3, currentTime.getSec() + 999.9};

    Map<String,Object> interpolateEphKW = sv.toKeywordsEph(interpolateTimes); // <-- API to test

    int ii = 1; // result is one-based
    for (double time : expectedTimes) {
      assertEquals("interpolated ephKeyword D:ETIM"+ii, time, (double)interpolateEphKW.get("D:ETIM"+ii),1e-6);
      ii++;
    }

  }
  /** Sets time (tc=false) or timecode (tc=true). */
  public void testSetTime () {
    if (isGradleBuild) {
      M.warning("nxm.sys.junit.lib.StateVectorTest skipping testSetTime() because fails in Gradle build");
      return;
    }
    String      fname = urlDat+"sv1.prm{TIMECODE=2008:01:01::00:00:00}";
    DataFile    df    = new DataFile(M, fname);
    StateVector svT   = new StateVector(M, fname);
    StateVector svTC  = new StateVector(M, fname);
    StateVector svCT  = new StateVector(M, fname);

    df.open();
    Time   startTime = df.getTime();
    double timecode  = df.getTimeCode();
    int    xdelta    = (int)df.getXDelta();
    int    size      = (int)df.getSize();
    double tol       = 0.001;

    for (int i = 0; i < size; i++) {
      double time    = i * xdelta;
      double tc      = timecode + time;
      Time   curTime = Time.add(startTime, time);

      svT.setTime(time);
      svTC.setTimeCode(tc);
      svCT.setCurrentTime(curTime);

      String[]      descr = { "setTime("+time+")",           "setTime("+time+") [copy]",
                              "setTimeCode("+tc+")",         "setTimeCode("+tc+") [copy]",
                              "setCurrentTime("+curTime+")", "setCurrentTime("+curTime+") [copy]" };
      StateVector[] vec   = { svT,  svT.copy(),
                              svTC, svTC.copy(),
                              svCT, svCT.copy() };

      Table  tbl = df.getDataTable(i);
      Data   pos = tbl.getData("POS");
      Data   vel = tbl.getData("VEL");
      for (int j = 0; j < vec.length; j++) {
        if ((j % 2) == 1) { // if (isCopy) {
          // In 3.3.0 we made it so that .copy() copies the currentTime too!
          assertEquals(descr[j]+" -> getTimeCode()",           tc,                vec[j].getTimeCode(),              tol);
          assertEquals(descr[j]+" -> getCurrentTime() [wsec]", curTime.getWSec(), vec[j].getCurrentTime().getWSec(), 0.0);
          assertEquals(descr[j]+" -> getCurrentTime() [fsec]", curTime.getFSec(), vec[j].getCurrentTime().getFSec(), tol);
        }
        assertEquals(descr[j]+" -> getTime()", time,        vec[j].getTime(), tol);
        assertEquals(descr[j]+" -> getX()",    pos.getD(0), vec[j].getX(),    tol);
        assertEquals(descr[j]+" -> getY()",    pos.getD(1), vec[j].getY(),    tol);
        assertEquals(descr[j]+" -> getZ()",    pos.getD(2), vec[j].getZ(),    tol);
        assertEquals(descr[j]+" -> getVX()",   vel.getD(0), vec[j].getVX(),   tol);
        assertEquals(descr[j]+" -> getVY()",   vel.getD(1), vec[j].getVY(),   tol);
        assertEquals(descr[j]+" -> getVZ()",   vel.getD(2), vec[j].getVZ(),   tol);
        assertEquals(descr[j]+" -> getAX()",   0.0,         vec[j].getAX(),   0.0); // no ACC in file
        assertEquals(descr[j]+" -> getAY()",   0.0,         vec[j].getAY(),   0.0); // no ACC in file
        assertEquals(descr[j]+" -> getAZ()",   0.0,         vec[j].getAZ(),   0.0); // no ACC in file

        // Make sure the toTable() function returns the expected values. Note that the
        // check for t.size() should be updated if any additional fields are inserted
        // subsequent to NeXtMidas 3.3.2.
        Table t = vec[j].toTable();
        assertEquals(descr[j]+" -> getTable() size",            24,                      t.size());
        assertEquals(descr[j]+" -> getTable() for CURRENTTIME", vec[j].getCurrentTime(), t.getO("CURRENTTIME"));
        assertEquals(descr[j]+" -> getTable() for TIME",        vec[j].getTime(),        t.getD("TIME"));
        assertEquals(descr[j]+" -> getTable() for TIMECODE",    vec[j].getTimeCode(),    t.getD("TIMECODE"));
        assertEquals(descr[j]+" -> getTable() for SPEED",       vec[j].getSpeed(),       t.getD("SPEED"));
        assertEquals(descr[j]+" -> getTable() for HEADING",     vec[j].getHeading(),     t.getD("HEADING"));
        assertEquals(descr[j]+" -> getTable() for R",           vec[j].getR(),           t.getD("R"));
        assertEquals(descr[j]+" -> getTable() for LON",         vec[j].getLon(),         t.getD("LON"));
        assertEquals(descr[j]+" -> getTable() for LAT",         vec[j].getLat(),         t.getD("LAT"));
        assertEquals(descr[j]+" -> getTable() for ALT",         vec[j].getAlt(),         t.getD("ALT"));
        assertEquals(descr[j]+" -> getTable() for X",           vec[j].getX(),           t.getD("X"));
        assertEquals(descr[j]+" -> getTable() for Y",           vec[j].getY(),           t.getD("Y"));
        assertEquals(descr[j]+" -> getTable() for Z",           vec[j].getZ(),           t.getD("Z"));
        assertEquals(descr[j]+" -> getTable() for VX",          vec[j].getVX(),          t.getD("VX"));
        assertEquals(descr[j]+" -> getTable() for VY",          vec[j].getVY(),          t.getD("VY"));
        assertEquals(descr[j]+" -> getTable() for VZ",          vec[j].getVZ(),          t.getD("VZ"));
        assertEquals(descr[j]+" -> getTable() for AX",          vec[j].getAX(),          t.getD("AX"));
        assertEquals(descr[j]+" -> getTable() for AY",          vec[j].getAY(),          t.getD("AY"));
        assertEquals(descr[j]+" -> getTable() for AZ",          vec[j].getAZ(),          t.getD("AZ"));
        assertEquals(descr[j]+" -> getTable() for VU",          vec[j].getVU(),          t.getD("VU"));
        assertEquals(descr[j]+" -> getTable() for VE",          vec[j].getVE(),          t.getD("VE"));
        assertEquals(descr[j]+" -> getTable() for VN",          vec[j].getVN(),          t.getD("VN"));
        assertEquals(descr[j]+" -> getTable() for AU",          vec[j].getAU(),          t.getD("AU"));
        assertEquals(descr[j]+" -> getTable() for AE",          vec[j].getAE(),          t.getD("AE"));
        assertEquals(descr[j]+" -> getTable() for AN",          vec[j].getAN(),          t.getD("AN"));
      }
    }
    df.close();
  }

  /** @since NeXtMidas 3.1.1  */
  public void testGetPosVelAccAtTime () {
    double   tol   = 0.0000001;
    String   fname = urlDat+"sv3.prm";  // expected values in the assets correlates to this file
    DataFile df    = new DataFile(M, fname);

    // The values in this first test case are just taken from the output of NeXtMidas 3.3.2
    // and are not ones we actually computed by hand. This therefore presumes:
    //   (a) The default interpolation method remains CubicHermite (which docs say
    //       may change), and
    //   (b) The algorithm in StateVector.interpolate(..) is correct in NeXtMidas 3.3.2
    //       (hint: it wasn't in NeXtMidas 3.3.1).
    // If either assumption proves incorrect, then the tested values should change. Note
    // that this test is NOT intended to test StateVector.interpolate(..) [there are already
    // tests for that, but rather tests StateVector.getPosVelAccAtTime(..).
    //
    // t=3100.0 falls between entries 10 and 11 in sv3.prm
    double[] pos = StateVector.getPosVelAccAtTime(df, 3100.0, null, false);
    assertEquals("getPosVelAccAtTime() interpolated -> POSX", -1.8734353265262954E7, pos[0],  tol);
    assertEquals("getPosVelAccAtTime() interpolated -> POSY", -1.5909419432624778E7, pos[1],  tol);
    assertEquals("getPosVelAccAtTime() interpolated -> POSZ",  1.6718852091992319E7, pos[2],  tol);
    assertEquals("getPosVelAccAtTime() interpolated -> VELX",    1647.0472695916728, pos[3],  tol);
    assertEquals("getPosVelAccAtTime() interpolated -> VELY",    1445.2382807725166, pos[4],  tol);
    assertEquals("getPosVelAccAtTime() interpolated -> VELZ",    1980.7977183606752, pos[5],  tol);
    assertEquals("getPosVelAccAtTime() interpolated -> ACCX",                     0, pos[6],  tol);
    assertEquals("getPosVelAccAtTime() interpolated -> ACCY",                     0, pos[7],  tol);
    assertEquals("getPosVelAccAtTime() interpolated -> ACCZ",                     0, pos[8],  tol);

    pos = StateVector.getPosVelAccAtTime(df, 0, null, false);  // should return the first element
    assertEquals("getPosVelAccAtTime() -> first row -> POSX", -2.1939673736313824E7, pos[0],  tol);
    assertEquals("getPosVelAccAtTime() -> first row -> POSY", -2.0515243050649006E7, pos[1],  tol);
    assertEquals("getPosVelAccAtTime() -> first row -> POSZ",     9581556.450969407, pos[2],  tol);
    assertEquals("getPosVelAccAtTime() -> first row -> VELX",    433.40716847538744, pos[3],  tol);
    assertEquals("getPosVelAccAtTime() -> first row -> VELY",    1436.3514393584549, pos[4],  tol);
    assertEquals("getPosVelAccAtTime() -> first row -> VELZ",    2555.4188687730907, pos[5],  tol);
    assertEquals("getPosVelAccAtTime() -> first row -> ACCX",                     0, pos[6],  tol);
    assertEquals("getPosVelAccAtTime() -> first row -> ACCY",                     0, pos[7],  tol);
    assertEquals("getPosVelAccAtTime() -> first row -> ACCZ",                     0, pos[8],  tol);

    pos = StateVector.getPosVelAccAtTime(df, 300000000, null, false);  // should return the last element
    assertEquals("getPosVelAccAtTime() -> last row -> POSX", -2.1841718024894763E7, pos[0], tol);
    assertEquals("getPosVelAccAtTime() -> last row -> POSY",  -2.016125951513227E7, pos[1], tol);
    assertEquals("getPosVelAccAtTime() -> last row -> POSZ",  1.0176599639889173E7, pos[2], tol);
    assertEquals("getPosVelAccAtTime() -> last row -> VELX",     522.1453092013229, pos[3], tol);
    assertEquals("getPosVelAccAtTime() -> last row -> VELY",    1455.2893742830693, pos[4], tol);
    assertEquals("getPosVelAccAtTime() -> last row -> VELZ",    2525.8326203472557, pos[5], tol);
    assertEquals("getPosVelAccAtTime() -> last row -> ACCX",                     0, pos[6], tol);
    assertEquals("getPosVelAccAtTime() -> last row -> ACCY",                     0, pos[7], tol);
    assertEquals("getPosVelAccAtTime() -> last row -> ACCZ",                     0, pos[8], tol);

    // test cases for
    // getPosVelAccAtTime(List<Table>, Time, Transform, boolean, InterpolationOrder)
    final double   FSEC_DN =   0.599980016;
    final double   FSEC    =   0.599980032;
    final double   FSEC_UP =   0.599980048;
    final double[] POS1    = { 1.0,         2.0,         3.0,         1.0, 2.0, 3.0, 0.0, 0.0, 0.0 };
    final double[] POS1_UP = { 1.000000016, 2.000000032, 3.000000048, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0 };
    final double[] POS2_DN = { 1.999999984, 3.999999968, 5.999999952, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0 };
    final double[] POS2    = { 2.0,         4.0,         6.0,         1.0, 2.0, 3.0, 0.0, 0.0, 0.0 };
    final double[] POS2_UP = { 2.000000016, 4.000000032, 6.000000048, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0 };
    final double[] POS3_DN = { 2.999999984, 5.999999968, 8.999999952, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0 };
    final double[] POS3    = { 3.0,         6.0,         9.0,         1.0, 2.0, 3.0, 0.0, 0.0, 0.0 };
    final double   TOL     =   0.0000000005;
    final List<Table> lt = new ArrayList<>();
    lt.add(vectorTable(1, FSEC, POS1));
    lt.add(vectorTable(2, FSEC, POS2));
    lt.add(vectorTable(3, FSEC, POS3));
    final Transform t = new Transform(Transform.ECR, Units.CARTESIAN);
    final boolean b = false;
    final InterpolationOrder io = InterpolationOrder.CubicHermite;
    assertArrayEquals("getPosVelAccAtTime() for TIME 1-epsilon", POS1   , StateVector.getPosVelAccAtTime(lt,new Time(1,FSEC_DN),t,b,io), TOL);
    assertArrayEquals("getPosVelAccAtTime() for TIME 1",         POS1   , StateVector.getPosVelAccAtTime(lt,new Time(1,FSEC   ),t,b,io), TOL);
    assertArrayEquals("getPosVelAccAtTime() for TIME 1+epsilon", POS1_UP, StateVector.getPosVelAccAtTime(lt,new Time(1,FSEC_UP),t,b,io), TOL);
    assertArrayEquals("getPosVelAccAtTime() for TIME 2-epsilon", POS2_DN, StateVector.getPosVelAccAtTime(lt,new Time(2,FSEC_DN),t,b,io), TOL);
    assertArrayEquals("getPosVelAccAtTime() for TIME 2",         POS2   , StateVector.getPosVelAccAtTime(lt,new Time(2,FSEC   ),t,b,io), TOL);
    assertArrayEquals("getPosVelAccAtTime() for TIME 2+epsilon", POS2_UP, StateVector.getPosVelAccAtTime(lt,new Time(2,FSEC_UP),t,b,io), TOL);
    assertArrayEquals("getPosVelAccAtTime() for TIME 3-epsilon", POS3_DN, StateVector.getPosVelAccAtTime(lt,new Time(3,FSEC_DN),t,b,io), TOL);
    assertArrayEquals("getPosVelAccAtTime() for TIME 3",         POS3   , StateVector.getPosVelAccAtTime(lt,new Time(3,FSEC   ),t,b,io), TOL);
    assertArrayEquals("getPosVelAccAtTime() for TIME 3+epsilon", POS3   , StateVector.getPosVelAccAtTime(lt,new Time(3,FSEC_UP),t,b,io), TOL);
  }

  private Table vectorTable (double wsec, double fsec, double[] pos) {
    Map<String, Object> map = new HashMap<>();
    map.put("TIME", new Time(wsec, fsec));
    map.put( "X", pos[0]);
    map.put( "Y", pos[1]);
    map.put( "Z", pos[2]);
    map.put("VX", pos[3]);
    map.put("VY", pos[4]);
    map.put("VZ", pos[5]);
    map.put("AX", pos[6]);
    map.put("AY", pos[7]);
    map.put("AZ", pos[8]);
    return new Table(map);
  }

  //////////////////////////////////////////////////////////////////////////////////////////////////
  // Interpolation tests.
  //////////////////////////////////////////////////////////////////////////////////////////////////

  public void testInterpolate () {
    final double TOL = 1e-12; // tolerance for linear (rounding errors only)
    final PositionPolynomial[] TESTS = {
      // Fixed position tests
      new PositionPolynomial(3, 0,  0, 0),   // ( 0, 0, 0)
      new PositionPolynomial(3, 1, -2, 3),   // (+1,-2,+3)
      // Fixed velocity tests
      new PositionPolynomial(3, 1,0, -2,0, 3,0 ),
      new PositionPolynomial(3, 1,2, -2,4, 3,-6),
      // Fixed acceleration tests
      new PositionPolynomial(3, 1,0,0, -2,0,0,  3,0,0 ),
      new PositionPolynomial(3, 1,2,9, -2,4,-2, 3,-6,5),
      // Fixed jerk tests
      new PositionPolynomial(3, 1,0,0,0,  -2,0,0,0,  3,0,0,0 ),
      new PositionPolynomial(3, 1,2,9,-1, -2,4,-2,4, 3,-6,5,2),
    };

    for (InterpolationOrder order : InterpolationOrder.values()) {
      for (PositionPolynomial pp : TESTS) {
        for (int i = 0; i <= 120; i++) {
          double     t0   = 0.0;
          double     t1   = 10.0;
          double     t    = (i / 10.0) - 1.0;  // test from -1.0 to +11.0
          double     dt   = t1 - t0;
          int        dim  = pp.dim;
          double[][] pva0 = pp.getPosVelAccAt(t0);
          double[][] pva1 = pp.getPosVelAccAt(t1);
          double[][] pvaE = pp.getPosVelAccAt(t );  // correct output @ t
          double[][] pvaA = new double[3][dim];     // actual  output @ t (double)
          double[][] pvaT = new double[3][dim];     // actual  output @ t (Time)
          double[][] pvaL = new double[3][dim];     // linear  output @ t

          // Get actual output
          StateVector.interpolate(pva0[0], pva0[1], pva0[2],  0.0,
                                  pva1[0], pva1[1], pva1[2], 10.0,
                                  pvaA[0], pvaA[1], pvaA[2],   t , 3, order);

          // Get output with Time values
          Time time0 = new Time("2012:01:01::00:00:00").addSec(t0);
          Time time1 = new Time("2012:01:01::00:00:00").addSec(t1);
          Time timeT = new Time("2012:01:01::00:00:00").addSec(t );
          StateVector.interpolate(pva0[0], pva0[1], pva0[2], time0,
                                  pva1[0], pva1[1], pva1[2], time1,
                                  pvaT[0], pvaT[1], pvaT[2], timeT, 3, order);
          for (int a = 0; a < 3; a++) {
            assertArrayEquals(""+time0+" matches "+t+" with\n"+pp, pvaT[a], pvaA[a], TOL);
          }

          // Solve for expected linear output
          for (int a = 0; a < 3; a++) {
            for (int d = 0; d < dim; d++) {
              pvaL[a][d] = pva0[a][d] + (pva1[a][d] - pva0[a][d])*(t/dt);
            }
          }

          if (t <= t0) {
            // Special case, must exactly match initial values
            for (int a = 0; a < 3; a++) {
              assertArrayEquals(""+order+" "+AXIS[a]+" for t="+t+" with\n"+pp, pva0[a], pvaA[a], TOL);
            }
          }
          else if (t >= t1) {
            // Special case, must exactly match final values
            for (int a = 0; a < 3; a++) {
              assertArrayEquals(""+order+" "+AXIS[a]+" for t="+t+" with\n"+pp, pva1[a], pvaA[a], TOL);
            }
          }
          else if (order == InterpolationOrder.Linear) {
            // Linear is simple, we just do a linear interp (above) and make sure
            // that the function in StateVector.java matches
            for (int a = 0; a < 3; a++) {
              assertArrayEquals(""+order+" "+AXIS[a]+" for t="+t+" with\n"+pp, pvaL[a], pvaA[a], TOL);
            }
          }
          else {
            // The Hermite versions are harder as we can't solve for the expected
            // values here with a trivial solution to check values against. Therefore
            // we just check to see that the delta for the actual values is less than
            // 150% of the delta for a trivial Linear solution. If the delta for
            // linear values is very small, we use 1% of avg value or a reasonable
            // tolerance. Note that this test isn't intended as a "be all end all"
            // test and merely a complement to the tests for the SVINTERP function
            // in %TEST_CONVERT.
            for (int d = 0; d < dim; d++) {
              double avg  = 0;
              double dlin = 0;
              double dact = 0;
              for (int a = 0; a < 3; a++) {
                avg  += pvaE[a][d];
                dlin += Math.abs(pvaL[a][d] - pvaE[a][d]);
                dact += Math.abs(pvaA[a][d] - pvaE[a][d]);
              }
              avg  = avg  / 3;
              dlin = dlin / 3;
              dact = dact / 3;

              double limit;
              switch (d) {
                case 0:  limit = Math.max(Math.abs(avg*0.01), 0.01    ); break; // POS
                case 1:  limit = Math.max(Math.abs(avg*0.01), 0.0001  ); break; // VEL
                case 2:  limit = Math.max(Math.abs(avg*0.01), 0.000001); break; // ACC
                default: throw new AssertionError("Got d="+d); // Impossible
              }

              double dexp  = (dlin < limit)? limit : dlin*1.5;
              assertTrue(""+order+" "+DIM[d]+" for t="+t+" with\n"+pp, dact <= dexp);
            }
          }
        }
      }
    }
  }

  /** A polynomial describing a given position. */
  private static class PositionPolynomial {
    private final int      dim;
    private final int      nterms;
    private final double[] terms;

    /** Creates a new instance.
        @param dim   Dimensionality (1={X}, 2={X,Y}, 3={X,Y,Z}).
        @param terms Polynomial terms. This must be a multiple of dim; for example:
                     { x3,x2,x1,x0,  y3,y2,y1,y0,  z3,z2,z1,z0}
    */
    public PositionPolynomial (int dim, double ... terms) {
      this.dim    = dim;
      this.nterms = terms.length / dim;
      this.terms  = terms;
    }

    @Override
    public String toString () {
      Formatter strFormatter = new Formatter();

      int off = 0;
      for (int i = 0; i < dim; i++) {
        if (i > 0) strFormatter.format("%n");
        strFormatter.format("%s(t) = ", AXIS[i]);

        for (int j = 0; j < nterms; j++) {
          int    pow = nterms-j-1;
          double val = terms[off++];
          char   sgn = '+';

          if (val < 0) {
            val = -val;
            sgn = '-';
          }

          if (j == 0) strFormatter.format("%c", sgn);
          else        strFormatter.format(" %c ", sgn);

               if (pow == 0) strFormatter.format("%g",      val);
          else if (pow == 1) strFormatter.format("%g t",    val);
          else               strFormatter.format("%g t^%d", val, pow);
        }
      }
      String retValue = strFormatter.toString();
      strFormatter.close();
      return retValue;
    }

    /** Evaluates the polynomial at the given time.
        @return {pos,vel,acc}
     */
    public double[][] getPosVelAccAt (double t) {
      double[][] pva = new double[3][dim];

      int off = 0;
      for (int i = 0; i < dim; i++) {
        double p = 0;
        double v = 0;
        double a = 0;

        for (int j = 0; j < nterms; j++) {
          int    pow = nterms-j-1;
          double val = terms[off++];

          if (pow >= 0) p += val * Math.pow(t, pow  );
          if (pow >= 1) v += val * Math.pow(t, pow-1);
          if (pow >= 2) a += val * Math.pow(t, pow-2);
        }
        pva[0][i] = p;
        pva[1][i] = v;
        pva[2][i] = a;
      }
      return pva;
    }
  }

}
