SpiceRotation.cpp

#include "SpiceRotation.h"

#include <algorithm>
#include <cfloat>
#include <cmath>
#include <iomanip>
#include <string>
#include <vector>

#include <QDebug>
#include <QString>

#include <SpiceUsr.h>
#include <SpiceZfc.h>
#include <SpiceZmc.h>

#include "BasisFunction.h"
#include "IException.h"
#include "IString.h"
#include "LeastSquares.h"
#include "LineEquation.h"
#include "NaifStatus.h"
#include "PolynomialUnivariate.h"
#include "Quaternion.h"
#include "Table.h"
#include "TableField.h"

// Declarations for bindings for Naif Spicelib routines that do not have
// a wrapper
extern int refchg_(integer *frame1, integer *frame2, doublereal *et,
                   doublereal *rotate);
extern int frmchg_(integer *frame1, integer *frame2, doublereal *et,
                   doublereal *rotate);
extern int invstm_(doublereal *mat, doublereal *invmat);
// Temporary declarations for bindings for Naif supportlib routines

// These three declarations should be removed once supportlib is in Isis3
extern int ck3sdn(double sdntol, bool avflag, int *nrec,
                  double *sclkdp, double *quats, double *avvs,
                  int nints, double *starts, double *dparr,
                  int *intarr);

namespace Isis {
  SpiceRotation::SpiceRotation(int frameCode) {
    p_constantFrames.push_back(frameCode);
    p_timeBias = 0.0;
    p_source = Spice;
    p_CJ.resize(9);
    p_matrixSet = false;
    p_et = -DBL_MAX;
    p_degree = 2;
    p_degreeApplied = false;
    p_noOverride = true;
    p_axis1 = 3;
    p_axis2 = 1;
    p_axis3 = 3;
    p_minimizeCache = No;
    p_hasAngularVelocity = false;
    p_av.resize(3);
    p_fullCacheStartTime = 0;
    p_fullCacheEndTime = 0;
    p_fullCacheSize = 0;
    m_frameType = UNKNOWN;
    m_tOrientationAvailable = false;
  }


  SpiceRotation::SpiceRotation(int frameCode, int targetCode) {
    NaifStatus::CheckErrors();

    p_constantFrames.push_back(frameCode);
    p_targetCode = targetCode;
    p_timeBias = 0.0;
    p_source = Nadir;
    p_CJ.resize(9);
    p_matrixSet = false;
    p_et = -DBL_MAX;
    p_axisP = 3;
    p_degree = 2;
    p_degreeApplied = false;
    p_noOverride = true;
    p_axis1 = 3;
    p_axis2 = 1;
    p_axis3 = 3;
    p_minimizeCache = No;
    p_hasAngularVelocity = false;
    p_av.resize(3);
    p_fullCacheStartTime = 0;
    p_fullCacheEndTime = 0;

    p_fullCacheSize = 0;
    m_frameType = DYN;
    m_tOrientationAvailable = false;

    // Determine the axis for the velocity vector
    QString key = "INS" + toString(frameCode) + "_TRANSX";
    SpiceDouble transX[2];
    SpiceInt number;
    SpiceBoolean found;
    //Read starting at element 1 (skipping element 0)
    gdpool_c(key.toLatin1().data(), 1, 2, &number, transX, &found);

    if (!found) {
      QString msg = "Cannot find [" + key + "] in text kernels";
      throw IException(IException::Io, msg, _FILEINFO_);
    }

    p_axisV = 2;
    if (transX[0] < transX[1]) p_axisV = 1;

    NaifStatus::CheckErrors();
  }


  SpiceRotation::SpiceRotation(const SpiceRotation &rotToCopy) {
    p_cacheTime = rotToCopy.p_cacheTime;
    p_cache = rotToCopy.p_cache;
    p_cacheAv = rotToCopy.p_cacheAv;
    p_av = rotToCopy.p_av;
    p_degree = rotToCopy.p_degree;
    p_axis1 = rotToCopy.p_axis1;
    p_axis2 = rotToCopy.p_axis2;
    p_axis3 = rotToCopy.p_axis3;

    p_constantFrames = rotToCopy.p_constantFrames;
    p_timeFrames = rotToCopy.p_timeFrames;
    p_timeBias = rotToCopy.p_timeBias;

    p_et = rotToCopy.p_et;
    p_quaternion = rotToCopy.p_quaternion; 
    p_matrixSet = rotToCopy.p_matrixSet;
    p_source = rotToCopy.p_source;
    p_axisP = rotToCopy.p_axisP;
    p_axisV = rotToCopy.p_axisV;
    p_targetCode = rotToCopy.p_targetCode;
    p_baseTime = rotToCopy.p_baseTime;
    p_timeScale = rotToCopy.p_timeScale;
    p_degreeApplied = rotToCopy.p_degreeApplied;

//    for (std::vector<double>::size_type i = 0; i < rotToCopy.p_coefficients[0].size(); i++) 
    for (int i = 0; i < 3; i++) 
      p_coefficients[i] = rotToCopy.p_coefficients[i];

    p_noOverride = rotToCopy.p_noOverride;
    p_overrideBaseTime = rotToCopy.p_overrideBaseTime;
    p_overrideTimeScale = rotToCopy.p_overrideTimeScale;
    p_minimizeCache = rotToCopy.p_minimizeCache;
    p_fullCacheStartTime = rotToCopy.p_fullCacheStartTime;
    p_fullCacheEndTime = rotToCopy.p_fullCacheEndTime;
    p_fullCacheSize = rotToCopy.p_fullCacheSize;
    p_TC = rotToCopy.p_TC;

    p_CJ = rotToCopy.p_CJ;
    p_degree = rotToCopy.p_degree;
    p_hasAngularVelocity = rotToCopy.p_hasAngularVelocity;
    m_frameType = rotToCopy.m_frameType;

  }


  SpiceRotation::~SpiceRotation() { 
  }


  void SpiceRotation::SetFrame(int frameCode) {
    p_constantFrames[0] = frameCode;
  }


  int SpiceRotation::Frame() {
    return p_constantFrames[0];
  }


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


  void SpiceRotation::SetEphemerisTime(double et) {

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

    // Read from the cache
    if (p_source == Memcache) {
      setEphemerisTimeMemcache();
    }

    // Apply coefficients defining a function for each of the three camera angles and angular
    //  velocity if available
    else if (p_source == PolyFunction) {
      setEphemerisTimePolyFunction();
    }
    // Apply coefficients defining a function for each of the three camera angles and angular
    //  velocity if available
    else if (p_source == PolyFunctionOverSpice) {
      setEphemerisTimePolyFunctionOverSpice();
    }
    // Read from the kernel
    else if (p_source == Spice) {
      setEphemerisTimeSpice();
      // Retrieve the J2000 (code=1) to reference rotation matrix
    }
    // Apply coefficients from PCK version of IAU solution for target body orientation and angular
    //  velocity???
    else if (p_source == PckPolyFunction) {
      setEphemerisTimePckPolyFunction();
    }
    // Compute from Nadir
    else {
      setEphemerisTimeNadir();
    }

    // Set the quaternion for this rotation
//   p_quaternion.Set ( p_CJ );
  }


  double SpiceRotation::EphemerisTime() const {
    return p_et;
  }


  bool SpiceRotation::IsCached() const {
    return (p_cache.size() > 0);
  }


  void SpiceRotation::MinimizeCache(DownsizeStatus status) {
    p_minimizeCache = status;
  }


