SpicePosition.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
#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"
 
using json = nlohmann::json;
 
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);
 
 
    m_swapObserverTarget = swapObserverTarget;
    m_lt = 0.0;
    m_state = NULL;
 
    // 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
    std::vector<ale::State> stateCache;
    for(int i = 0; i < size; i++) {
      double et = p_cacheTime[i];
      SetEphemerisTime(et);
      ale::State currentState = ale::State(ale::Vec3d(p_coordinate));
      if (p_hasVelocity) {
        currentState.velocity = ale::Vec3d(p_velocity);
      }
      stateCache.push_back(currentState);
    }
 
    if (m_state != NULL) {
      delete m_state;
    }
 
    m_state = new ale::States(p_cacheTime, stateCache);
    p_source = Memcache;
  }
 
 
  void SpicePosition::LoadCache(double time) {
    LoadCache(time, time, 1);
  }
 
 
  void SpicePosition::LoadCache(json &isdPos) {
    if (p_source != Spice) {
        throw IException(IException::Programmer, "SpicePosition::LoadCache(json) only supports Spice source", _FILEINFO_);
    }
 
    // Load the full cache time information from the label if available
    p_fullCacheStartTime = isdPos["spk_table_start_time"].get<double>();
    p_fullCacheEndTime = isdPos["spk_table_end_time"].get<double>();
    p_fullCacheSize = isdPos["spk_table_original_size"].get<double>();
    p_cacheTime = isdPos["ephemeris_times"].get<std::vector<double>>();
 
    p_hasVelocity = isdPos.find("velocities") != isdPos.end();
 
    std::vector<ale::State> stateCache;
    if (p_hasVelocity) {
      for (auto it = isdPos["positions"].begin(); it != isdPos["positions"].end(); it++) {
        int index = it - isdPos["positions"].begin();
        std::vector<double> pos = it->get<std::vector<double>>();
        std::vector<double> vel = isdPos["velocities"][index];
        stateCache.push_back(ale::State(ale::Vec3d(pos), ale::Vec3d(vel)));
      }
    }
    else {
      for (auto it = isdPos["positions"].begin(); it != isdPos["positions"].end(); it++) {
        std::vector<double> pos = {it->at(0).get<double>(), it->at(1).get<double>(), it->at(2).get<double>()};
        stateCache.push_back(ale::State(ale::Vec3d(pos)));
      }
    }
 
    if (m_state != NULL) {
      delete m_state;
    }
 
    m_state = new ale::States(p_cacheTime, stateCache);
 
    p_source = Memcache;
    SetEphemerisTime(p_cacheTime[0]);
  }
 
  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_);
    }
 
    std::vector<ale::State> stateCache;
    // 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_);
        }
 
        ale::State currentState(ale::Vec3d((double)rec[0],
                                           (double)rec[1],
                                           (double)rec[2]));
 
        int inext = 3;
 
        if (p_hasVelocity) {
          currentState.velocity = ale::Vec3d((double)rec[3],
                                             (double)rec[4],
                                             (double)rec[5]);
          inext = 6;
        }
        stateCache.push_back(currentState);
        p_cacheTime.push_back((double)rec[inext]);
      }
 
      if (m_state != NULL) {
        delete m_state;
      }
      m_state = new ale::States(p_cacheTime, stateCache);
    }
    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  && m_state->hasVelocity()) 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);
    }
 
    // 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;
 
      std::vector<ale::State> stateCache = m_state->getStates();
      for (int i = 0; i < (int)stateCache.size(); i++) {
        record[inext++] = stateCache[i].position.x;          // record[0]
        record[inext++] = stateCache[i].position.y;          // record[1]
        record[inext++] = stateCache[i].position.z;          // record[2]
        if (p_hasVelocity) {
          record[inext++] = stateCache[i].velocity.x;        // record[3]
          record[inext++] = stateCache[i].velocity.y;        // record[4]
          record[inext++] = stateCache[i].velocity.z;        // 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();
 
    // Load the time cache first
    LoadTimeCache();
 
    if (p_fullCacheSize > 1) {
      // Load the positions and velocity caches
      p_et = -DBL_MAX;   // Forces recalculation in SetEphemerisTime
      std::vector<ale::State> stateCache;
      for (std::vector<double>::size_type pos = 0; pos < p_cacheTime.size(); pos++) {
        SetEphemerisTime(p_cacheTime.at(pos));
        stateCache.push_back(ale::State(ale::Vec3d(p_coordinate), ale::Vec3d(p_velocity)));
      }
      if (m_state != NULL) {
        delete m_state;
      }
      m_state = new ale::States(p_cacheTime, stateCache);
    }
    else {
    // Load the position for the single updated time instance
      p_et = p_cacheTime[0];
      SetEphemerisTime(p_et);
      std::vector<ale::State> stateCache;
      stateCache.push_back(ale::Vec3d(p_coordinate));
      std::vector<double> timeCache;
      timeCache.push_back(p_cacheTime[0]);
      if (m_state != NULL) {
        delete m_state;
      }
      m_state = new ale::States(timeCache, stateCache);
    }
 
    // 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;
 
    // 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
    std::vector<ale::State> stateCache;
    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);
 
      // 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);
      stateCache.push_back(ale::State(p_coordinate, p_velocity));
    }
    if (m_state != NULL) {
      delete m_state;
    }
    m_state = new ale::States(p_cacheTime, stateCache);
 
    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
    int size = m_state->getStates().size();
    if (size == 1) {
      p_degree = 0;
    }
    else if (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(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(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() {
    ale::State state;
    if (p_cacheTime.size() == 1) {
      state = m_state->getStates().front();
    }
    else {
      state = m_state->getState(p_et, ale::LINEAR);
    }
    p_coordinate[0] = state.position.x;
    p_coordinate[1] = state.position.y;
    p_coordinate[2] = state.position.z;
    if (p_hasVelocity){
      p_velocity[0] = state.velocity.x;
      p_velocity[1] = state.velocity.y;
      p_velocity[2] = state.velocity.z;
    }
  }
 
 
  void SpicePosition::SetEphemerisTimeHermiteCache() {
    if (p_hasVelocity) {
      ale::State state = m_state->getState(p_et, ale::SPLINE);
 
      p_coordinate[0] = state.position.x;
      p_coordinate[1] = state.position.y;
      p_coordinate[2] = state.position.z;
 
      p_velocity[0] = state.velocity.x;
      p_velocity[1] = state.velocity.y;
      p_velocity[2] = state.velocity.z;
    }
    else {
      throw IException(IException::Io, "No velocities available. Cannot calculate Hermite Cache.",
                       _FILEINFO_);
    }
  }
 
 
 
  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) {
        ale::Vec3d velocity = m_state->getVelocities()[0];
        p_velocity[0] = velocity.x;
        p_velocity[1] = velocity.y;
        p_velocity[2] = velocity.z;
      }
      else {
        p_velocity[0] = ComputeVelocityInTime(WRT_X);
        p_velocity[1] = ComputeVelocityInTime(WRT_Y);
        p_velocity[2] = ComputeVelocityInTime(WRT_Z);
      }
    }
  }
 
 
  void SpicePosition::SetEphemerisTimePolyFunctionOverHermiteConstant() {
    SetEphemerisTimeHermiteCache();
    std::vector<double> hermiteCoordinate = p_coordinate;
 
    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_);
    }
 
    ale::States *tempStates = new ale::States(m_state->minimizeCache(tolerance));
    delete m_state;
    m_state = tempStates;
    p_cacheTime = m_state->getTimes();
    p_source = HermiteCache;
  }
 
 
  void SpicePosition::ClearCache() {
    if (m_state) {
      delete m_state;
      m_state = NULL;
    }
 
    p_cacheTime.clear();
  }
 
 
  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;
  //  }
}