SpicePosition.cpp

#include "SpicePosition.h"

#include <algorithm>
#include <cfloat>
#include <iomanip>

#include <QString>
#include <QStringList>
#include <QChar>

#include "BasisFunction.h"
#include "IException.h"
#include "LeastSquares.h"
#include "LineEquation.h"
#include "NaifStatus.h"
#include "NumericalApproximation.h"
#include "PolynomialUnivariate.h"
#include "TableField.h"

namespace Isis {
  SpicePosition::SpicePosition(int targetCode, int observerCode) {
    init(targetCode, observerCode, false);
  }

  SpicePosition::SpicePosition(int targetCode, int observerCode,
                               bool swapObserverTarget) {
    init(targetCode, observerCode, swapObserverTarget);
  }

  void SpicePosition::init(int targetCode, int observerCode,
                           const bool &swapObserverTarget) {

    p_aberrationCorrection = "LT+S";
    p_baseTime = 0.;
    p_coefficients[0].clear();
    p_coefficients[1].clear();
    p_coefficients[2].clear();
    p_coordinate.resize(3);
    p_degree = 2;
    p_degreeApplied = false;
    p_et = -DBL_MAX;
    p_fullCacheStartTime = 0;
    p_fullCacheEndTime = 0;
    p_fullCacheSize = 0;
    p_hasVelocity = false;
 
    p_override = NoOverrides;
    p_source = Spice;

    p_timeBias = 0.0;
    p_timeScale = 1.;
    p_velocity.resize(3);
    p_xhermite = NULL;
    p_yhermite = NULL;
    p_zhermite = NULL;

    m_swapObserverTarget = swapObserverTarget;
    m_lt = 0.0;

    // Determine observer/target ordering
    if ( m_swapObserverTarget ) {
      // New/improved settings.. Results in vector negation in SetEphemerisTimeSpice
      p_targetCode = observerCode;
      p_observerCode = targetCode;
    }
    else {
      // Traditional settings
      p_targetCode = targetCode;
      p_observerCode = observerCode;
    }
  }


  SpicePosition::~SpicePosition() {
    ClearCache();
  }


  void SpicePosition::SetTimeBias(double timeBias) {
    p_timeBias = timeBias;
  }

  double SpicePosition::GetTimeBias() const {
    return (p_timeBias);
  }