  void SpiceRotation::LoadCache(double startTime, double endTime, int size) {

    // Check for valid arguments
    if (size <= 0) {
      QString msg = "Argument cacheSize must not be less or equal to zero";
      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 and endTime differ";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

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

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

    // Make sure the constant frame is loaded.  This method also does the frame trace.
    if (p_timeFrames.size() == 0) InitConstantRotation(startTime);

    // Set the frame type.  If the frame class is PCK, load the constants.
    if (p_source == Spice) {
      setFrameType();
    }

    LoadTimeCache();
    int cacheSize = p_cacheTime.size();

    // Loop and load the cache
    for (int i = 0; i < cacheSize; i++) {
      double et = p_cacheTime[i];
      SetEphemerisTime(et);
      p_cache.push_back(p_CJ);
      if (p_hasAngularVelocity) p_cacheAv.push_back(p_av);
    }
    p_source = Memcache;

    // Downsize already loaded caches (both time and quats)
    if (p_minimizeCache == Yes  &&  cacheSize > 5) {
      LoadTimeCache();
    }
  }


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


  void SpiceRotation::LoadCache(Table &table) {
    // Clear any existing cached data to make it reentrant (KJB 2011-07-20).
    p_timeFrames.clear();
    p_TC.clear();
    p_cache.clear();
    p_cacheTime.clear();
    p_cacheAv.clear();
    p_hasAngularVelocity = false;

    // Load the constant and time-based frame traces and the constant rotation
    if (table.Label().hasKeyword("TimeDependentFrames")) {
      PvlKeyword labelTimeFrames = table.Label()["TimeDependentFrames"];
      for (int i = 0; i < labelTimeFrames.size(); i++) {
        p_timeFrames.push_back(toInt(labelTimeFrames[i]));
      }
    }
    else {
      p_timeFrames.push_back(p_constantFrames[0]);
      p_timeFrames.push_back(J2000Code);
    }

    if (table.Label().hasKeyword("ConstantRotation")) {
      PvlKeyword labelConstantFrames = table.Label()["ConstantFrames"];
      p_constantFrames.clear();

      for (int i = 0; i < labelConstantFrames.size(); i++) {
        p_constantFrames.push_back(toInt(labelConstantFrames[i]));
      }
      PvlKeyword labelConstantRotation = table.Label()["ConstantRotation"];

      for (int i = 0; i < labelConstantRotation.size(); i++) {
        p_TC.push_back(toDouble(labelConstantRotation[i]));
      }
    }
    else {
      p_TC.resize(9);
      ident_c((SpiceDouble( *)[3]) &p_TC[0]);
    }

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

    // Load FrameTypeCode from labels if available and the planetary constants keywords 
    if (table.Label().hasKeyword("FrameTypeCode")) {
      m_frameType = (FrameType) toInt(table.Label().findKeyword("FrameTypeCode")[0]);
    }
    else {
      m_frameType = UNKNOWN;
    }

    if (m_frameType  == PCK) {
      loadPCFromTable(table.Label());
    }

    int recFields = table[0].Fields();

    // Loop through and move the table to the cache.  Retrieve the first record to
    // establish the type of cache and then use the appropriate loop.

    // list table of quaternion and time
    if (recFields == 5) {
      for (int r = 0; r < table.Records(); r++) {
        TableRecord &rec = table[r];

        if (rec.Fields() != recFields) {
          // throw and error
        }

        std::vector<double> j2000Quat;
        j2000Quat.push_back((double)rec[0]);
        j2000Quat.push_back((double)rec[1]);
        j2000Quat.push_back((double)rec[2]);
        j2000Quat.push_back((double)rec[3]);

        Quaternion q(j2000Quat);
        std::vector<double> CJ = q.ToMatrix();
        p_cache.push_back(CJ);
        p_cacheTime.push_back((double)rec[4]);
      }
      p_source = Memcache;
    }

    // list table of quaternion, angular velocity vector, and time
    else if (recFields == 8) {
      for (int r = 0; r < table.Records(); r++) {
        TableRecord &rec = table[r];

        if (rec.Fields() != recFields) {
          // throw and error
        }

        std::vector<double> j2000Quat;
        j2000Quat.push_back((double)rec[0]);
        j2000Quat.push_back((double)rec[1]);
        j2000Quat.push_back((double)rec[2]);
        j2000Quat.push_back((double)rec[3]);


        Quaternion q(j2000Quat);
        std::vector<double> CJ = q.ToMatrix();
        p_cache.push_back(CJ);

        std::vector<double> av;
        av.push_back((double)rec[4]);
        av.push_back((double)rec[5]);
        av.push_back((double)rec[6]);
        p_cacheAv.push_back(av);

        p_cacheTime.push_back((double)rec[7]);
        p_hasAngularVelocity = true;
      }
      p_source = Memcache;
    }

    // coefficient table for angle1, angle2, and angle3
    else if (recFields == 3) {
      std::vector<double> coeffAng1, coeffAng2, coeffAng3;

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

        if (rec.Fields() != recFields) {
          // throw an error
        }
        coeffAng1.push_back((double)rec[0]);
        coeffAng2.push_back((double)rec[1]);
        coeffAng3.push_back((double)rec[2]);
      }

      // Take care of 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(coeffAng1, coeffAng2, coeffAng3);
      p_source = PolyFunction;
      if (degree > 0)  p_hasAngularVelocity = true;
      if (degree == 0  && p_cacheAv.size() > 0) p_hasAngularVelocity = true;
    }
    else  {
      QString msg = "Expecting either three, five, or eight fields in the SpiceRotation table";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
  }


  void SpiceRotation::ReloadCache() {
    // Save current et
    double et = p_et;
    p_et = -DBL_MAX;

    if (p_source == PolyFunction) {
    // Clear existing matrices from cache
      p_cacheTime.clear();
      p_cache.clear();

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

      // Load the time cache first
      p_minimizeCache = No;
      LoadTimeCache();

      if (p_fullCacheSize > 1) {
      // Load the matrix and av caches
        for (std::vector<double>::size_type pos = 0; pos < p_cacheTime.size(); pos++) {
          SetEphemerisTime(p_cacheTime.at(pos));
          p_cache.push_back(p_CJ);
          p_cacheAv.push_back(p_av);
        }
      }
      else {
      // Load the matrix for the single updated time instance
        SetEphemerisTime(p_cacheTime[0]);
        p_cache.push_back(p_CJ);
      }
    }
    else if (p_source == PolyFunctionOverSpice) {
      SpiceRotation tempRot(*this);

      std::vector<double>::size_type maxSize = p_fullCacheSize;

      // Clear the existing caches
      p_cache.clear();
      p_cacheTime.clear();
      p_cacheAv.clear();

      // Reload the time cache first
      p_minimizeCache = No;
      LoadTimeCache();

      for (std::vector<double>::size_type pos = 0; pos < maxSize; pos++) {
        tempRot.SetEphemerisTime(p_cacheTime.at(pos));
        p_cache.push_back(tempRot.TimeBasedMatrix());
        if (p_hasAngularVelocity) p_cacheAv.push_back(tempRot.AngularVelocity());
      }
    }
    else { //(p_source < PolyFunction) 
      QString msg = "The SpiceRotation has not yet been fit to a function";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // Set source to cache and reset current et
    // Make sure source is Memcache now
    p_source = Memcache;
    p_et = -DBL_MAX;
    SetEphemerisTime(et);
  }


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

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

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


  Table SpiceRotation::Cache(const QString &tableName) {
   // First handle conversion of PolyFunctionOverSpiceConstant
    // by converting it to the full Memcache and try to downsize it
    if (p_source == PolyFunctionOverSpice) {
      LineCache(tableName);

      //std::cout << "Full cache size is " << p_cache.size() << endl;

      p_minimizeCache = Yes;
      LoadTimeCache();

      //std::cout << "Minimized cache size is " << p_cache.size() << endl;
    }

    // Load the list of rotations and their corresponding times
    if (p_source == Memcache) {
      TableField q0("J2000Q0", TableField::Double);
      TableField q1("J2000Q1", TableField::Double);
      TableField q2("J2000Q2", TableField::Double);
      TableField q3("J2000Q3", TableField::Double);
      TableField t("ET", TableField::Double);

      TableRecord record;
      record += q0;
      record += q1;
      record += q2;
      record += q3;
      int timePos = 4;

      if (p_hasAngularVelocity) {
        TableField av1("AV1", TableField::Double);
        TableField av2("AV2", TableField::Double);
        TableField av3("AV3", TableField::Double);
        record += av1;
        record += av2;
        record += av3;
        timePos = 7;
      }

      record += t;
      Table table(tableName, record);

      for (int i = 0; i < (int)p_cache.size(); i++) {
        Quaternion q(p_cache[i]);
        std::vector<double> v = q.GetQuaternion();
        record[0] = v[0];
        record[1] = v[1];
        record[2] = v[2];
        record[3] = v[3];

        if (p_hasAngularVelocity) {
          record[4] = p_cacheAv[i][0];
          record[5] = p_cacheAv[i][1];
          record[6] = p_cacheAv[i][2];
        }

        record[timePos] = p_cacheTime[i];
        table += record;
      }

      CacheLabel(table);
      return table;
    }
    // Just load the position for the single epoch
    else if (p_source == PolyFunction  &&  p_degree == 0  &&  p_fullCacheSize == 1) {
      return LineCache(tableName);
    }
    // Load the coefficients for the curves fit to the 3 camera angles
    else if (p_source == PolyFunction) {
      TableField angle1("J2000Ang1", TableField::Double);
      TableField angle2("J2000Ang2", TableField::Double);
      TableField angle3("J2000Ang3", TableField::Double);

      TableRecord record;
      record += angle1;
      record += angle2;
      record += angle3;

      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;

      table += record;
      CacheLabel(table);
      return table;
    }
    else {
      // throw an error -- should not get here -- invalid Spice Source
      QString msg = "To create table source of data must be either Memcache or PolyFunction";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
  }


  void SpiceRotation::loadPCFromSpice(int centerBody) {
    NaifStatus::CheckErrors();
    SpiceInt centerBodyCode = (SpiceInt) centerBody;

    // Retrieve the frame class from Naif.  We distinguish PCK types into text PCK,
    // binary PCK, and PCK not referenced to J2000.  Isis binary PCK can
    // not be used to solve for target body orientation because of the variety and 
    // complexity models used with binary PCK.  Currently ISIS does not solve for
    // target body orientation on bodies not referenced to J2000, but it could be
    // changed to handle that case.
    checkForBinaryPck();
    
    if (m_frameType == PCK) {
      // Make sure the reference frame is J2000.  We will need to modify FrameTrace and
      // the pck methods in this class to handle this case.  At the time this method was
      // added, this case is not being used in the pck file.  It is mentioned in the Naif required
      // reading file, pck.req, as a possibility for future pck files.
      // Look for a reference frame keyword for the body.  The default is J2000.
      QString naifKeyword = "BODY" + toString(centerBodyCode) + "_CONSTANTS_REF_FRAME" ;
      SpiceInt numExpected;
      SpiceInt numReturned;
      SpiceChar naifType;
      SpiceDouble relativeFrameCode = 0;
      SpiceBoolean found;
      dtpool_c(naifKeyword.toLatin1().data(), &found, &numExpected, &naifType);

      if (found) {
        // Go get the frame name if it is not the default J2000
        SpiceDouble relativeFrameCode;
        bodvcd_c(centerBodyCode, "CONSTANTS_REF_FRAME", 1, 
                        &numReturned, &relativeFrameCode);
      } 

      if (!found || relativeFrameCode == 1) {  // We only work with J2000 relative frames for now

        // Make sure the standard coefficients are available for the body code by 
        // checking for ra
        naifKeyword = "BODY" + toString(centerBodyCode) + "_POLE_RA" ;
        dtpool_c(naifKeyword.toLatin1().data(), &found, &numExpected, &naifType);

        if (found) {
          std::vector<SpiceDouble> d(3);
          m_raPole.resize(numExpected);
          m_decPole.resize(numExpected);
          m_pm.resize(numExpected);

          bodvcd_c(centerBodyCode, "POLE_RA", numExpected, &numReturned, &d[0]);
          m_raPole[0].setDegrees(d[0]);
          m_raPole[1].setDegrees(d[1]);
          m_raPole[2].setDegrees(d[2]);

          bodvcd_c(centerBodyCode, "POLE_DEC", numExpected, &numReturned, &d[0]);
          m_decPole[0].setDegrees(d[0]);
          m_decPole[1].setDegrees(d[1]);
          m_decPole[2].setDegrees(d[2]);

          bodvcd_c(centerBodyCode, "PM", numExpected, &numReturned, &d[0]);
          m_pm[0].setDegrees(d[0]);
          m_pm[1].setDegrees(d[1]);
          m_pm[2].setDegrees(d[2]);

          // ***TODO*** Get long axis value
          m_tOrientationAvailable = true;

          // Now check for nutation/precession terms.  Check for nut/prec ra values
          // first to see if the terms are even used for this body.
          naifKeyword = "BODY" + toString(centerBodyCode) + "_NUT_PREC_RA" ;
          dtpool_c(naifKeyword.toLatin1().data(), &found, &numReturned, &naifType);
          if (found) {
            // Get the barycenter (bc) linear coefficients first (2 for each period).
            // Then we can get the maximum expected coefficients.
            SpiceInt bcCode = centerBodyCode/100;  // Ex: bc code for Jupiter (599) & its moons is 5
            naifKeyword = "BODY" + toString(bcCode) + "_NUT_PREC_ANGLES" ;
            dtpool_c(naifKeyword.toLatin1().data(), &found, &numExpected, &naifType);
            std::vector<double>npAngles(numExpected, 0.);
            bodvcd_c(bcCode, "NUT_PREC_ANGLES", numExpected, &numReturned, &npAngles[0]);
            numExpected /= 2.;
            m_raNutPrec.resize(numExpected, 0.);
            m_decNutPrec.resize(numExpected, 0.);
            m_pmNutPrec.resize(numExpected, 0.);
            
            std::vector<SpiceDouble> angles(numExpected);
            bodvcd_c(centerBodyCode, "NUT_PREC_RA", numExpected,  &numReturned, &m_raNutPrec[0]);
            bodvcd_c(centerBodyCode, "NUT_PREC_DEC", numExpected,  &numReturned, &m_decNutPrec[0]);
            bodvcd_c(centerBodyCode, "NUT_PREC_PM", numExpected,  &numReturned, &m_pmNutPrec[0]);

            // Finally get the system linear terms separated into vectors of the constants and the
            //  linear coefficients

            for (int i = 0; i < numExpected; i++) {
              m_sysNutPrec0.push_back(Angle(npAngles[i*2], Angle::Degrees));
              m_sysNutPrec1.push_back(Angle(npAngles[i*2+1], Angle::Degrees));
            }
          } // barycenter terms
        }  // PCK constants available
      }  // Relative to J2000
      else {
        m_frameType = NOTJ2000PCK;
      }
    }   // PCK

    NaifStatus::CheckErrors();
  }


  void SpiceRotation::loadPCFromTable(const PvlObject &label) {
    NaifStatus::CheckErrors();

    // First clear existing cached data
    m_raPole.clear();
    m_decPole.clear();
    m_pm.clear();
    m_raNutPrec.clear();
    m_decNutPrec.clear();
    m_sysNutPrec0.clear();
    m_sysNutPrec1.clear();
    int numLoaded = 0;

    // Load the PCK coeffcients if they are on the label
    if (label.hasKeyword("PoleRa")) {
      PvlKeyword labelCoeffs = label["PoleRa"];
      for (int i = 0; i < labelCoeffs.size(); i++){
        m_raPole.push_back(Angle(toDouble(labelCoeffs[i]), Angle::Degrees));
      }
      numLoaded += 1;
    }
    if (label.hasKeyword("PoleDec")) {
      PvlKeyword labelCoeffs = label["PoleDec"];
      for (int i = 0; i < labelCoeffs.size(); i++){
        m_decPole.push_back(Angle(toDouble(labelCoeffs[i]), Angle::Degrees));
      }
      numLoaded += 1;
    }
    if (label.hasKeyword("PrimeMeridian")) {
      PvlKeyword labelCoeffs = label["PrimeMeridian"];
      for (int i = 0; i < labelCoeffs.size(); i++){
        m_pm.push_back(Angle(toDouble(labelCoeffs[i]), Angle::Degrees));
      }
      numLoaded += 1;
    }
    if (numLoaded > 2) m_tOrientationAvailable = true;

    if (label.hasKeyword("PoleRaNutPrec")) {
      PvlKeyword labelCoeffs = label["PoleRaNutPrec"];
      for (int i = 0; i < labelCoeffs.size(); i++){
        m_raNutPrec.push_back(toDouble(labelCoeffs[i]));
      }
    }
    if (label.hasKeyword("PoleDecNutPrec")) {
      PvlKeyword labelCoeffs = label["PoleDecNutPrec"];
      for (int i = 0; i < labelCoeffs.size(); i++){
        m_decNutPrec.push_back(toDouble(labelCoeffs[i]));
      }
    }
    if (label.hasKeyword("PmNutPrec")) {
      PvlKeyword labelCoeffs = label["PmNutPrec"];
      for (int i = 0; i < labelCoeffs.size(); i++){
        m_pmNutPrec.push_back(toDouble(labelCoeffs[i]));
      }
    }
    if (label.hasKeyword("SysNutPrec0")) {
      PvlKeyword labelCoeffs = label["SysNutPrec0"];
      for (int i = 0; i < labelCoeffs.size(); i++){
        m_sysNutPrec0.push_back(Angle(toDouble(labelCoeffs[i]), Angle::Degrees));
      }
    }
    if (label.hasKeyword("SysNutPrec1")) {
      PvlKeyword labelCoeffs = label["SysNutPrec1"];
      for (int i = 0; i < labelCoeffs.size(); i++){
        m_sysNutPrec1.push_back(Angle(toDouble(labelCoeffs[i]), Angle::Degrees));
      }
    }
 
    NaifStatus::CheckErrors();
  }


  void SpiceRotation::CacheLabel(Table &table) {
    NaifStatus::CheckErrors();
    // Load the constant and time-based frame traces and the constant rotation
    // into the table as labels
    if (p_timeFrames.size() > 1) {
      table.Label() += PvlKeyword("TimeDependentFrames");

      for (int i = 0; i < (int) p_timeFrames.size(); i++) {
        table.Label()["TimeDependentFrames"].addValue(toString(p_timeFrames[i]));
      }
    }

    if (p_constantFrames.size() > 1) {
      table.Label() += PvlKeyword("ConstantFrames");

      for (int i = 0; i < (int) p_constantFrames.size(); i++) {
        table.Label()["ConstantFrames"].addValue(toString(p_constantFrames[i]));
      }

      table.Label() += PvlKeyword("ConstantRotation");

      for (int i = 0; i < (int) p_TC.size(); i++) {
        table.Label()["ConstantRotation"].addValue(toString(p_TC[i]));
      }
    }

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

 // Begin section added 06-20-2015 DAC
    table.Label() += PvlKeyword("FrameTypeCode");
    table.Label()["FrameTypeCode"].addValue(toString(m_frameType)); 

    if (m_frameType == PCK) {
      // Write out all the body orientation constants
      // Pole right ascension coefficients for a quadratic equation
      table.Label() += PvlKeyword("PoleRa");

      for (int i = 0; i < (int) m_raPole.size(); i++) {
        table.Label()["PoleRa"].addValue(toString(m_raPole[i].degrees()));
      }

      // Pole right ascension, declination coefficients for a quadratic equation
      table.Label() += PvlKeyword("PoleDec");
      for (int i = 0; i < (int) m_decPole.size(); i++) {
        table.Label()["PoleDec"].addValue(toString(m_decPole[i].degrees()));
      }

      // Prime meridian coefficients for a quadratic equation
      table.Label() += PvlKeyword("PrimeMeridian");
      for (int i = 0; i < (int) m_pm.size(); i++) {
        table.Label()["PrimeMeridian"].addValue(toString(m_pm[i].degrees()));
      }

      if (m_raNutPrec.size() > 0) {
        // Pole right ascension nutation precision coefficients to the trig terms
        table.Label() += PvlKeyword("PoleRaNutPrec");
        for (int i = 0; i < (int) m_raNutPrec.size(); i++) {
          table.Label()["PoleRaNutPrec"].addValue(toString(m_raNutPrec[i]));
        }

        // Pole declination nutation precision coefficients to the trig terms
        table.Label() += PvlKeyword("PoleDecNutPrec");
        for (int i = 0; i < (int) m_decNutPrec.size(); i++) {
          table.Label()["PoleDecNutPrec"].addValue(toString(m_decNutPrec[i]));
        }

        // Prime meridian nutation precision coefficients to the trig terms
        table.Label() += PvlKeyword("PmNutPrec");
        for (int i = 0; i < (int) m_pmNutPrec.size(); i++) {
          table.Label()["PmNutPrec"].addValue(toString(m_pmNutPrec[i]));
        }

        // System nutation precision constant terms of linear model of periods
        table.Label() += PvlKeyword("SysNutPrec0");
        for (int i = 0; i < (int) m_sysNutPrec0.size(); i++) {
          table.Label()["SysNutPrec0"].addValue(toString(m_sysNutPrec0[i].degrees()));
        }

        // System nutation precision linear terms of linear model of periods
        table.Label() += PvlKeyword("SysNutPrec1");
        for (int i = 0; i < (int) m_sysNutPrec1.size(); i++) {
          table.Label()["SysNutPrec1"].addValue(toString(m_sysNutPrec1[i].degrees()));
        }
      }
    }
 // End section added 06-20-2015 DAC

    NaifStatus::CheckErrors();
  }


  std::vector<double> SpiceRotation::GetCenterAngles() {
    // Compute the center time
    double etCenter = (p_fullCacheEndTime + p_fullCacheStartTime) / 2.;
    SetEphemerisTime(etCenter);

    return Angles(p_axis3, p_axis2, p_axis1);
  }


  std::vector<double> SpiceRotation::Angles(int axis3, int axis2, int axis1) {
    NaifStatus::CheckErrors();

    SpiceDouble ang1, ang2, ang3;
    m2eul_c((SpiceDouble *) &p_CJ[0], axis3, axis2, axis1, &ang3, &ang2, &ang1);

    std::vector<double> angles;
    angles.push_back(ang1);
    angles.push_back(ang2);
    angles.push_back(ang3);

    NaifStatus::CheckErrors();
    return angles;
  }



  void SpiceRotation::SetAngles(std::vector<double> angles, int axis3, int axis2, int axis1) {
    eul2m_c(angles[2], angles[1], angles[0], axis3, axis2, axis1, (SpiceDouble (*)[3]) &(p_CJ[0]));
    p_cache[0] = p_CJ;
    // Reset to get the new values 
    p_et = -DBL_MAX;
    SetEphemerisTime(p_et);
  }


  std::vector<double> SpiceRotation::AngularVelocity() {
    return p_av;
  }


  std::vector<int> SpiceRotation::ConstantFrameChain() {
    return p_constantFrames;
  }


  std::vector<int> SpiceRotation::TimeFrameChain() {
    return p_timeFrames;
  }


  bool SpiceRotation::HasAngularVelocity() {
    return p_hasAngularVelocity;
  }


  std::vector<double> SpiceRotation::J2000Vector(const std::vector<double> &rVec) {
    NaifStatus::CheckErrors();
    
    std::vector<double> jVec;

    if (rVec.size() == 3) {
      double TJ[3][3];
      mxm_c((SpiceDouble *) &p_TC[0], (SpiceDouble *) &p_CJ[0], TJ);
      jVec.resize(3);
      mtxv_c(TJ, (SpiceDouble *) &rVec[0], (SpiceDouble *) &jVec[0]);
    }
    
    else if (rVec.size() == 6) {
      // See Naif routine frmchg for the format of the state matrix.  The constant rotation, TC,
      // has a derivative with respect to time of I.
      if (!p_hasAngularVelocity) {
        // throw an error
      }
      std::vector<double> stateTJ(36);
      stateTJ = StateTJ();

      // Now invert (inverse of a state matrix is NOT simply the transpose)
      xpose6_c(&stateTJ[0], (SpiceDouble( *) [6]) &stateTJ[0]);
      double stateJT[6][6];
      invstm_((doublereal *) &stateTJ[0], (doublereal *) stateJT);
      xpose6_c(stateJT, stateJT);
      jVec.resize(6);

      mxvg_c(stateJT, (SpiceDouble *) &rVec[0], 6, 6, (SpiceDouble *) &jVec[0]);
    }

    NaifStatus::CheckErrors();
    return (jVec);
  }


  std::vector<Angle> SpiceRotation::poleRaCoefs() {
    return m_raPole;
  }


  std::vector<Angle> SpiceRotation::poleDecCoefs() {
    return m_decPole;
  }


  std::vector<Angle> SpiceRotation::pmCoefs() {
    return m_pm;
  }


  std::vector<double> SpiceRotation::poleRaNutPrecCoefs() {
    return m_raNutPrec;
  }


  std::vector<double> SpiceRotation::poleDecNutPrecCoefs() {
    return m_decNutPrec;
  }


  std::vector<double> SpiceRotation::pmNutPrecCoefs() {
    return m_pmNutPrec;
  }


  std::vector<Angle> SpiceRotation::sysNutPrecConstants() {
    return m_sysNutPrec0;
  }


  std::vector<Angle> SpiceRotation::sysNutPrecCoefs() {
    return m_sysNutPrec1;
  }


  std::vector<double> SpiceRotation::toJ2000Partial(const std::vector<double> &lookT, 
                                                    SpiceRotation::PartialType partialVar, 
                                                    int coeffIndex) {
    NaifStatus::CheckErrors();

    std::vector<double> jVec;

    // Get the rotation angles and form the derivative matrix for the partialVar
    std::vector<double> angles = Angles(p_axis3, p_axis2, p_axis1);
    int angleIndex = partialVar;
    int axes[3] = {p_axis1, p_axis2, p_axis3};

    double angle = angles.at(angleIndex);

    // Get TJ and apply the transpose to the input vector to get it to J2000
    double dmatrix[3][3];
    drotat_(&angle, (integer *) axes + angleIndex, (doublereal *) dmatrix);
    // Transpose to obtain row-major order
    xpose_c(dmatrix, dmatrix);

    // Get the derivative of the polynomial with respect to partialVar
    double dpoly = 0.;
    QString msg;
    switch (m_frameType) {
     case CK:
       dpoly = DPolynomial(coeffIndex);
       break;
     case PCK:
       dpoly = DPckPolynomial(partialVar, coeffIndex);
       break;
     case BPC:
       msg = "Body rotation uses a binary PCK.  Solutions for this model are not supported";
       throw IException(IException::User, msg, _FILEINFO_);
       break;
     case NOTJ2000PCK:
       msg = "Body rotation uses a PCK not referenced to J2000. "
             "Solutions for this model are not supported";    
       throw IException(IException::User, msg, _FILEINFO_);
       break;
     default:
       msg = "Solutions are not supported for this frame type.";
       throw IException(IException::User, msg, _FILEINFO_);
    }

    // Multiply dpoly to complete dmatrix
    for (int row = 0;  row < 3;  row++) {
      for (int col = 0;  col < 3;  col++) {
        dmatrix[row][col] *= dpoly;
      }
    }

    // Apply the other 2 angles and chain them all together to get J2000 to constant frame
    double dCJ[3][3];
    switch (angleIndex) {
      case 0:
        rotmat_c(dmatrix, angles[1], axes[1], dCJ);
        rotmat_c(dCJ, angles[2], axes[2], dCJ);
        break;
      case 1:
        rotate_c(angles[0], axes[0], dCJ);
        mxm_c(dmatrix, dCJ, dCJ);
        rotmat_c(dCJ, angles[2], axes[2], dCJ);
        break;
      case 2:
        rotate_c(angles[0], axes[0], dCJ);
        rotmat_c(dCJ, angles[1], axes[1], dCJ);
        mxm_c(dmatrix, dCJ, dCJ);
        break;
    }

    // Multiply the constant matrix to rotate to target reference frame
    double dTJ[3][3];
    mxm_c((SpiceDouble *) &p_TC[0], dCJ[0], dTJ);

    // Finally rotate the target vector with the transpose of the 
    // derivative matrix, dTJ to get a J2000 vector
    std::vector<double> lookdJ(3);

    mtxv_c(dTJ, (const SpiceDouble *) &lookT[0], (SpiceDouble *) &lookdJ[0]);

    NaifStatus::CheckErrors();
    return (lookdJ);
  }


  std::vector<double> SpiceRotation::ReferenceVector(const std::vector<double> &jVec) {
    NaifStatus::CheckErrors();

    std::vector<double> rVec(3);

    if (jVec.size() == 3) {
      double TJ[3][3];
      mxm_c((SpiceDouble *) &p_TC[0], (SpiceDouble *) &p_CJ[0], TJ);
      rVec.resize(3);
      mxv_c(TJ, (SpiceDouble *) &jVec[0], (SpiceDouble *) &rVec[0]);
    }
    else if (jVec.size() == 6) {
      // See Naif routine frmchg for the format of the state matrix.  The constant rotation, TC,
      // has a derivative with respect to time of I.
      if (!p_hasAngularVelocity) {
        // throw an error
      }
      std::vector<double>  stateTJ(36);
      stateTJ = StateTJ();
      rVec.resize(6);
      mxvg_c((SpiceDouble *) &stateTJ[0], (SpiceDouble *) &jVec[0], 6, 6, (SpiceDouble *) &rVec[0]);
    }

    NaifStatus::CheckErrors();
    return (rVec);
  }


  void SpiceRotation::SetPolynomial(const Source type) {
    NaifStatus::CheckErrors();
    std::vector<double> coeffAng1, coeffAng2, coeffAng3;

    // Rotation is already stored as a polynomial -- throw an error
    if (p_source == PolyFunction) {
      // Nothing to do
      return;
//      QString msg = "Rotation already fit to a polynomial -- spiceint first to refit";
//      throw IException(IException::User,msg,_FILEINFO_);
    }

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

    //Check for polynomial over original pointing constant and initialize coefficients
    if (type == PolyFunctionOverSpice) {
      coeffAng1.assign(p_degree + 1, 0.);
      coeffAng2.assign(p_degree + 1, 0.);
      coeffAng3.assign(p_degree + 1, 0.);
      SetPolynomial(coeffAng1, coeffAng2, coeffAng3, type);
      return;
    }

    Isis::PolynomialUnivariate function1(p_degree);   
    Isis::PolynomialUnivariate function2(p_degree);   
    Isis::PolynomialUnivariate function3(p_degree);   
    //
    LeastSquares *fitAng1 = new LeastSquares(function1);
    LeastSquares *fitAng2 = new LeastSquares(function2);
    LeastSquares *fitAng3 = new LeastSquares(function3);

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

    if (p_cache.size() == 1) {
      double t = p_cacheTime.at(0);
      SetEphemerisTime(t);
      std::vector<double> angles = Angles(p_axis3, p_axis2, p_axis1);
      coeffAng1.push_back(angles[0]);
      coeffAng2.push_back(angles[1]);
      coeffAng3.push_back(angles[2]);
    }
    else if (p_cache.size() == 2) {
// Load the times and get the corresponding rotation angles
      p_degree = 1;
      double t1 = p_cacheTime.at(0);
      SetEphemerisTime(t1);
      t1 -= p_baseTime;
      t1 = t1 / p_timeScale;
      std::vector<double> angles1 = Angles(p_axis3, p_axis2, p_axis1);
      double t2 = p_cacheTime.at(1);
      SetEphemerisTime(t2);
      t2 -= p_baseTime;
      t2 = t2 / p_timeScale;
      std::vector<double> angles2 = Angles(p_axis3, p_axis2, p_axis1);
      angles2[0] = WrapAngle(angles1[0], angles2[0]);
      angles2[2] = WrapAngle(angles1[2], angles2[2]);
      double slope[3];
      double intercept[3];

// Compute the linear equation for each angle and save them
      for (int angleIndex = 0; angleIndex < 3; angleIndex++) {
        Isis::LineEquation angline(t1, angles1[angleIndex], t2, angles2[angleIndex]);
        slope[angleIndex] = angline.Slope();
        intercept[angleIndex] = angline.Intercept();
      }
      coeffAng1.push_back(intercept[0]);
      coeffAng1.push_back(slope[0]);
      coeffAng2.push_back(intercept[1]);
      coeffAng2.push_back(slope[1]);
      coeffAng3.push_back(intercept[2]);
      coeffAng3.push_back(slope[2]);
    }
    else {
      // Load the known values to compute the fit equation
      double start1 = 0.; // value of 1st angle1 in cache
      double start3 = 0.; // value of 1st angle1 in cache

      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> angles = Angles(p_axis3, p_axis2, p_axis1);

// Fix 180/-180 crossovers on angles 1 and 3 before doing fit.
        if (pos == 0) {
          start1 = angles[0];
          start3 = angles[2];
        }
        else {
          angles[0] = WrapAngle(start1, angles[0]);
          angles[2] = WrapAngle(start3, angles[2]);
        }

        fitAng1->AddKnown(time, angles[0]);
        fitAng2->AddKnown(time, angles[1]);
        fitAng3->AddKnown(time, angles[2]);
        time.clear();

      }
      //Solve the equations for the coefficients
      fitAng1->Solve();
      fitAng2->Solve();
      fitAng3->Solve();

      // Delete the least squares objects now that we have all the coefficients
      delete fitAng1;
      delete fitAng2;
      delete fitAng3;

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

      for (int i = 0;  i < function1.Coefficients(); i++) {
        coeffAng1.push_back(function1.Coefficient(i));
        coeffAng2.push_back(function2.Coefficient(i));
        coeffAng3.push_back(function3.Coefficient(i));
      }

    }

    // Now that the coefficients have been calculated set the polynomial with them
    SetPolynomial(coeffAng1, coeffAng2, coeffAng3);

    NaifStatus::CheckErrors();
    return;
  }


  void SpiceRotation::SetPolynomial(const std::vector<double> &coeffAng1,
                                    const std::vector<double> &coeffAng2,
                                    const std::vector<double> &coeffAng3,
                                    const Source type) {

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

    // Load the functions with the coefficients
    function1.SetCoefficients(coeffAng1);
    function2.SetCoefficients(coeffAng2);
    function3.SetCoefficients(coeffAng3);

    // Compute the base time
    ComputeBaseTime();

    // Save the current coefficients
    p_coefficients[0] = coeffAng1;
    p_coefficients[1] = coeffAng2;
    p_coefficients[2] = coeffAng3;

    // Set the flag indicating p_degree has been applied to the camera angles, the
    // coefficients of the polynomials have been saved, and the cache reloaded from
    // the polynomials
    p_degreeApplied = true;
    //    p_source = PolyFunction;
    p_source = type;

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

    NaifStatus::CheckErrors();
    return;
  }


  void SpiceRotation::usePckPolynomial() {

    // Check to see if rotation is already stored as a polynomial 
    if (p_source == PckPolyFunction) {
      p_hasAngularVelocity = true;
      return;
    }

    // No target body orientation information available -- throw an error
    if (!m_tOrientationAvailable) {
      QString msg = "Target body orientation information not available.  Rerun spiceinit.";
      throw IException(IException::User, msg, _FILEINFO_);
    }

    /* I think we have nothing to do, but check for available coefficients and set the Source
    // Set the scales
    p_dscale =  0.000011574074074;   // seconds to days conversion
    p_tscale = 3.1688087814029D-10; // seconds to Julian centuries conversion
    //Set the quadratic part of the equations  
    //TODO consider just hard coding this part in evaluate
    Isis::PolynomialUnivariate functionRa(p_degree);  // Basis function fit to target body pole ra
    Isis::PolynomialUnivariate functionDec(p_degree);  // Basis function fit to target body pole dec
    Isis::PolynomialUnivariate functionPm(p_degree);  // Basis function fit to target body pm

    // Load the functions with the coefficients
    function1.SetCoefficients(p_raPole);
    function2.SetCoefficients(p_decPole);
    function3.SetCoefficients(p_pm);

    int numNutPrec = p_sysNutPrec0.size;
    if (numNutPrec > 0) {
      // Set the trigonometric part of the equation 
      Isis::TrigBasis functionRaNutPrec("raNutPrec", Isis::TrigBasis::Sin, numNutPrec);
      Isis::TrigBasis functionRaNutPrec("decNutPrec", Isis::TrigBasis::Cos, numNutPrec);
      Isis::TrigBasis functionRaNutPrec("pmNutPrec", Isis::TrigBasis::Sin, numNutPrec);
    // Load the functions with the coefficients
      functionRaNutPrec.SetTCoefficients(p_raNutPrec);
      functionDecNutPrec.SetTCoefficients(p_decNutPrec);
      functionPmNutPrec.SetTCoefficients(p_pmNutPrec);
      functionRaNutPrec.SetConstants(p_sysNutPrec0);
      functionDecNutPrec.SetConstants(p_sysNutPrec0);
      functionPmNutPrec.SetConstants(p_sysNutPrec0);
      functionRaNutPrec.SetLCoefficients(p_sysNutPrec1);
      functionDecNutPrec.SetLCoefficients(p_sysNutPrec1);
      functionPmNutPrec.SetLCoefficients(p_sysNutPrec1);
    }
    */

    // Set the flag indicating p_degree has been applied to the planet angles, the
    // coefficients of the polynomials have been saved, and the cache reloaded from  
    // TODO cache reloaded???
    // the polynomials.  
    // ****At least for the planet angles, I don't think we want to reload the cache until just
    // before we write out the table
    p_degreeApplied = true;
    p_hasAngularVelocity = true;
    p_source = PckPolyFunction;
   return;
  }


  void SpiceRotation::setPckPolynomial(const std::vector<Angle> &raCoeff,
                                       const std::vector<Angle> &decCoeff,
                                       const std::vector<Angle> &pmCoeff) {
    // Just set the constants and let usePckPolynomial() do the rest
    m_raPole = raCoeff;
    m_decPole = decCoeff;
    m_pm = pmCoeff;
    usePckPolynomial();
    // Apply new function parameters
    setEphemerisTimePckPolyFunction();
  }
                               

  void SpiceRotation::GetPolynomial(std::vector<double> &coeffAng1,
                                    std::vector<double> &coeffAng2,
                                    std::vector<double> &coeffAng3) {
    coeffAng1 = p_coefficients[0];
    coeffAng2 = p_coefficients[1];
    coeffAng3 = p_coefficients[2];

    return;
  }


  void SpiceRotation::getPckPolynomial(std::vector<Angle> &raCoeff,
                                       std::vector<Angle> &decCoeff,
                                       std::vector<Angle> &pmCoeff) {
    raCoeff = m_raPole;
    decCoeff = m_decPole;
    pmCoeff = m_pm;

    return;
  }


  void SpiceRotation::ComputeBaseTime() {
    if (p_noOverride) {
      p_baseTime = (p_cacheTime.at(0) + p_cacheTime.at(p_cacheTime.size() - 1)) / 2.;
      p_timeScale = p_baseTime - p_cacheTime.at(0);
      // Take care of case where 1st and last times are the same
      if (p_timeScale == 0)  p_timeScale = 1.0;
    }
    else {
      p_baseTime = p_overrideBaseTime;
      p_timeScale = p_overrideTimeScale;
    }

    return;
  }


  void SpiceRotation::SetOverrideBaseTime(double baseTime, double timeScale) {
    p_overrideBaseTime = baseTime;
    p_overrideTimeScale = timeScale;
    p_noOverride = false;
    return;
  }


  double SpiceRotation::DPolynomial(const int coeffIndex) {
    double derivative;
    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 rotation 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;
  }


  double SpiceRotation::DPckPolynomial(SpiceRotation::PartialType partialVar,
                                       const int coeffIndex) {
    const double p_dayScale = 86400; // number of seconds in a day
 // Number of 24 hour days in a Julian century = 36525    
    const double p_centScale = p_dayScale * 36525;
    double time = 0.;

    switch (partialVar) {
     case WRT_RightAscension:
     case WRT_Declination:
       time = p_et / p_centScale;
       break;
     case WRT_Twist:
       time = p_et / p_dayScale;
       break;
    }

    double derivative;

    switch (coeffIndex) {
     case 0:
       derivative = 1;
       break;
    case 1:
    case 2:
      derivative = pow(time, coeffIndex);
      break;
    default:
      QString msg = "Unable to evaluate the derivative of the target body rotation 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_);
    }

    // Now apply any difference to the partials due to building the rotation matrix on the angles
    //    90. + ra, 90. - dec, and pm.  The only partial that changes is dec.
    if (partialVar == WRT_Declination) {
      derivative = -derivative;
    }

    return derivative;
  }


  std::vector<double> SpiceRotation::ToReferencePartial(std::vector<double> &lookJ,
                                                        SpiceRotation::PartialType partialVar,
                                                        int coeffIndex) {
    NaifStatus::CheckErrors();
    //TODO** To save time possibly save partial matrices

    // Get the rotation angles and form the derivative matrix for the partialVar
    std::vector<double> angles = Angles(p_axis3, p_axis2, p_axis1);
    int angleIndex = partialVar;
    int axes[3] = {p_axis1, p_axis2, p_axis3};

    double angle = angles.at(angleIndex);

    double dmatrix[3][3];
    drotat_(&angle, (integer *) axes + angleIndex, (doublereal *) dmatrix);
    // Transpose to obtain row-major order
    xpose_c(dmatrix, dmatrix);

    // Get the derivative of the polynomial with respect to partialVar
    double dpoly = 0.;
    switch (m_frameType) {
     case UNKNOWN:  // For now let everything go through for backward compatability
     case CK:
       dpoly = DPolynomial(coeffIndex);
       break;
     case PCK:
       dpoly = DPckPolynomial(partialVar, coeffIndex);
       break;
     default:
      QString msg = "Only CK and PCK partials can be calculated";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }

    // Multiply dpoly to complete dmatrix
    for (int row = 0;  row < 3;  row++) {
      for (int col = 0;  col < 3;  col++) {
        dmatrix[row][col] *= dpoly;
      }
    }

    // Apply the other 2 angles and chain them all together
    double dCJ[3][3];
    switch (angleIndex) {
      case 0:
        rotmat_c(dmatrix, angles[1], axes[1], dCJ);
        rotmat_c(dCJ, angles[2], axes[2], dCJ);
        break;
      case 1:
        rotate_c(angles[0], axes[0], dCJ);
        mxm_c(dmatrix, dCJ, dCJ);
        rotmat_c(dCJ, angles[2], axes[2], dCJ);
        break;
      case 2:
        rotate_c(angles[0], axes[0], dCJ);
        rotmat_c(dCJ, angles[1], axes[1], dCJ);
        mxm_c(dmatrix, dCJ, dCJ);
        break;
    }

    // Multiply the constant matrix to rotate to the targeted reference frame
    double dTJ[3][3];
    mxm_c((SpiceDouble *) &p_TC[0], dCJ[0], dTJ);

    // Finally rotate the J2000 vector with the derivative matrix, dTJ to 
    // get the vector in the targeted reference frame.
    std::vector<double> lookdT(3);

    mxv_c(dTJ, (const SpiceDouble *) &lookJ[0], (SpiceDouble *) &lookdT[0]);

    NaifStatus::CheckErrors();
    return lookdT;
  }


  double SpiceRotation::WrapAngle(double compareAngle, double angle) {
    NaifStatus::CheckErrors();
    double diff1 = compareAngle - angle;

    if (diff1 < -1 * pi_c()) {
      angle -= twopi_c();
    }
    else if (diff1 > pi_c()) {
      angle += twopi_c();
    }

    NaifStatus::CheckErrors();
    return angle;
  }


  void SpiceRotation::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 then 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> coefAngle1(p_coefficients[0]),
          coefAngle2(p_coefficients[1]),
          coefAngle3(p_coefficients[2]);

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

      for (int icoef = 0;  icoef <= degree;  icoef++) {
        coefAngle1[icoef] = p_coefficients[0][icoef];
        coefAngle2[icoef] = p_coefficients[1][icoef];
        coefAngle3[icoef] = p_coefficients[2][icoef];
      }
      p_degree = degree;
      SetPolynomial(coefAngle1, coefAngle2, coefAngle3, p_source);
    }
  }


  SpiceRotation::FrameType SpiceRotation::getFrameType() {
    return m_frameType;
  }


  SpiceRotation::Source SpiceRotation::GetSource() {
    return p_source;
  }


  void SpiceRotation::SetSource(Source source) {
    p_source = source;
    return;
  }


  double SpiceRotation::GetBaseTime() {
    return p_baseTime;
  }


  double SpiceRotation::GetTimeScale() {
    return p_timeScale;
  }


  void SpiceRotation::SetAxes(int axis1, int axis2, int axis3) {
    if (axis1 < 1  ||  axis2 < 1  || axis3 < 1  || axis1 > 3  || axis2 > 3  || axis3 > 3) {
      QString msg = "A rotation axis is outside the valid range of 1 to 3";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    p_axis1 = axis1;
    p_axis2 = axis2;
    p_axis3 = axis3;
  }


  void SpiceRotation::LoadTimeCache() {
    NaifStatus::CheckErrors();
    int count = 0;

    double observStart  =  p_fullCacheStartTime + p_timeBias;
    double observEnd  = p_fullCacheEndTime + p_timeBias;
    //  Next line added 12-03-2009 to allow observations to cross segment boundaries
    double currentTime = observStart;
    bool timeLoaded = false;

    // Get number of ck loaded for this rotation.  This method assumes only one SpiceRotation
    // object is loaded.
    NaifStatus::CheckErrors();
    ktotal_c("ck", (SpiceInt *) &count);

    // Downsize the loaded cache
    if ((p_source == Memcache) && p_minimizeCache == Yes) {
      // Multiple ck case, type 5 ck case, or PolyFunctionOverSpice
      //  final step -- downsize loaded cache and reload

      if (p_fullCacheSize != (int) p_cache.size()) {

        QString msg =
          "Full cache size does NOT match cache size in LoadTimeCache -- should never happen";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }

      SpiceDouble timeSclkdp[p_fullCacheSize];
      SpiceDouble quats[p_fullCacheSize][4];
      double avvs[p_fullCacheSize][3];// Angular velocity vector

      // We will treat et as the sclock time and avoid converting back and forth 
     for (int r = 0; r < p_fullCacheSize; r++) {
        timeSclkdp[r] = p_cacheTime[r];
        SpiceDouble CJ[9] = { p_cache[r][0], p_cache[r][1], p_cache[r][2],
                              p_cache[r][3], p_cache[r][4], p_cache[r][5],
                              p_cache[r][6], p_cache[r][7], p_cache[r][8]
                            };
        m2q_c(CJ, quats[r]);
        if (p_hasAngularVelocity) {
          vequ_c((SpiceDouble *) &p_cacheAv[r][0], avvs[r]);
        }
      }

      double cubeStarts = timeSclkdp[0]; //,timsSclkdp[ckBlob.Records()-1] };
      double radTol = 0.000000017453; //.000001 degrees  Make this instrument dependent TODO
      SpiceInt avflag = 1;            // Don't use angular velocity for now
      SpiceInt nints = 1;             // Always make an observation a single interpolation interval
      double dparr[p_fullCacheSize];    // Double precision work array
      SpiceInt intarr[p_fullCacheSize]; // Integer work array
      SpiceInt sizOut = p_fullCacheSize; // Size of downsized cache

      ck3sdn(radTol, avflag, (int *) &sizOut, timeSclkdp, (doublereal *) quats, 
             (SpiceDouble *) avvs, nints, &cubeStarts, dparr, (int *) intarr);

      // Clear full cache and load with downsized version
      p_cacheTime.clear();
      p_cache.clear();
      p_cacheAv.clear();
      std::vector<double> av;
      av.resize(3);

      for (int r = 0; r < sizOut; r++) {
        SpiceDouble et;
        // sct2e_c(spcode, timeSclkdp[r], &et);
        et = timeSclkdp[r];
        p_cacheTime.push_back(et);
        std::vector<double> CJ(9);
        q2m_c(quats[r], (SpiceDouble( *)[3]) &CJ[0]);
        p_cache.push_back(CJ);
        vequ_c(avvs[r], (SpiceDouble *) &av[0]);
        p_cacheAv.push_back(av);
      }

      timeLoaded = true;
      p_minimizeCache = Done;
    }
    else if (count == 1  && p_minimizeCache == Yes) {
      // case of a single ck -- read instances and data straight from kernel for given time range
      SpiceInt handle;

      // Define some Naif constants
      int FILESIZ = 128;
      int TYPESIZ = 32;
      int SOURCESIZ = 128;
//      double DIRSIZ = 100;

      SpiceChar file[FILESIZ];
      SpiceChar filtyp[TYPESIZ];  // kernel type (ck, ek, etc.)
      SpiceChar source[SOURCESIZ];

      SpiceBoolean found;
      bool observationSpansToNextSegment = false;

      double segStartEt;
      double segStopEt;

      kdata_c(0, "ck", FILESIZ, TYPESIZ, SOURCESIZ, file, filtyp, source, &handle, &found);
      dafbfs_c(handle);
      daffna_c(&found);
      int spCode = ((int)(p_constantFrames[0] / 1000)) * 1000;

      while (found) {
        observationSpansToNextSegment = false;
        double sum[10];   // daf segment summary
        double dc[2];     // segment starting and ending times in tics
        SpiceInt ic[6];   // segment summary values:
        // instrument code for platform,
        // reference frame code,
        // data type,
        // velocity flag,
        // offset to quat 1,
        // offset to end.
        dafgs_c(sum);
        dafus_c(sum, (SpiceInt) 2, (SpiceInt) 6, dc, ic);

        // Don't read type 5 ck here
        if (ic[2] == 5) break;
//      
        // Check times for type 3 ck segment if spacecraft matches
        if (ic[0] == spCode && ic[2] == 3) {
          sct2e_c((int) spCode / 1000, dc[0], &segStartEt);
          sct2e_c((int) spCode / 1000, dc[1], &segStopEt);
          NaifStatus::CheckErrors();
          double et;

          // Get times for this segment
          if (currentTime >= segStartEt  &&  currentTime <= segStopEt) {

            // Check for a gap in the time coverage by making sure the time span of the observation 
            //  does not cross a segment unless the next segment starts where the current one ends
            if (observationSpansToNextSegment && currentTime > segStartEt) {
              QString msg = "Observation crosses segment boundary--unable to interpolate pointing";
              throw IException(IException::Programmer, msg, _FILEINFO_);
            }
            if (observEnd > segStopEt) {
              observationSpansToNextSegment = true;
            }

            // Extract necessary header parameters
            int dovelocity = ic[3];
            int end = ic[5];
            double val[2];
            dafgda_c(handle, end - 1, end, val);
//            int nints = (int) val[0];
            int ninstances = (int) val[1];
            int numvel  =  dovelocity * 3;
            int quatnoff  =  ic[4] + (4 + numvel) * ninstances - 1;
//            int nrdir = (int) (( ninstances - 1 ) / DIRSIZ); /* sclkdp directory records */
            int sclkdp1off  =  quatnoff + 1;
            int sclkdpnoff  =  sclkdp1off + ninstances - 1;
//            int start1off = sclkdpnoff + nrdir + 1;
//            int startnoff = start1off + nints - 1;
            int sclkSpCode = spCode / 1000;

            // Now get the times
            std::vector<double> sclkdp(ninstances);
            dafgda_c(handle, sclkdp1off, sclkdpnoff, (SpiceDouble *) &sclkdp[0]);

            int instance = 0;
            sct2e_c(sclkSpCode, sclkdp[0], &et);

            while (instance < (ninstances - 1)  &&  et < currentTime) {
              instance++;
              sct2e_c(sclkSpCode, sclkdp[instance], &et);
            }

            if (instance > 0) instance--;
            sct2e_c(sclkSpCode, sclkdp[instance], &et);

            while (instance < (ninstances - 1)  &&  et < observEnd) {
              p_cacheTime.push_back(et - p_timeBias);
              instance++;
              sct2e_c(sclkSpCode, sclkdp[instance], &et);
            }
            p_cacheTime.push_back(et - p_timeBias);

            if (!observationSpansToNextSegment) {
              timeLoaded = true;
              p_minimizeCache = Done;
              break;
            }
            else {
              currentTime = segStopEt;
            }
          }
        }
        dafcs_c(handle);     // Continue search in daf last searched
        daffna_c(&found);    // Find next forward array in current daf
      }
    }
    else if (count == 0  &&  p_source != Nadir  &&  p_minimizeCache == Yes) {
      QString msg = "No camera kernels loaded...Unable to determine time cache to downsize";
      throw IException(IException::User, msg, _FILEINFO_);
    }

    // Load times according to cache size (body rotations) -- handle first round of type 5 ck case 
    //   and multiple ck case --Load a time for every line scan line and downsize later
    if (!timeLoaded) {
      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++)
        p_cacheTime.push_back(p_fullCacheStartTime + (double) i * cacheSlope);
      if (p_source == Nadir) p_minimizeCache = No;
    }
    NaifStatus::CheckErrors();
  }