  void SpicePosition::SetAberrationCorrection(const QString &correction) {
    QString abcorr(correction);
    abcorr.remove(QChar(' '));
    abcorr = abcorr.toUpper();
    QStringList validAbcorr;
    validAbcorr << "NONE" << "LT" << "LT+S" << "CN" << "CN+S"
                << "XLT" << "XLT+S" << "XCN" << "XCN+S";

    if (validAbcorr.indexOf(abcorr) >= 0) {
      p_aberrationCorrection = abcorr;
    }
    else {
      QString msg = "Invalid abberation correction [" + correction + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
  }

  QString SpicePosition::GetAberrationCorrection() const {
    return (p_aberrationCorrection);
  }

  double SpicePosition::GetLightTime() const {
    return (m_lt);
  }
 
  const std::vector<double> &SpicePosition::SetEphemerisTime(double et) {
    NaifStatus::CheckErrors();

    // Save the time
    if(et == p_et) return p_coordinate;
    p_et = et;

    // Read from the cache
    if(p_source == Memcache) {
      SetEphemerisTimeMemcache();
    }
    else if(p_source == HermiteCache) {
      SetEphemerisTimeHermiteCache();
    }
    else if(p_source == PolyFunction) {
      SetEphemerisTimePolyFunction();
    }
    else if(p_source == PolyFunctionOverHermiteConstant) {
      SetEphemerisTimePolyFunctionOverHermiteConstant();
    }
    else {  // Read from the kernel
      SetEphemerisTimeSpice();
    }

    NaifStatus::CheckErrors();

    // Return the coordinate
    return p_coordinate;
  }


  void SpicePosition::LoadCache(double startTime, double endTime, int size) {
    // Make sure cache isn't already loaded
    if(p_source == Memcache || p_source == HermiteCache) {
      QString msg = "A SpicePosition cache has already been created";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if(startTime > endTime) {
      QString msg = "Argument startTime must be less than or equal to endTime";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    if((startTime != endTime) && (size == 1)) {
      QString msg = "Cache size must be more than 1 if startTime endTime differ";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // Save full cache parameters
    p_fullCacheStartTime = startTime;
    p_fullCacheEndTime = endTime;
    p_fullCacheSize = size;
    LoadTimeCache();

    // Loop and load the cache
    for(int i = 0; i < size; i++) {
      double et = p_cacheTime[i];
      SetEphemerisTime(et);
      p_cache.push_back(p_coordinate);
      if(p_hasVelocity) p_cacheVelocity.push_back(p_velocity);
    }

    p_source = Memcache;
  }


  void SpicePosition::LoadCache(double time) {
    LoadCache(time, time, 1);
  }


  void SpicePosition::LoadCache(Table &table) {

    // Make sure cache isn't alread loaded
    if(p_source == Memcache || p_source == HermiteCache) {
      QString msg = "A SpicePosition cache has already been created";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // Load the full cache time information from the label if available
    if(table.Label().hasKeyword("SpkTableStartTime")) {
      p_fullCacheStartTime = toDouble(table.Label().findKeyword("SpkTableStartTime")[0]);
    }
    if(table.Label().hasKeyword("SpkTableEndTime")) {
      p_fullCacheEndTime = toDouble(table.Label().findKeyword("SpkTableEndTime")[0]);
    }
    if(table.Label().hasKeyword("SpkTableOriginalSize")) {
      p_fullCacheSize = toDouble(table.Label().findKeyword("SpkTableOriginalSize")[0]);
    }


    // set source type by table's label keyword
    if(!table.Label().hasKeyword("CacheType")) {
      p_source = Memcache;
    }
    else if(table.Label().findKeyword("CacheType")[0] == "Linear") {
      p_source = Memcache;
    }
    else if(table.Label().findKeyword("CacheType")[0] == "HermiteSpline") {
      p_source = HermiteCache;
      p_overrideTimeScale = 1.;
      p_override = ScaleOnly;
    }
    else if(table.Label().findKeyword("CacheType")[0] == "PolyFunction") {
      p_source = PolyFunction;
    }
    else {
      throw IException(IException::Io,
                       "Invalid value for CacheType keyword in the table "
                       + table.Name(),
                       _FILEINFO_);
    }

    // Loop through and move the table to the cache
    if (p_source != PolyFunction) {
      for (int r = 0; r < table.Records(); r++) {
        TableRecord &rec = table[r];
        if (rec.Fields() == 7) {
          p_hasVelocity = true;
        }
        else if (rec.Fields() == 4) {
          p_hasVelocity = false;
        }
        else  {
          QString msg = "Expecting four or seven fields in the SpicePosition table";
          throw IException(IException::Programmer, msg, _FILEINFO_);
        }

        std::vector<double> j2000Coord;
        j2000Coord.push_back((double)rec[0]);
        j2000Coord.push_back((double)rec[1]);
        j2000Coord.push_back((double)rec[2]);
        int inext = 3;

        p_cache.push_back(j2000Coord);
        if (p_hasVelocity) {
          std::vector<double> j2000Velocity;
          j2000Velocity.push_back((double)rec[3]);
          j2000Velocity.push_back((double)rec[4]);
          j2000Velocity.push_back((double)rec[5]);
          inext = 6;

          p_cacheVelocity.push_back(j2000Velocity);
        }
        p_cacheTime.push_back((double)rec[inext]);
      }
    }
    else {
      // Coefficient table for postion coordinates x, y, and z
      std::vector<double> coeffX, coeffY, coeffZ;

      for (int r = 0; r < table.Records() - 1; r++) {
        TableRecord &rec = table[r];

        if(rec.Fields() != 3) {
          // throw an error
        }
        coeffX.push_back((double)rec[0]);
        coeffY.push_back((double)rec[1]);
        coeffZ.push_back((double)rec[2]);
      }
      // Take care of function time parameters
      TableRecord &rec = table[table.Records()-1];
      double baseTime = (double)rec[0];
      double timeScale = (double)rec[1];
      double degree = (double)rec[2];
      SetPolynomialDegree((int) degree);
      SetOverrideBaseTime(baseTime, timeScale);
      SetPolynomial(coeffX, coeffY, coeffZ);
      if (degree > 0)  p_hasVelocity = true;
      if(degree == 0  && p_cacheVelocity.size() > 0) p_hasVelocity = true;
    }
  }


  Table SpicePosition::Cache(const QString &tableName) {
    if (p_source == PolyFunctionOverHermiteConstant) {
      LineCache(tableName);
      // TODO Figure out how to get the tolerance -- for now hard code .01
      Memcache2HermiteCache(0.01);

      //std::cout << "Cache size is " << p_cache.size();

    }

    // record to be added to table
    TableRecord record;

    if (p_source != PolyFunction) {
    // add x,y,z position labels to record
      TableField x("J2000X", TableField::Double);
      TableField y("J2000Y", TableField::Double);
      TableField z("J2000Z", TableField::Double);
      record += x;
      record += y;
      record += z;

      if (p_hasVelocity) {
      // add x,y,z velocity labels to record
        TableField vx("J2000XV", TableField::Double);
        TableField vy("J2000YV", TableField::Double);
        TableField vz("J2000ZV", TableField::Double);
        record += vx;
        record += vy;
        record += vz;
      }
    // add time label to record
      TableField t("ET", TableField::Double);
      record += t;

    // create output table
      Table table(tableName, record);

      int inext = 0;

      for (int i = 0; i < (int)p_cache.size(); i++) {
        record[inext++] = p_cache[i][0];                     // record[0]
        record[inext++] = p_cache[i][1];                     // record[1]
        record[inext++] = p_cache[i][2];                     // record[2]
        if (p_hasVelocity) {
          record[inext++] = p_cacheVelocity[i][0];           // record[3]
          record[inext++] = p_cacheVelocity[i][1];           // record[4]
          record[inext++] = p_cacheVelocity[i][2];           // record[5]
        }
        record[inext] = p_cacheTime[i];                      // record[6]
        table += record;

        inext = 0;
      }
      CacheLabel(table);
      return table;
    }

    else if(p_source == PolyFunction  &&  p_degree == 0  &&  p_fullCacheSize == 1)
      // Just load the position for the single epoch
      return LineCache(tableName);

    // Load the coefficients for the curves fit to the 3 camera angles
    else if (p_source == PolyFunction) {
      // PolyFunction case
      TableField spacecraftX("J2000SVX", TableField::Double);
      TableField spacecraftY("J2000SVY", TableField::Double);
      TableField spacecraftZ("J2000SVZ", TableField::Double);

      TableRecord record;
      record += spacecraftX;
      record += spacecraftY;
      record += spacecraftZ;

      Table table(tableName, record);

      for(int cindex = 0; cindex < p_degree + 1; cindex++) {
        record[0] = p_coefficients[0][cindex];
        record[1] = p_coefficients[1][cindex];
        record[2] = p_coefficients[2][cindex];
        table += record;
      }

      // Load one more table entry with the time adjustments for the fit equation
      // t = (et - baseTime)/ timeScale
      record[0] = p_baseTime;
      record[1] = p_timeScale;
      record[2] = (double) p_degree;

      CacheLabel(table);
      table += record;
      return table;
    }

    else {
      throw IException(IException::Io,
                       "Cannot create Table, no Cache is loaded.",
                       _FILEINFO_);
    }

  }



  void SpicePosition::CacheLabel(Table &table) {
    // determine type of table to return
    QString tabletype = "";

    if(p_source == Memcache) {
      tabletype = "Linear";
    }
    else if(p_source == HermiteCache) {
      tabletype = "HermiteSpline";
    }
    else {
      tabletype = "PolyFunction";
    }

    table.Label() += PvlKeyword("CacheType", tabletype);

    // Write original time coverage
    if(p_fullCacheStartTime != 0) {
      table.Label() += PvlKeyword("SpkTableStartTime");
      table.Label()["SpkTableStartTime"].addValue(toString(p_fullCacheStartTime));
    }
    if(p_fullCacheEndTime != 0) {
      table.Label() += PvlKeyword("SpkTableEndTime");
      table.Label()["SpkTableEndTime"].addValue(toString(p_fullCacheEndTime));
    }
    if(p_fullCacheSize != 0) {
      table.Label() += PvlKeyword("SpkTableOriginalSize");
      table.Label()["SpkTableOriginalSize"].addValue(toString(p_fullCacheSize));
    }
  }



  Table SpicePosition::LineCache(const QString &tableName) {

    // Apply the function and fill the caches
    if(p_source >= HermiteCache)  ReloadCache();

    if(p_source != Memcache) {
      QString msg = "Only cached positions can be returned as a line cache of positions and time";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    // Load the table and return it to caller
    return Cache(tableName);
  }



  void SpicePosition::ReloadCache() {
    NaifStatus::CheckErrors();

    // Save current et
    double et = p_et;

    // Make sure source is a function
    if(p_source < HermiteCache) {
      QString msg = "The SpicePosition has not yet been fit to a function";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // Clear existing positions from thecache
    p_cacheTime.clear();
    p_cache.clear();

    // Clear the velocity cache if we can calculate it instead.  It can't be calculated for
    // functions of degree 0 (framing cameras), so keep the original velocity.  It is better than nothing.
    if (p_degree > 0  && p_cacheVelocity.size() > 1)  p_cacheVelocity.clear();

    // Load the time cache first
    LoadTimeCache();

    if (p_fullCacheSize > 1) {
    // Load the positions and velocity caches
    p_et = -DBL_MAX;   // Forces recalculation in SetEphemerisTime

      for (std::vector<double>::size_type pos = 0; pos < p_cacheTime.size(); pos++) {
        //        p_et = p_cacheTime.at(pos);
        SetEphemerisTime(p_cacheTime.at(pos));
        p_cache.push_back(p_coordinate);
        p_cacheVelocity.push_back(p_velocity);
      }
    }
    else {
    // Load the position for the single updated time instance
      p_et = p_cacheTime[0];
      SetEphemerisTime(p_et);
      p_cache.push_back(p_coordinate);
    }

    // Set source to cache and reset current et
    p_source = Memcache;
    p_et = -DBL_MAX;
    SetEphemerisTime(et);

    NaifStatus::CheckErrors();
  }


   Table SpicePosition::LoadHermiteCache(const QString &tableName) {
    // find the first and last time values
    double firstTime = p_fullCacheStartTime;
    double lastTime = p_fullCacheEndTime;
    int cacheTimeSize = (int) p_fullCacheSize;

    // Framing cameras are already cached and don't need to be reduced.
    if(cacheTimeSize == 1) return Cache(tableName);

    //If it's already a hermite cache, just return it.
    if (p_source == HermiteCache) {
      return Cache(tableName); 
    }

    // Make sure a polynomial function is already loaded
    if(p_source != PolyFunction) {
      QString msg = "A SpicePosition polynomial function has not been created yet";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // Load the polynomial functions
    Isis::PolynomialUnivariate function1(p_degree);
    Isis::PolynomialUnivariate function2(p_degree);
    Isis::PolynomialUnivariate function3(p_degree);
    function1.SetCoefficients(p_coefficients[0]);
    function2.SetCoefficients(p_coefficients[1]);
    function3.SetCoefficients(p_coefficients[2]);

    // Clear existing coordinates from cache
    ClearCache();

    // Set velocity vector to true since it is calculated
    p_hasVelocity = true;

//    std::cout <<"time cache size is " << p_cacheTime.size() << std::endl;

    // Calculate new postition coordinates from polynomials fit to coordinates &
    // fill cache
//    std::cout << "Before" << std::endl;
//    double savetime = p_cacheTime.at(0);
//    SetEphemerisTime(savetime);
//    std::cout << "     at time " << savetime << std::endl;
//    for (int i=0; i<3; i++) {
//      std::cout << p_coordinate[i] << std::endl;
//    }


    // find time for the extremum of each polynomial
    // since this is only a 2nd degree polynomial,
    // finding these extrema is simple
    double b1 = function1.Coefficient(1);
    double c1 = function1.Coefficient(2);
    double extremumXtime = -b1 / (2.*c1) + p_baseTime; // extremum is time value for root of 1st derivative
    // make sure we are in the domain
    if(extremumXtime < firstTime) {
      extremumXtime = firstTime;
    }
    if(extremumXtime > lastTime) {
      extremumXtime = lastTime;
    }

    double b2 = function2.Coefficient(1);
    double c2 = function2.Coefficient(2);
    double extremumYtime = -b2 / (2.*c2) + p_baseTime;
    // make sure we are in the domain
    if(extremumYtime < firstTime) {
      extremumYtime = firstTime;
    }
    if(extremumYtime > lastTime) {
      extremumYtime = lastTime;
    }

    double b3 = function3.Coefficient(1);
    double c3 = function3.Coefficient(2);
    double extremumZtime = -b3 / (2.*c3) + p_baseTime;
    // make sure we are in the domain
    if(extremumZtime < firstTime) {
      extremumZtime = firstTime;
    }
    if(extremumZtime > lastTime) {
      extremumZtime = lastTime;
    }

    // refill the time vector
    p_cacheTime.clear();
    p_cacheTime.push_back(firstTime);
    p_cacheTime.push_back(extremumXtime);
    p_cacheTime.push_back(extremumYtime);
    p_cacheTime.push_back(extremumZtime);
    p_cacheTime.push_back(lastTime);
    // we don't know order of extrema, so sort
    sort(p_cacheTime.begin(), p_cacheTime.end());
    // in case an extremum is an endpoint
    std::vector <double>::iterator it = unique(p_cacheTime.begin(), p_cacheTime.end());
    p_cacheTime.resize(it - p_cacheTime.begin());

    if(p_cacheTime.size() == 2) {
      p_cacheTime.clear();
      p_cacheTime.push_back(firstTime);
      p_cacheTime.push_back((firstTime + lastTime) / 2);
      p_cacheTime.push_back(lastTime);
    }

    // add positions and velocities for these times
    for(int i = 0; i < (int) p_cacheTime.size(); i++) {
      // x,y,z positions
      double time;
      time = p_cacheTime[i] - p_baseTime;
      p_coordinate[0] = function1.Evaluate(time);
      p_coordinate[1] = function2.Evaluate(time);
      p_coordinate[2] = function3.Evaluate(time);
      p_cache.push_back(p_coordinate);

      // x,y,z velocities
      p_velocity[0] = b1 + 2 * c1 * (p_cacheTime[i] - p_baseTime);
      p_velocity[1] = b2 + 2 * c2 * (p_cacheTime[i] - p_baseTime);
      p_velocity[2] = b3 + 2 * c3 * (p_cacheTime[i] - p_baseTime);
      p_cacheVelocity.push_back(p_velocity);
    }

    p_source = HermiteCache;
    double et = p_et;
    p_et = -DBL_MAX;
    SetEphemerisTime(et);

    return Cache(tableName);
  }


  void SpicePosition::SetPolynomial(Source type) {
    std::vector<double> XC, YC, ZC;

    // Check to see if the position is already a Polynomial Function
    if (p_source == PolyFunction)
      return;

    // Adjust the degree of the polynomial to the available data
    if (p_cache.size() == 1) {
      p_degree = 0;
    }
    else if (p_cache.size() == 2) {
      p_degree = 1;
    }

    //Check for polynomial over Hermite constant and initialize coefficients
    if (type == PolyFunctionOverHermiteConstant) {
      XC.assign(p_degree + 1, 0.);
      YC.assign(p_degree + 1, 0.);
      ZC.assign(p_degree + 1, 0.);
      SetPolynomial(XC, YC, ZC, type);
      return;
    }

    Isis::PolynomialUnivariate function1(p_degree);       
    Isis::PolynomialUnivariate function2(p_degree);       
    Isis::PolynomialUnivariate function3(p_degree);       

    // Compute the base time
    ComputeBaseTime();
    std::vector<double> time;

    if(p_cache.size() == 1) {
      double t = p_cacheTime.at(0);
      SetEphemerisTime(t);
      XC.push_back(p_coordinate[0]);
      YC.push_back(p_coordinate[1]);
      ZC.push_back(p_coordinate[2]);
    }
    else if(p_cache.size() == 2) {
// Load the times and get the corresponding coordinates
      double t1 = p_cacheTime.at(0);
      SetEphemerisTime(t1);
      std::vector<double> coord1 = p_coordinate;
      t1 = (t1 - p_baseTime) / p_timeScale;
      double t2 = p_cacheTime.at(1);
      SetEphemerisTime(t2);
      std::vector<double> coord2 = p_coordinate;
      t2 = (t2 - p_baseTime) / p_timeScale;
      double slope[3];
      double intercept[3];

// Compute the linear equation for each coordinate and save them
      for(int cIndex = 0; cIndex < 3; cIndex++) {
        Isis::LineEquation posline(t1, coord1[cIndex], t2, coord2[cIndex]);
        slope[cIndex] = posline.Slope();
        intercept[cIndex] = posline.Intercept();
      }
      XC.push_back(intercept[0]);
      XC.push_back(slope[0]);
      YC.push_back(intercept[1]);
      YC.push_back(slope[1]);
      ZC.push_back(intercept[2]);
      ZC.push_back(slope[2]);
    }
    else {
      LeastSquares *fitX = new LeastSquares(function1);
      LeastSquares *fitY = new LeastSquares(function2);
      LeastSquares *fitZ = new LeastSquares(function3);

      // Load the known values to compute the fit equation
      for(std::vector<double>::size_type pos = 0; pos < p_cacheTime.size(); pos++) {
        double t = p_cacheTime.at(pos);
        time.push_back( (t - p_baseTime) / p_timeScale);
        SetEphemerisTime(t);
        std::vector<double> coord = p_coordinate;

        fitX->AddKnown(time, coord[0]);
        fitY->AddKnown(time, coord[1]);
        fitZ->AddKnown(time, coord[2]);
        time.clear();
      }
      //Solve the equations for the coefficients
      fitX->Solve();
      fitY->Solve();
      fitZ->Solve();

      // Delete the least squares objects now that we have all the coefficients
      delete fitX;
      delete fitY;
      delete fitZ;

      // For now assume all three coordinates are fit to a polynomial function.
      // Later they may each be fit to a unique basis function.
      // Fill the coefficient vectors

      for(int i = 0;  i < function1.Coefficients(); i++) {
        XC.push_back(function1.Coefficient(i));
        YC.push_back(function2.Coefficient(i));
        ZC.push_back(function3.Coefficient(i));
      }

    }

    // Now that the coefficients have been calculated set the polynomial with them
    SetPolynomial(XC, YC, ZC);
    return;
  }



  void SpicePosition::SetPolynomial(const std::vector<double>& XC,
                                    const std::vector<double>& YC,
                                    const std::vector<double>& ZC,
                                    const Source type) {

    Isis::PolynomialUnivariate function1(p_degree);
    Isis::PolynomialUnivariate function2(p_degree);
    Isis::PolynomialUnivariate function3(p_degree);

    // Load the functions with the coefficients
    function1.SetCoefficients(XC);
    function2.SetCoefficients(YC);
    function3.SetCoefficients(ZC);

    // Compute the base time
    ComputeBaseTime();

    // Save the current coefficients
    p_coefficients[0] = XC;
    p_coefficients[1] = YC;
    p_coefficients[2] = ZC;

    // Set the flag indicating p_degree has been applied to the spacecraft
    // positions and the coefficients of the polynomials have been saved.
    p_degreeApplied = true;

    // Reset the interpolation source
    p_source = type;

    // Update the current position
    double et = p_et;
    p_et = -DBL_MAX;
    SetEphemerisTime(et);

    return;
  }



  void SpicePosition::GetPolynomial(std::vector<double>& XC,
                                    std::vector<double>& YC,
                                    std::vector<double>& ZC) {
    XC = p_coefficients[0];
    YC = p_coefficients[1];
    ZC = p_coefficients[2];

    return;
  }



  void SpicePosition::ComputeBaseTime() {
    if(p_override == NoOverrides) {
      p_baseTime = (p_cacheTime.at(0) + p_cacheTime.at(p_cacheTime.size() - 1)) / 2.;
      p_timeScale = p_baseTime - p_cacheTime.at(0);
    }
    else if (p_override == ScaleOnly) {
      p_baseTime = (p_cacheTime.at(0) + p_cacheTime.at(p_cacheTime.size() - 1)) / 2.;
      p_timeScale = p_overrideTimeScale;
    }
    else {
      p_baseTime = p_overrideBaseTime;
      p_timeScale = p_overrideTimeScale;
    }

    // Take care of case where 1st and last times are the same
    if(p_timeScale == 0)  p_timeScale = 1.0;
    return;
  }


  void SpicePosition::SetOverrideBaseTime(double baseTime, double timeScale) {
    p_overrideBaseTime = baseTime;
    p_overrideTimeScale = timeScale;
    p_override = BaseAndScale;
    return;
  }



  std::vector<double> SpicePosition::CoordinatePartial(
         SpicePosition::PartialType partialVar, int coeffIndex) {
    // Start with a zero vector since the derivative of the other coordinates
    // with respect to the partial var will be 0.
    std::vector<double> coordinate(3, 0);

    // Get the index of the coordinate to update with the partial derivative
    int coordIndex = partialVar;

//  if(coeffIndex > 2) {
//    QString msg = "SpicePosition only supports up to a 2nd order fit for the spacecraft position";
//    throw IException(IException::Programmer, msg, _FILEINFO_);
//  }
//
    // Reset the coordinate to its derivative
    coordinate[coordIndex] = DPolynomial(coeffIndex);
    return coordinate;
  }



  std::vector<double> SpicePosition::VelocityPartial(
                    SpicePosition::PartialType partialVar, int coeffIndex) {
    // Start with a zero vector since the derivative of the other coordinates with
    // respect to the partial var will be 0.
    std::vector<double> dvelocity(3, 0);

    // Get the index of the coordinate to update with the partial derivative
    int coordIndex = partialVar;

    double time = (p_et - p_baseTime) / p_timeScale;
    double derivative = 0.;

    // Handle arithmetic failures
    double Epsilon = 1.0e-15;
    if (fabs(time) <= Epsilon) time = 0.0;

    // Only compute for coefficient index > 0 to avoid problems like 0 ** -1
    if (coeffIndex   > 0) 
      derivative = coeffIndex * pow(time, coeffIndex-1) / p_timeScale;

    // Reset the velocity coordinate to its derivative
    dvelocity[coordIndex] = derivative;
    return dvelocity;
  }


  double SpicePosition::DPolynomial(const int coeffIndex) {
    double derivative = 1.;
    double time = (p_et - p_baseTime) / p_timeScale;

    if(coeffIndex > 0  && coeffIndex <= p_degree) {
      derivative = pow(time, coeffIndex);
    }
    else if(coeffIndex == 0) {
      derivative = 1;
    }
    else {
      QString msg = "Unable to evaluate the derivative of the SPICE position fit polynomial for "
                    "the given coefficient index [" + toString(coeffIndex) + "]. "
                    "Index is negative or exceeds degree of polynomial ["
                    + toString(p_degree) + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    return derivative;
  }

  const std::vector<double> &SpicePosition::Velocity() {
    if(p_hasVelocity) {
      return p_velocity;
    }
    else {
      QString msg = "No velocity vector available";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
  }



  void SpicePosition::SetEphemerisTimeMemcache() {
    // If the cache has only one position return it
    if(p_cache.size() == 1) {
      p_coordinate[0] = p_cache[0][0];
      p_coordinate[1] = p_cache[0][1];
      p_coordinate[2] = p_cache[0][2];

      if(p_hasVelocity) {
        p_velocity[0] = p_cacheVelocity[0][0];
        p_velocity[1] = p_cacheVelocity[0][1];
        p_velocity[2] = p_cacheVelocity[0][2];
      }
    }

    else {
      // Otherwise determine the interval to interpolate
      std::vector<double>::iterator pos;
      pos = upper_bound(p_cacheTime.begin(), p_cacheTime.end(), p_et);

      int cacheIndex;
      if(pos != p_cacheTime.end()) {
        cacheIndex = distance(p_cacheTime.begin(), pos);
        cacheIndex--;
      }
      else {
        cacheIndex = p_cacheTime.size() - 2;
      }

      if(cacheIndex < 0) cacheIndex = 0;

      // Interpolate the coordinate
      double mult = (p_et - p_cacheTime[cacheIndex]) /
                    (p_cacheTime[cacheIndex+1] - p_cacheTime[cacheIndex]);
      std::vector<double> p2 = p_cache[cacheIndex+1];
      std::vector<double> p1 = p_cache[cacheIndex];

      p_coordinate[0] = (p2[0] - p1[0]) * mult + p1[0];
      p_coordinate[1] = (p2[1] - p1[1]) * mult + p1[1];
      p_coordinate[2] = (p2[2] - p1[2]) * mult + p1[2];

      if(p_hasVelocity) {
        p2 = p_cacheVelocity[cacheIndex+1];
        p1 = p_cacheVelocity[cacheIndex];
        p_velocity[0] = (p2[0] - p1[0]) * mult + p1[0];
        p_velocity[1] = (p2[1] - p1[1]) * mult + p1[1];
        p_velocity[2] = (p2[2] - p1[2]) * mult + p1[2];
      }
    }
  }



  void SpicePosition::SetEphemerisTimeHermiteCache() {

    // what if framing camera???

    // On the first SetEphemerisTime, create our splines. Later calls should
    // reuse these splines.
    if(p_xhermite == NULL) {
      p_overrideTimeScale = 1.;
      p_override = ScaleOnly;
      ComputeBaseTime();

      p_xhermite = new NumericalApproximation(
          NumericalApproximation::CubicHermite);
      p_yhermite = new NumericalApproximation(
          NumericalApproximation::CubicHermite);
      p_zhermite = new NumericalApproximation(
          NumericalApproximation::CubicHermite);

      vector<double> xvalues(p_cache.size());
      vector<double> xhermiteYValues(p_cache.size());
      vector<double> yhermiteYValues(p_cache.size());
      vector<double> zhermiteYValues(p_cache.size());

      vector<double> xhermiteVelocities(p_cache.size());
      vector<double> yhermiteVelocities(p_cache.size());
      vector<double> zhermiteVelocities(p_cache.size());

      for(unsigned int i = 0; i < p_cache.size(); i++) {
        vector<double> &cache = p_cache[i];
        double &cacheTime = p_cacheTime[i];
        xvalues[i] = (cacheTime - p_baseTime) / p_timeScale;

        xhermiteYValues[i] = cache[0];
        yhermiteYValues[i] = cache[1];
        zhermiteYValues[i] = cache[2];

        if(p_hasVelocity) {  // Line scan camera
          vector<double> &cacheVelocity = p_cacheVelocity[i];
          xhermiteVelocities[i] = cacheVelocity[0];
          yhermiteVelocities[i] = cacheVelocity[1];
          zhermiteVelocities[i] = cacheVelocity[2];
        }
        else { // Not line scan camera
          throw IException(IException::Io, "No velocities available.",
                           _FILEINFO_);
        }
      }

      p_xhermite->AddData(xvalues, xhermiteYValues);
      p_xhermite->AddCubicHermiteDeriv(xhermiteVelocities);

      p_yhermite->AddData(xvalues, yhermiteYValues);
      p_yhermite->AddCubicHermiteDeriv(yhermiteVelocities);

      p_zhermite->AddData(xvalues, zhermiteYValues);
      p_zhermite->AddCubicHermiteDeriv(zhermiteVelocities);
    }

    // Next line added 07/13/2009 to prevent Camera unit test from bombing
    // because time is outside domain. DAC Also added etype to Evaluate calls
    NumericalApproximation::ExtrapType etype =
        NumericalApproximation::Extrapolate;

    vector<double> &coordinate = p_coordinate;
    double sTime = (p_et - p_baseTime) / p_timeScale;
    coordinate[0] = p_xhermite->Evaluate(sTime, etype);
    coordinate[1] = p_yhermite->Evaluate(sTime, etype);
    coordinate[2] = p_zhermite->Evaluate(sTime, etype);

    vector<double> &velocity = p_velocity;
    velocity[0] = p_xhermite->EvaluateCubicHermiteFirstDeriv(sTime);
    velocity[1] = p_yhermite->EvaluateCubicHermiteFirstDeriv(sTime);
    velocity[2] = p_zhermite->EvaluateCubicHermiteFirstDeriv(sTime);
  }



  void SpicePosition::SetEphemerisTimePolyFunction() {
    // Create the empty functions
    Isis::PolynomialUnivariate functionX(p_degree);
    Isis::PolynomialUnivariate functionY(p_degree);
    Isis::PolynomialUnivariate functionZ(p_degree);

    // Load the coefficients to define the functions
    functionX.SetCoefficients(p_coefficients[0]);
    functionY.SetCoefficients(p_coefficients[1]);
    functionZ.SetCoefficients(p_coefficients[2]);

    // Normalize the time
    double rtime;
    rtime = (p_et - p_baseTime) / p_timeScale;

    // Evaluate the polynomials at current et to get position;
    p_coordinate[0] = functionX.Evaluate(rtime);
    p_coordinate[1] = functionY.Evaluate(rtime);
    p_coordinate[2] = functionZ.Evaluate(rtime);

    if(p_hasVelocity) {

      if( p_degree == 0) {
        p_velocity = p_cacheVelocity[0];
      }
      else { 
        p_velocity[0] = ComputeVelocityInTime(WRT_X);
        p_velocity[1] = ComputeVelocityInTime(WRT_Y);
        p_velocity[2] = ComputeVelocityInTime(WRT_Z);
      }
        
//         p_velocity[0] = functionX.DerivativeVar(rtime);
//         p_velocity[1] = functionY.DerivativeVar(rtime);
//         p_velocity[2] = functionZ.DerivativeVar(rtime);
    }
  }


  void SpicePosition::SetEphemerisTimePolyFunctionOverHermiteConstant() {
    SetEphemerisTimeHermiteCache();
    std::vector<double> hermiteCoordinate = p_coordinate;

//    std::cout << hermiteCoordinate << std::endl;

    std::vector<double> hermiteVelocity = p_velocity;
    SetEphemerisTimePolyFunction();

    for (int index = 0; index < 3; index++) {
      p_coordinate[index] += hermiteCoordinate[index];
      p_velocity[index] += hermiteVelocity[index];
    }
  }


  void SpicePosition::SetEphemerisTimeSpice() {

    double state[6];
    bool hasVelocity;
    double lt;

    // NOTE:  Prefer method getter access as some are virtualized and portray
    // the appropriate internal representation!!!
    computeStateVector(getAdjustedEphemerisTime(), getTargetCode(), getObserverCode(),
                       "J2000", GetAberrationCorrection(), state, hasVelocity,
                       lt);

    // Set the internal state
    setStateVector(state, hasVelocity);
    setLightTime(lt);
    return;
  }


  void SpicePosition::Memcache2HermiteCache(double tolerance) {
    if(p_source == HermiteCache) {
      return;
    }
    else if(p_source != Memcache) {
      throw IException(IException::Programmer,
                       "Source type is not Memcache, cannot convert.",
                       _FILEINFO_);
    }

    // make sure base time is set before it is needed
    p_overrideTimeScale = 1.;
    p_override = ScaleOnly;
    ComputeBaseTime();

    // find current size of cache
    int n = p_cacheTime.size() - 1;

    // create 3 starting values for the new table
    vector <int> inputIndices;
    inputIndices.push_back(0);
    inputIndices.push_back(n / 2);
    inputIndices.push_back(n);

    // find all indices needed to make a hermite table within the appropriate tolerance
    vector <int> indexList = HermiteIndices(tolerance, inputIndices);

    // remove all lines from cache vectors that are not in the index list????
    for(int i = n; i >= 0; i--) {
      if(!binary_search(indexList.begin(), indexList.end(), i)) {
        p_cache.erase(p_cache.begin() + i);
        p_cacheTime.erase(p_cacheTime.begin() + i);
        p_cacheVelocity.erase(p_cacheVelocity.begin() + i);
      }
    }

    p_source = HermiteCache;
  }

  std::vector<int> SpicePosition::HermiteIndices(double tolerance, std::vector <int> indexList) {

    unsigned int n = indexList.size();
    double sTime;

    NumericalApproximation xhermite(NumericalApproximation::CubicHermite);
    NumericalApproximation yhermite(NumericalApproximation::CubicHermite);
    NumericalApproximation zhermite(NumericalApproximation::CubicHermite);
    for(unsigned int i = 0; i < indexList.size(); i++) {
      sTime = (p_cacheTime[indexList[i]] - p_baseTime) / p_timeScale;
      xhermite.AddData(sTime, p_cache[indexList[i]][0]);  // add time, x-position to x spline
      yhermite.AddData(sTime, p_cache[indexList[i]][1]);  // add time, y-position to y spline
      zhermite.AddData(sTime, p_cache[indexList[i]][2]);  // add time, z-position to z spline

      if(p_hasVelocity) {  // Line scan camera
        xhermite.AddCubicHermiteDeriv(p_cacheVelocity[i][0]); // add x-velocity to x spline
        yhermite.AddCubicHermiteDeriv(p_cacheVelocity[i][1]); // add y-velocity to y spline
        zhermite.AddCubicHermiteDeriv(p_cacheVelocity[i][2]); // add z-velocity to z spline
      }
      else { // Not line scan camera
        throw IException(IException::Io, "No velocities available.", _FILEINFO_);
        // xhermite.AddCubicHermiteDeriv(0.0); // spacecraft didn't move => velocity = 0
        // yhermite.AddCubicHermiteDeriv(0.0); // spacecraft didn't move => velocity = 0
        // zhermite.AddCubicHermiteDeriv(0.0); // spacecraft didn't move => velocity = 0
      }
    }

    // loop through the saved indices from the end
    for(unsigned int i = indexList.size() - 1; i > 0; i--) {
      double xerror = 0;
      double yerror = 0;
      double zerror = 0;

      // check every value of the original kernel values within interval
      for(int line = indexList[i-1] + 1; line < indexList[i]; line++) {
        sTime = (p_cacheTime[line] - p_baseTime) / p_timeScale;

        // find the errors at each value
        xerror = fabs(xhermite.Evaluate(sTime) - p_cache[line][0]);
        yerror = fabs(yhermite.Evaluate(sTime) - p_cache[line][1]);
        zerror = fabs(zhermite.Evaluate(sTime) - p_cache[line][2]);

        if(xerror > tolerance || yerror > tolerance || zerror > tolerance) {
          // if any error is greater than tolerance, no need to continue looking, break
          break;
        }
      }

      if(xerror < tolerance && yerror < tolerance && zerror < tolerance) {
        // if errors are less than tolerance after looping interval, no new point is necessary
        continue;
      }
      else {
        // if any error is greater than tolerance, add midpoint of interval to indexList vector
        indexList.push_back((indexList[i] + indexList[i-1]) / 2);
      }
    }

    if(indexList.size() > n) {
      sort(indexList.begin(), indexList.end());
      indexList = HermiteIndices(tolerance, indexList);
    }
    return indexList;
  }


  void SpicePosition::ClearCache() {
    p_cache.clear();
    p_cacheVelocity.clear();
    p_cacheTime.clear();

    if(p_xhermite) {
      delete p_xhermite;
      p_xhermite = NULL;
    }

    if(p_yhermite) {
      delete p_xhermite;
      p_xhermite = NULL;
    }

    if(p_zhermite) {
      delete p_xhermite;
      p_xhermite = NULL;
    }
  }



  void SpicePosition::SetPolynomialDegree(int degree) {
    // Adjust the degree for the data
    if(p_fullCacheSize == 1) {
      degree = 0;
    }
    else if(p_fullCacheSize == 2) {
      degree = 1;
    }
    // If polynomials have not been applied yet simply set the degree and return
    if(!p_degreeApplied) {
      p_degree = degree;
    }

    // Otherwise the existing polynomials need to be either expanded ...
    else if(p_degree < degree) {   // (increase the number of terms)
      std::vector<double> coefX(p_coefficients[0]),
          coefY(p_coefficients[1]),
          coefZ(p_coefficients[2]);

      for(int icoef = p_degree + 1;  icoef <= degree; icoef++) {
        coefX.push_back(0.);
        coefY.push_back(0.);
        coefZ.push_back(0.);
      }
      p_degree = degree;
      SetPolynomial(coefX, coefY, coefZ);
    }
    // ... or reduced (decrease the number of terms)
    else if(p_degree > degree) {
      std::vector<double> coefX(degree + 1),
          coefY(degree + 1),
          coefZ(degree + 1);

      for(int icoef = 0;  icoef <= degree;  icoef++) {
        coefX.push_back(p_coefficients[0][icoef]);
        coefY.push_back(p_coefficients[1][icoef]);
        coefZ.push_back(p_coefficients[2][icoef]);
      }
      p_degree = degree;
      SetPolynomial(coefX, coefY, coefZ);
    }
  }



  void SpicePosition::ReloadCache(Table &table) {
    p_source = Spice;
    ClearCache();
    LoadCache(table);
  }

  void SpicePosition::LoadTimeCache() {
    // Loop and load the time cache
    double cacheSlope = 0.0;

    if(p_fullCacheSize > 1)
      cacheSlope = (p_fullCacheEndTime - p_fullCacheStartTime) /
         (double)(p_fullCacheSize - 1);

    for(int i = 0; i < p_fullCacheSize; i++) {
      double et = p_fullCacheStartTime + (double) i * cacheSlope;
      p_cacheTime.push_back(et);
    }
  }

  const std::vector<double> &SpicePosition::GetCenterCoordinate() {
    // Compute the center time
    double etCenter = (p_fullCacheEndTime + p_fullCacheStartTime) / 2.;
    SetEphemerisTime(etCenter);

    return Coordinate();
  }


  double SpicePosition::ComputeVelocityInTime(PartialType var) {
    double velocity = 0.;

    for (int icoef=1; icoef <= p_degree; icoef++) {
      velocity += icoef*p_coefficients[var][icoef]*pow((p_et - p_baseTime), (icoef - 1))
                  / pow(p_timeScale, icoef);
    }

    return velocity;
  }


   std::vector<double> SpicePosition::Extrapolate(double timeEt) {
     if (!p_hasVelocity) return p_coordinate;

     double diffTime = timeEt - p_et;
     std::vector<double> extrapPos(3, 0.);
     extrapPos[0] = p_coordinate[0] + diffTime*p_velocity[0];
     extrapPos[1] = p_coordinate[1] + diffTime*p_velocity[1];
     extrapPos[2] = p_coordinate[2] + diffTime*p_velocity[2];

     return extrapPos;
   }


  std::vector<double> SpicePosition::HermiteCoordinate() {
    if (p_source != PolyFunctionOverHermiteConstant) {
      throw IException(IException::Programmer, 
        "Hermite coordinates only available for PolyFunctionOverHermiteConstant",
          _FILEINFO_);
    }

    // Save the current coordinate so it can be reset
    std::vector<double> coordinate = p_coordinate;
    SetEphemerisTimeHermiteCache();
    std::vector<double> hermiteCoordinate = p_coordinate;
    p_coordinate = coordinate;
    return hermiteCoordinate;
  }

  //=========================================================================
  //  Observer/Target swap and light time correction methods 
  //=========================================================================

  int SpicePosition::getObserverCode() const {
    return (p_observerCode);
  }

  int SpicePosition::getTargetCode() const {
    return (p_targetCode);
  }


  double SpicePosition::getAdjustedEphemerisTime() const {
    return (EphemerisTime() + GetTimeBias());
  }


  void SpicePosition::computeStateVector(double et, int target, int observer, 
                                         const QString &refFrame, 
                                         const QString &abcorr, 
                                         double state[6], bool &hasVelocity,
                                         double &lightTime) const {

    // First try getting the entire state (including the velocity vector)
    NaifStatus::CheckErrors();
    hasVelocity = true;
    lightTime = 0.0;
    spkez_c((SpiceInt) target, (SpiceDouble) et, refFrame.toLatin1().data(), 
            abcorr.toLatin1().data(), (SpiceInt) observer, state, &lightTime);

    // If Naif fails attempting to get the entire state, assume the velocity vector is
    // not available and just get the position.  First turn off Naif error reporting and
    // return any error without printing them.
    SpiceBoolean spfailure = failed_c();
    reset_c();                   // Reset Naif error system to allow caller to recover
    if ( spfailure ) {
      hasVelocity = false;
      lightTime = 0.0;
      spkezp_c((SpiceInt) target, (SpiceDouble) et, refFrame.toLatin1().data(), 
               abcorr.toLatin1().data(), (SpiceInt) observer, state, &lightTime);
    }
    NaifStatus::CheckErrors();
    return;
  }

  void SpicePosition::setStateVector(const double state[6],
                                     const bool &hasVelocity) {

    p_coordinate[0] = state[0];
    p_coordinate[1] = state[1];
    p_coordinate[2] = state[2];

    if ( hasVelocity ) {
      p_velocity[0] = state[3];
      p_velocity[1] = state[4];
      p_velocity[2] = state[5];
    }
    else {
      p_velocity[0] = 0.0;
      p_velocity[1] = 0.0;
      p_velocity[2] = 0.0;
    }
    p_hasVelocity = hasVelocity;

    // Negate vectors if swap of observer target is requested so interface 
    // remains consistent
    if (m_swapObserverTarget) {
      vminus_c(&p_velocity[0],   &p_velocity[0]);
      vminus_c(&p_coordinate[0], &p_coordinate[0]);
    }

    return;
  }

  void SpicePosition::setLightTime(const double &lightTime) {
    m_lt = lightTime;
    return;
  }

  // /** Resets the source interpolation of the position.
  //  *
  //  * @param [in]   source    The interpolation to use for calculating position
  //  *
  //  */
  //  void SetSource(Source source) {
  //    p_source = source;
  //    return;
  //  }
}