  std::vector<double> SpiceRotation::GetFullCacheTime() {

    // No time cache was initialized -- throw an error
    if (p_fullCacheSize < 1) {
      QString msg = "Time cache not available -- rerun spiceinit";
      throw IException(IException::User, msg, _FILEINFO_);
    }

    std::vector<double> fullCacheTime;
    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++)
      fullCacheTime.push_back(p_fullCacheStartTime + (double) i * cacheSlope);

    return fullCacheTime;
  }


  void SpiceRotation::FrameTrace(double et) {
    NaifStatus::CheckErrors();
    // The code for this method was extracted from the Naif routine rotget written by N.J. Bachman &
    //   W.L. Taber (JPL)
    int           center;
    FrameType type;
    int           typid;
    SpiceBoolean  found;
    int           frmidx;  // Frame chain index for current frame
    SpiceInt      nextFrame;   // Naif frame code of next frame
    NaifStatus::CheckErrors();
    std::vector<int> frameCodes;
    std::vector<int> frameTypes;
    frameCodes.push_back(p_constantFrames[0]);

    while (frameCodes[frameCodes.size()-1] != J2000Code) {
      frmidx  =  frameCodes.size() - 1;
      // First get the frame type  (Note:: we may also need to save center if we use dynamic frames)
      // (Another note:  the type returned is the Naif from type.  This is not quite the same as the
      // SpiceRotation enumerated FrameType.  The SpiceRotation FrameType differentiates between
      // pck types.  FrameTypes of 2, 6, and 7 will all be considered to be Naif frame type 2.  The
      // logic for FrameTypes in this method is correct for all types except type 7.  Current pck
      // do not exercise this option.  Should we ever use pck with a target body not referenced to
      // the J2000 frame and epoch, both this method and loadPCFromSpice will need to be modified.
      frinfo_c((SpiceInt) frameCodes[frmidx],
               (SpiceInt *) &center,
               (SpiceInt *) &type,
               (SpiceInt *) &typid, &found);

      if (!found) {

        if (p_source == Nadir) {
          frameTypes.push_back(0);
          break;
        }

        QString msg = "The frame" + toString((int) frameCodes[frmidx]) + " is not supported by Naif";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }

      double matrix[3][3];

      // To get the next link in the frame chain, use the frame type
      if (type == INERTL ||  type == PCK) {
        nextFrame = J2000Code;
      }
      else if (type == CK) {
        ckfrot_((SpiceInt *) &typid, &et, (double *) matrix, &nextFrame, (logical *) &found);

        if (!found) {

          if (p_source == Nadir) {
            frameTypes.push_back(0);
            break;
          }

          QString msg = "The ck rotation from frame " + toString(frameCodes[frmidx]) + " can not "
               + "be found due to no pointing available at requested time or a problem with the "
               + "frame";
          throw IException(IException::Programmer, msg, _FILEINFO_);
        }
      }
      else if (type == TK) {
        tkfram_((SpiceInt *) &typid, (double *) matrix, &nextFrame, (logical *) &found);
        if (!found) {
          QString msg = "The tk rotation from frame " + toString(frameCodes[frmidx]) +
                        " can not be found";
          throw IException(IException::Programmer, msg, _FILEINFO_);
        }
      }
      else if (type == DYN) {
        //
        //        Unlike the other frame classes, the dynamic frame evaluation
        //        routine ZZDYNROT requires the input frame ID rather than the
        //        dynamic frame class ID. ZZDYNROT also requires the center ID
        //        we found via the FRINFO call.

        zzdynrot_((SpiceInt *) &typid, (SpiceInt *) &center, &et, (double *) matrix, &nextFrame);
      }

      else {
        QString msg = "The frame " + toString(frameCodes[frmidx]) +
            " has a type " + toString(type) + " not supported by your version of Naif Spicelib." 
            + "You need to update.";
        throw IException(IException::Programmer, msg, _FILEINFO_);

      }
      frameCodes.push_back(nextFrame);
      frameTypes.push_back(type);
    }

    if ((int) frameCodes.size() == 1  &&  p_source != Nadir) {  // Must be Sky
      p_constantFrames.push_back(frameCodes[0]);
      p_timeFrames.push_back(frameCodes[0]);
      return;
    }

    int nConstants = 0;
    p_constantFrames.clear();
    while (frameTypes[nConstants] == TK  &&  nConstants < (int) frameTypes.size()) nConstants++;


    for (int i = 0; i <= nConstants; i++) {
      p_constantFrames.push_back(frameCodes[i]);
    }

    if (p_source != Nadir)  {
      for (int i = nConstants;  i < (int) frameCodes.size(); i++) {
        p_timeFrames.push_back(frameCodes[i]);
      }
    }
    else {
      // Nadir rotation is from spacecraft to J2000
      p_timeFrames.push_back(frameCodes[nConstants]);
      p_timeFrames.push_back(J2000Code);
    }
    NaifStatus::CheckErrors();
  }


  std::vector<double> SpiceRotation::Matrix() {
    NaifStatus::CheckErrors();
    std::vector<double> TJ;
    TJ.resize(9);
    mxm_c((SpiceDouble *) &p_TC[0], (SpiceDouble *) &p_CJ[0], (SpiceDouble( *) [3]) &TJ[0]);
    NaifStatus::CheckErrors();
    return TJ;
  }


  std::vector<double> SpiceRotation::ConstantRotation() {
    NaifStatus::CheckErrors();
    std::vector<double> q;
    q.resize(4);
    m2q_c((SpiceDouble( *)[3]) &p_TC[0], &q[0]);
    NaifStatus::CheckErrors();
    return q;
  }


  std::vector<double> &SpiceRotation::ConstantMatrix() {
    return p_TC;
  }


  void SpiceRotation::SetConstantMatrix(std::vector<double> constantMatrix) {
    p_TC = constantMatrix;
    return;
  }

  std::vector<double> SpiceRotation::TimeBasedRotation() {
    NaifStatus::CheckErrors();
    std::vector<double> q;
    q.resize(4);
    m2q_c((SpiceDouble( *)[3]) &p_CJ[0], &q[0]);
    NaifStatus::CheckErrors();
    return q;
  }


  std::vector<double> &SpiceRotation::TimeBasedMatrix() {
    return p_CJ;
  }


  void SpiceRotation::SetTimeBasedMatrix(std::vector<double> timeBasedMatrix) {
    p_CJ = timeBasedMatrix;
    return;
  }


  void SpiceRotation::InitConstantRotation(double et) {
    FrameTrace(et);
    // Get constant rotation which applies in all cases
    int targetFrame = p_constantFrames[0];
    int fromFrame = p_timeFrames[0];
    p_TC.resize(9);
    refchg_((SpiceInt *) &fromFrame, (SpiceInt *) &targetFrame, &et, (doublereal *) &p_TC[0]);
    // Transpose to obtain row-major order
    xpose_c((SpiceDouble( *)[3]) &p_TC[0], (SpiceDouble( *)[3]) &p_TC[0]);
  }


  void SpiceRotation::ComputeAv() {
    NaifStatus::CheckErrors();

    // Make sure the angles have been fit to polynomials so we can calculate the derivative
    if (p_source < PolyFunction ) {
      QString msg = "The SpiceRotation pointing angles must be fit to polynomials in order to ";
      msg += "compute angular velocity.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    if (p_source == PckPolyFunction) {
      QString msg = "Planetary angular velocity must be fit computed with PCK polynomials ";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    std::vector<double> dCJdt;
    dCJdt.resize(9);

    switch (m_frameType) {
        // Treat all cases the same except for target body rotations
      case UNKNOWN:
      case INERTL:
      case TK:
      case DYN:
      case CK:
        DCJdt(dCJdt);
        break;
      case PCK:
      case BPC:
      case NOTJ2000PCK:
        break;
      }
    double omega[3][3];
    mtxm_c((SpiceDouble( *)[3]) &dCJdt[0], (SpiceDouble( *)[3]) &p_CJ[0], omega);
    p_av[0] = omega[2][1];
    p_av[1] = omega[0][2];
    p_av[2] = omega[1][0];
    NaifStatus::CheckErrors();
  }


  void SpiceRotation::DCJdt(std::vector<double> &dCJ) {
    NaifStatus::CheckErrors();

    // Get the rotation angles and axes
    std::vector<double> angles = Angles(p_axis3, p_axis2, p_axis1);
    int axes[3] = {p_axis1, p_axis2, p_axis3};

    double dmatrix[3][3];
    double dangle;
    double wmatrix[3][3]; // work matrix
    dCJ.assign(9, 0.);

    for (int angleIndex = 0; angleIndex < 3; angleIndex++) {
      drotat_(&(angles[angleIndex]), (integer *) axes + angleIndex, (doublereal *) dmatrix);
      // Transpose to obtain row-major order
      xpose_c(dmatrix, dmatrix);

      // To get the derivative of the polynomial fit to the angle with respect to time
      // first create the function object for this angle and load its coefficients
      Isis::PolynomialUnivariate function(p_degree);
      function.SetCoefficients(p_coefficients[angleIndex]);

      // Evaluate the derivative of function at p_et
      //      dangle = function.DerivativeVar((p_et - p_baseTime) / p_timeScale);
      dangle = function.DerivativeVar((p_et - p_baseTime) / p_timeScale) / p_timeScale;

      // Multiply dangle to complete dmatrix
      for (int row = 0;  row < 3;  row++) {
        for (int col = 0;  col < 3;  col++) {
          dmatrix[row][col] *= dangle;
        }
      }
      // Apply the other 2 angles and chain them all together
      switch (angleIndex) {
        case 0:
          rotmat_c(dmatrix, angles[1], axes[1], dmatrix);
          rotmat_c(dmatrix, angles[2], axes[2], dmatrix);
          break;
        case 1:
          rotate_c(angles[0], axes[0], wmatrix);
          mxm_c(dmatrix, wmatrix, dmatrix);
          rotmat_c(dmatrix, angles[2], axes[2], dmatrix);
          break;
        case 2:
          rotate_c(angles[0], axes[0], wmatrix);
          rotmat_c(wmatrix, angles[1], axes[1], wmatrix);
          mxm_c(dmatrix, wmatrix, dmatrix);
          break;
      }
      int i, j;
      for (int index = 0; index < 9; index++) {
        i = index / 3;
        j = index % 3;
        dCJ[index] += dmatrix[i][j];
      }
    }

    NaifStatus::CheckErrors();
  }


  std::vector<double> SpiceRotation::StateTJ() {
    std::vector<double> stateTJ(36);

    // Build the state matrix for the time-based rotation from the matrix and angulary velocity
    double stateCJ[6][6];
    rav2xf_c(&p_CJ[0], &p_av[0], stateCJ);
// (SpiceDouble (*) [3]) &p_CJ[0]
    int irow = 0;
    int jcol = 0;
    int vpos = 0;

    for (int row = 3; row < 6; row++) {
      irow  =  row - 3;
      vpos  =  irow * 3;

      for (int col = 0; col < 3; col++) {
        jcol  =  col + 3;
        // Fill the upper left corner
        stateTJ[irow*6 + col] = p_TC[vpos] * stateCJ[0][col] + p_TC[vpos+1] * stateCJ[1][col] 
                                              + p_TC[vpos+2] * stateCJ[2][col];
        // Fill the lower left corner
        stateTJ[row*6 + col]  =  p_TC[vpos] * stateCJ[3][col] + p_TC[vpos+1] * stateCJ[4][col]
                                              + p_TC[vpos+2] * stateCJ[5][col];
        // Fill the upper right corner
        stateTJ[irow*6 + jcol] = 0;
        // Fill the lower right corner
        stateTJ[row*6 +jcol] = stateTJ[irow*6 + col];
      }
    }
    return stateTJ;
  }


   std::vector<double> SpiceRotation::Extrapolate(double timeEt) {
    NaifStatus::CheckErrors();

    if (!p_hasAngularVelocity) return p_CJ;

    double diffTime = timeEt - p_et;
    std::vector<double> CJ(9, 0.0);
    double dmat[3][3];

    // Create a rotation matrix for the axis and magnitude of the angular velocity multiplied by  
    //   the time difference
    axisar_c((SpiceDouble *) &p_av[0], diffTime*vnorm_c((SpiceDouble *) &p_av[0]), dmat);

    // Rotate from the current time to the desired time assuming constant angular velocity
    mxm_c(dmat, (SpiceDouble *) &p_CJ[0], (SpiceDouble( *)[3]) &CJ[0]);
    NaifStatus::CheckErrors();
    return CJ;
   }


   void SpiceRotation::SetFullCacheParameters(double startTime, double endTime, int cacheSize) {
     // Save full cache parameters
     p_fullCacheStartTime = startTime;
     p_fullCacheEndTime = endTime;
     p_fullCacheSize = cacheSize;
   }


  void SpiceRotation::checkForBinaryPck() {
    // Get a count of all the loaded kernels
    SpiceInt count;
    ktotal_c("PCK", &count);
    
    // Define some Naif constants
    int FILESIZ = 128;
    int TYPESIZ = 32;
    int SOURCESIZ = 128;
    SpiceChar file[FILESIZ];
    SpiceChar filetype[TYPESIZ];
    SpiceChar source[SOURCESIZ];
    SpiceInt handle;
    SpiceBoolean found;
       
    // Get the name of each loaded kernel.  The accuracy of this test depends on the use of the
    // "bpc" suffix to name a binary pck.  If a binary bpc does not have the "bpc" suffix, it will not
    // be found by this test.  This test was suggested by Boris Semenov at Naif.
    for (SpiceInt knum = 0; knum < count; knum++){
      kdata_c(knum, "PCK", FILESIZ, TYPESIZ, SOURCESIZ, file, filetype, source, &handle, &found);

      // search file for "bpc"
      if (strstr(file, "bpc") != NULL) {
        m_frameType = BPC;
      }
    }
  }
  

  void SpiceRotation::setFrameType() {
    SpiceInt frameCode = p_constantFrames[0];
    SpiceBoolean found;
    SpiceInt centerBodyCode;
    SpiceInt frameClass;
    SpiceInt classId;
    frinfo_c(frameCode, &centerBodyCode, &frameClass, &classId, &found);

    if (found) {
      if (frameClass == 2  ||  centerBodyCode > 0) {
        m_frameType = PCK;
        // Load the PC information while it is available and set member values
        loadPCFromSpice(centerBodyCode);
      }
      else if (p_constantFrames.size() > 1) {
        for (std::vector<int>::size_type idx = 1; idx < p_constantFrames.size(); idx++) {
          frameCode = p_constantFrames[idx];
          frinfo_c(frameCode, &centerBodyCode, &frameClass, &classId, &found);
          if (frameClass == 3) m_frameType = CK;
        }
      }
      else {
        switch (frameClass) {
        case 1:
          m_frameType = INERTL;
          break;
          // case 2:  handled in first if block
        case 3:
          m_frameType = CK;
          break;
        case 4:
          m_frameType = TK;
          break;
        case 5:
          m_frameType = DYN;
          break;
        default:
          m_frameType = UNKNOWN;
        }
      }
    }      
  }


  void SpiceRotation::setEphemerisTimeMemcache() {
    // If the cache has only one rotation, set it
    NaifStatus::CheckErrors();

    if (p_cache.size() == 1) {
      /*        p_quaternion = p_cache[0];*/
      p_CJ = p_cache[0];
//         p_CJ = p_quaternion.ToMatrix();

      if (p_hasAngularVelocity) {
        p_av = p_cacheAv[0];
      }

    }

    // Otherwise determine the interval to interpolate
    else {
      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 rotation
      double mult = (p_et - p_cacheTime[cacheIndex]) /
                    (p_cacheTime[cacheIndex+1] - p_cacheTime[cacheIndex]);
      /*        Quaternion Q2 (p_cache[cacheIndex+1]);
               Quaternion Q1 (p_cache[cacheIndex]);*/
      std::vector<double> CJ2(p_cache[cacheIndex+1]);
      std::vector<double> CJ1(p_cache[cacheIndex]);
      SpiceDouble J2J1[3][3];
      mtxm_c((SpiceDouble( *)[3]) &CJ2[0], (SpiceDouble( *)[3]) &CJ1[0], J2J1);
      SpiceDouble axis[3];
      SpiceDouble angle;
      raxisa_c(J2J1, axis, &angle);
      SpiceDouble delta[3][3];
      axisar_c(axis, angle * (SpiceDouble)mult, delta);
      mxmt_c((SpiceDouble *) &CJ1[0], delta, (SpiceDouble( *) [3]) &p_CJ[0]);
 
      if (p_hasAngularVelocity) {
        double v1[3], v2[3]; // Vectors surrounding desired time
        vequ_c((SpiceDouble *) &p_cacheAv[cacheIndex][0], v1);
        vequ_c((SpiceDouble *) &p_cacheAv[cacheIndex+1][0], v2);
        vscl_c((1. - mult), v1, v1);
        vscl_c(mult, v2, v2);
        vadd_c(v1, v2, (SpiceDouble *) &p_av[0]);
      }
    }
    NaifStatus::CheckErrors();
  }


  void SpiceRotation::setEphemerisTimeNadir() {
   // TODO what about spk time bias and mission setting of light time corrections
   //      That information has only been passed to the SpicePosition class and
   //      is not available to this class, but probably should be applied to the
   //      spkez call.

   // Make sure the constant frame is loaded.  This method also does the frame trace.
   NaifStatus::CheckErrors();
    if (p_timeFrames.size() == 0) InitConstantRotation(p_et);

    SpiceDouble stateJ[6];  // Position and velocity vector in J2000
    SpiceDouble lt;
    SpiceInt spkCode = p_constantFrames[0] / 1000;
    spkez_c((SpiceInt) spkCode, p_et, "J2000", "LT+S",
            (SpiceInt) p_targetCode, stateJ, &lt);
   // reverse the position to be relative to the spacecraft.  This may be
   // a mission dependent value and possibly the sense of the velocity as well.
    SpiceDouble sJ[3], svJ[3];
    vpack_c(-1 * stateJ[0], -1 * stateJ[1], -1 * stateJ[2], sJ);
    vpack_c(stateJ[3], stateJ[4], stateJ[5], svJ);
    twovec_c(sJ,
             p_axisP,
             svJ,
             p_axisV,
             (SpiceDouble( *)[3]) &p_CJ[0]);
    NaifStatus::CheckErrors();
  }


  void SpiceRotation::setEphemerisTimeSpice() {
   NaifStatus::CheckErrors();
   SpiceInt j2000 = J2000Code;

   SpiceDouble time = p_et + p_timeBias;

   // Make sure the constant frame is loaded.  This method also does the frame trace.
   if (p_timeFrames.size() == 0) InitConstantRotation(p_et);
   int toFrame = p_timeFrames[0];

   // First try getting the entire state matrix (6x6), which includes CJ and the angular velocity
   double stateCJ[6][6];
   frmchg_((integer *) &j2000, (integer *) &toFrame, &time, (doublereal *) stateCJ);

   // If Naif fails attempting to get the state matrix, assume the angular velocity vector is
   // not available and just get the rotation matrix.  First turn off Naif error reporting and
   // return any error without printing them.
   SpiceBoolean ckfailure = failed_c();
   reset_c();                   // Reset Naif error system to allow caller to recover

   if (!ckfailure) {
     xpose6_c(stateCJ, stateCJ);
     xf2rav_c(stateCJ, (SpiceDouble( *)[3]) &p_CJ[0], (SpiceDouble *) &p_av[0]);
     p_hasAngularVelocity = true;
   }
   else {
     refchg_((integer *) &j2000, (integer *) &toFrame, &time, (SpiceDouble *) &p_CJ[0]);

     if (failed_c()) {
       char naifstr[64];
       getmsg_c("SHORT", sizeof(naifstr), naifstr);
       reset_c();  // Reset Naif error system to allow caller to recover

       if (eqstr_c(naifstr, "SPICE(UNKNOWNFRAME)")) {
         Isis::IString msg = Isis::IString((int) p_constantFrames[0]) + " is an unrecognized " +
                             "reference frame code.  Has the mission frames kernel been loaded?";
         throw IException(IException::Io, msg, _FILEINFO_);
       }
       else {
         Isis::IString msg = "No pointing available at requested time [" +
                             Isis::IString(p_et + p_timeBias) + "] for frame code [" +
                             Isis::IString((int) p_constantFrames[0]) + "]";
         throw IException(IException::Io, msg, _FILEINFO_);
       }
     }

     // Transpose to obtain row-major order
     xpose_c((SpiceDouble( *)[3]) &p_CJ[0], (SpiceDouble( *)[3]) &p_CJ[0]);
   }
   NaifStatus::CheckErrors();
  }


  std::vector<double> SpiceRotation::EvaluatePolyFunction() {
   Isis::PolynomialUnivariate function1(p_degree);
   Isis::PolynomialUnivariate function2(p_degree);
   Isis::PolynomialUnivariate function3(p_degree);

   // Load the functions with the coefficients
   function1.SetCoefficients(p_coefficients[0]);
   function2.SetCoefficients(p_coefficients[1]);
   function3.SetCoefficients(p_coefficients[2]);

   std::vector<double> rtime;
   rtime.push_back((p_et - p_baseTime) / p_timeScale);
   std::vector<double> angles;
   angles.push_back(function1.Evaluate(rtime));
   angles.push_back(function2.Evaluate(rtime));
   angles.push_back(function3.Evaluate(rtime));

   // Get the first angle back into the range Naif expects [-180.,180.]
   if (angles[0] <= -1 * pi_c()) {
     angles[0] += twopi_c();
   }
   else if (angles[0] > pi_c()) {
     angles[0] -= twopi_c();
   }
   return angles;
  }


  void SpiceRotation::setEphemerisTimePolyFunction() {
   NaifStatus::CheckErrors();
   Isis::PolynomialUnivariate function1(p_degree);
   Isis::PolynomialUnivariate function2(p_degree);
   Isis::PolynomialUnivariate function3(p_degree);

   // Load the functions with the coefficients
   function1.SetCoefficients(p_coefficients[0]);
   function2.SetCoefficients(p_coefficients[1]);
   function3.SetCoefficients(p_coefficients[2]);

   std::vector<double> rtime;
   rtime.push_back((p_et - p_baseTime) / p_timeScale);
   double angle1 = function1.Evaluate(rtime);
   double angle2 = function2.Evaluate(rtime);
   double angle3 = function3.Evaluate(rtime);

   // Get the first angle back into the range Naif expects [-180.,180.]
   if (angle1 < -1 * pi_c()) {
     angle1 += twopi_c();
   }
   else if (angle1 > pi_c()) {
     angle1 -= twopi_c();
   }

   eul2m_c((SpiceDouble) angle3, (SpiceDouble) angle2, (SpiceDouble) angle1,
           p_axis3,              p_axis2,              p_axis1,
           (SpiceDouble( *)[3]) &p_CJ[0]);

   if (p_hasAngularVelocity) {
     if ( p_degree == 0)
       p_av = p_cacheAv[0];
     else
       ComputeAv();
   }
   NaifStatus::CheckErrors();
  }


  void SpiceRotation::setEphemerisTimePolyFunctionOverSpice() {
    setEphemerisTimeMemcache();
    NaifStatus::CheckErrors();
    std::vector<double> cacheAngles(3);
    std::vector<double> cacheVelocity(3);
    cacheAngles = Angles(p_axis3, p_axis2, p_axis1);
    cacheVelocity = p_av;
    setEphemerisTimePolyFunction();
    std::vector<double> polyAngles(3);
    // The decomposition fails because the angles are outside the valid range for Naif
    // polyAngles = Angles(p_axis3, p_axis2, p_axis1); 
    polyAngles = EvaluatePolyFunction();
    std::vector<double> polyVelocity(3);
    polyVelocity = p_av;
    std::vector<double> angles(3);

    for (int index = 0; index < 3; index++) {
      angles[index] = cacheAngles[index] + polyAngles[index];
      p_av[index] += cacheVelocity[index];
    }

   if (angles[0] <= -1 * pi_c()) {
     angles[0] += twopi_c();
   }
   else if (angles[0] > pi_c()) {
     angles[0] -= twopi_c();
   }

   if (angles[2] <= -1 * pi_c()) {
     angles[2] += twopi_c();
   }
   else if (angles[2] > pi_c()) {
     angles[2] -= twopi_c();
   }

   eul2m_c((SpiceDouble) angles[2], (SpiceDouble) angles[1], (SpiceDouble) angles[0],
           p_axis3,                 p_axis2,                 p_axis1,
           (SpiceDouble( *)[3]) &p_CJ[0]);
  }


  void SpiceRotation::setEphemerisTimePckPolyFunction() {
    // Set the scales
    double dTime =  p_et / 86400.;   // seconds to days conversion
    double centTime = dTime / 36525; // seconds to Julian centuries conversion
    double secondsPerJulianCentury = 86400. * 36525.;

    // Calculate the quadratic part of the expression for the angles in degrees
    // Note: phi = ra + 90. and delta = 90 - dec for comparing results with older Isis 2 data
    Angle ra = m_raPole[0] + centTime*(m_raPole[1] + m_raPole[2]*centTime);
    Angle dec =  m_decPole[0] + centTime*(m_decPole[1] + m_decPole[2]*centTime);
    Angle pm = m_pm[0] + dTime*(m_pm[1] + m_pm[2]*dTime);
    Angle dra = (m_raPole[1] + 2.*m_raPole[2]*centTime) / secondsPerJulianCentury;
    Angle ddec = (m_decPole[1] + 2.*m_decPole[2]*centTime) / secondsPerJulianCentury;
    Angle dpm = (m_pm[1] + 2.*m_pm[2]*dTime) / 86400;
    
    // Now add the nutation/precession (trig part) adjustment to the expression if any
    int numNutPrec = (int) m_raNutPrec.size();
    Angle theta;
    double dtheta;
    double costheta;
    double sintheta;
    
    for (int ia = 0;  ia < numNutPrec; ia++) {
      theta = m_sysNutPrec0[ia] + m_sysNutPrec1[ia]*centTime;
      dtheta = m_sysNutPrec1[ia].degrees() * DEG2RAD;
      costheta = cos(theta.radians());
      sintheta = sin(theta.radians());
      ra += Angle(m_raNutPrec[ia] * sintheta, Angle::Degrees);
      dec += Angle(m_decNutPrec[ia] * costheta, Angle::Degrees);
      pm += Angle(m_pmNutPrec[ia] * sintheta, Angle::Degrees);
      dra += Angle(m_raNutPrec[ia] * dtheta / secondsPerJulianCentury * costheta, Angle::Degrees);
      ddec -= Angle(m_decNutPrec[ia] * dtheta / secondsPerJulianCentury * sintheta, Angle::Degrees);
      dpm += Angle(m_pmNutPrec[ia] * dtheta / secondsPerJulianCentury * costheta, Angle::Degrees);
    }

    // Get pm in correct range
    pm = Angle(fmod(pm.degrees(), 360), Angle::Degrees);  // Get pm back into the domain range

    if (ra.radians() < -1 * pi_c()) {
      ra += Angle(twopi_c(), Angle::Radians);
    }
    else if (ra.radians() > pi_c()) {
      ra -= Angle(twopi_c(), Angle::Radians);
    }

    if (pm.radians() < -1 * pi_c()) {
      pm += Angle(twopi_c(), Angle::Radians);
    }
    else if (pm.radians() > pi_c()) {
      pm -= Angle(twopi_c(), Angle::Radians);
    }

    // Convert to Euler angles to get the matrix
    SpiceDouble phi = ra.radians() + HALFPI;
    SpiceDouble delta = HALFPI - dec.radians();
    SpiceDouble w = pm.radians();
    SpiceDouble dphi = dra.radians();
    SpiceDouble ddelta = -ddec.radians();
    SpiceDouble dw = dpm.radians();

    // Load the Euler angles and their derivatives into an array and create the state matrix
    SpiceDouble angsDangs[6];
    SpiceDouble BJs[6][6];
    vpack_c(w, delta, phi, angsDangs);
    vpack_c(dw, ddelta, dphi, &angsDangs[3]); 
    eul2xf_c (angsDangs, p_axis3, p_axis2, p_axis1, BJs);

    // Decompose the state matrix to the rotation and its angular velocity
    xf2rav_c(BJs, (SpiceDouble( *)[3]) &p_CJ[0], (SpiceDouble *) &p_av[0]);
  }
}