UsgsAstroSarSensorModel.cpp

#include "UsgsAstroSarSensorModel.h"
#include "Utilities.h"

#include <string.h>
#include <cmath>
#include <functional>
#include <iomanip>

#include <nlohmann/json.hpp>

using json = nlohmann::json;
using namespace std;

#define MESSAGE_LOG(...)         \
  if (m_logger) {                \
    m_logger->info(__VA_ARGS__); \
  }

const string UsgsAstroSarSensorModel::_SENSOR_MODEL_NAME =
    "USGS_ASTRO_SAR_SENSOR_MODEL";
const int UsgsAstroSarSensorModel::NUM_PARAMETERS = 6;
const string UsgsAstroSarSensorModel::PARAMETER_NAME[] = {
    "X Pos. Bias   ",  // 0
    "Y Pos. Bias   ",  // 1
    "Z Pos. Bias   ",  // 2
    "X Vel. Bias   ",  // 3
    "Y Vel. Bias   ",  // 4
    "Z Vel. Bias   "   // 5
};

const int UsgsAstroSarSensorModel::NUM_PARAM_TYPES = 4;
const string UsgsAstroSarSensorModel::PARAM_STRING_ALL[] = {
    "NONE", "FICTITIOUS", "REAL", "FIXED"};
const csm::param::Type UsgsAstroSarSensorModel::PARAM_CHAR_ALL[] = {
    csm::param::NONE, csm::param::FICTITIOUS, csm::param::REAL,
    csm::param::FIXED};

string UsgsAstroSarSensorModel::constructStateFromIsd(
    const string imageSupportData, csm::WarningList* warnings) {
  MESSAGE_LOG("UsgsAstroSarSensorModel constructing state from ISD, with {}",
              imageSupportData);
  json isd = json::parse(imageSupportData);
  json state = {};

  csm::WarningList* parsingWarnings = new csm::WarningList;

  state["m_modelName"] = getSensorModelName(isd, parsingWarnings);
  state["m_imageIdentifier"] = getImageId(isd, parsingWarnings);
  state["m_sensorName"] = getSensorName(isd, parsingWarnings);
  state["m_platformName"] = getPlatformName(isd, parsingWarnings);

  state["m_nLines"] = getTotalLines(isd, parsingWarnings);
  state["m_nSamples"] = getTotalSamples(isd, parsingWarnings);

  // Zero computed state values
  state["m_referencePointXyz"] = vector<double>(3, 0.0);

  // sun_position and velocity are required for getIlluminationDirection
  state["m_sunPosition"] = getSunPositions(isd, parsingWarnings);
  state["m_sunVelocity"] = getSunVelocities(isd, parsingWarnings);

  state["m_centerEphemerisTime"] = getCenterTime(isd, parsingWarnings);
  state["m_startingEphemerisTime"] = getStartingTime(isd, parsingWarnings);
  state["m_endingEphemerisTime"] = getEndingTime(isd, parsingWarnings);

  state["m_exposureDuration"] = getExposureDuration(isd, parsingWarnings);

  try {
    state["m_dtEphem"] = isd.at("dt_ephemeris");
  } catch (...) {
    std::string msg = "dt_ephemeris not in ISD";
    MESSAGE_LOG(msg);
    parsingWarnings->push_back(
        csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, msg,
                     "UsgsAstroSarSensorModel::constructStateFromIsd()"));
  }

  try {
    state["m_t0Ephem"] = isd.at("t0_ephemeris");
  } catch (...) {
    std::string msg = "t0_ephemeris not in ISD";
    MESSAGE_LOG(msg);
    parsingWarnings->push_back(
        csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, msg,
                     "UsgsAstroSarSensorModel::constructStateFromIsd()"));
  }

  state["m_positions"] = getSensorPositions(isd, parsingWarnings);
  state["m_velocities"] = getSensorVelocities(isd, parsingWarnings);

  state["m_currentParameterValue"] = vector<double>(NUM_PARAMETERS, 0.0);

  // get radii
  state["m_minorAxis"] = getSemiMinorRadius(isd, parsingWarnings);
  state["m_majorAxis"] = getSemiMajorRadius(isd, parsingWarnings);

  // set identifiers
  state["m_platformIdentifier"] = getPlatformName(isd, parsingWarnings);
  state["m_sensorIdentifier"] = getSensorName(isd, parsingWarnings);

  // get reference_height
  state["m_minElevation"] = -1000;
  state["m_maxElevation"] = 1000;

  // SAR specific values
  state["m_scaledPixelWidth"] = getScaledPixelWidth(isd, parsingWarnings);
  state["m_scaleConversionCoefficients"] =
      getScaleConversionCoefficients(isd, parsingWarnings);
  state["m_scaleConversionTimes"] =
      getScaleConversionTimes(isd, parsingWarnings);
  state["m_wavelength"] = getWavelength(isd, parsingWarnings);
  state["m_lookDirection"] = getLookDirection(isd, parsingWarnings);

  // Default to identity covariance
  state["m_covariance"] = vector<double>(NUM_PARAMETERS * NUM_PARAMETERS, 0.0);
  for (int i = 0; i < NUM_PARAMETERS; i++) {
    state["m_covariance"][i * NUM_PARAMETERS + i] = 1.0;
  }

  if (!parsingWarnings->empty()) {
    if (warnings) {
      warnings->insert(warnings->end(), parsingWarnings->begin(),
                       parsingWarnings->end());
    }
    std::string message =
        "ISD is invalid for creating the sensor model with error [";
    csm::Warning warn = parsingWarnings->front();
    message += warn.getMessage();
    message += "]";
    parsingWarnings = nullptr;
    delete parsingWarnings;
    MESSAGE_LOG(message);
    throw csm::Error(csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, message,
                     "UsgsAstroSarSensorModel::constructStateFromIsd");
  }

  delete parsingWarnings;
  parsingWarnings = nullptr;

  // The state data will still be updated when a sensor model is created since
  // some state data is not in the ISD and requires a SM to compute them.
  return state.dump();
}

string UsgsAstroSarSensorModel::getModelNameFromModelState(
    const string& model_state) {
  MESSAGE_LOG("Getting model name from model state: {}", model_state);
  // Parse the string to JSON
  auto j = json::parse(model_state);
  // If model name cannot be determined, return a blank string
  string model_name;

  if (j.find("m_modelName") != j.end()) {
    model_name = j["m_modelName"];
  } else {
    csm::Error::ErrorType aErrorType = csm::Error::INVALID_SENSOR_MODEL_STATE;
    string aMessage = "No 'm_modelName' key in the model state object.";
    string aFunction = "UsgsAstroSarSensorModel::getModelNameFromModelState";
    MESSAGE_LOG(aMessage);
    csm::Error csmErr(aErrorType, aMessage, aFunction);
    throw(csmErr);
  }
  if (model_name != _SENSOR_MODEL_NAME) {
    csm::Error::ErrorType aErrorType = csm::Error::SENSOR_MODEL_NOT_SUPPORTED;
    string aMessage = "Sensor model not supported.";
    string aFunction = "UsgsAstroSarSensorModel::getModelNameFromModelState()";
    MESSAGE_LOG(aMessage);
    csm::Error csmErr(aErrorType, aMessage, aFunction);
    throw(csmErr);
  }
  return model_name;
}

UsgsAstroSarSensorModel::UsgsAstroSarSensorModel() {
  MESSAGE_LOG("Constructing UsgsAstroSarSensorModel");
  reset();
}

void UsgsAstroSarSensorModel::reset() {
  MESSAGE_LOG("Resetting UsgsAstroSarSensorModel");
  m_imageIdentifier = "Unknown";
  m_sensorName = "Unknown";
  m_platformIdentifier = "Unknown";
  m_nLines = 0;
  m_nSamples = 0;
  m_exposureDuration = 0;
  m_scaledPixelWidth = 0;
  m_lookDirection = LookDirection::LEFT;
  m_startingEphemerisTime = 0;
  m_centerEphemerisTime = 0;
  m_endingEphemerisTime = 0;
  m_majorAxis = 0;
  m_minorAxis = 0;
  m_platformIdentifier = "Unknown";
  m_sensorIdentifier = "Unknown";
  m_trajectoryIdentifier = "Unknown";
  m_collectionIdentifier = "Unknown";
  m_minElevation = -1000;
  m_maxElevation = 1000;
  m_dtEphem = 0;
  m_t0Ephem = 0;
  m_scaleConversionCoefficients.clear();
  m_scaleConversionTimes.clear();
  m_positions.clear();
  m_velocities.clear();
  m_currentParameterValue = vector<double>(NUM_PARAMETERS, 0.0);
  m_parameterType = vector<csm::param::Type>(NUM_PARAMETERS, csm::param::REAL);
  m_referencePointXyz.x = 0.0;
  m_referencePointXyz.y = 0.0;
  m_referencePointXyz.z = 0.0;
  m_covariance = vector<double>(NUM_PARAMETERS * NUM_PARAMETERS, 0.0);
  m_sunPosition.clear();
  m_sunVelocity.clear();
  m_wavelength = 0;
  m_noAdjustments = std::vector<double>(NUM_PARAMETERS, 0.0);
}

void UsgsAstroSarSensorModel::replaceModelState(const string& argState) {
  reset();

  MESSAGE_LOG("Replacing model state with: {}", argState);
  auto stateJson = json::parse(argState);

  m_imageIdentifier = stateJson["m_imageIdentifier"].get<string>();
  m_platformIdentifier = stateJson["m_platformIdentifier"].get<string>();
  m_sensorName = stateJson["m_sensorName"].get<string>();
  m_nLines = stateJson["m_nLines"];
  m_nSamples = stateJson["m_nSamples"];
  m_exposureDuration = stateJson["m_exposureDuration"];
  m_scaledPixelWidth = stateJson["m_scaledPixelWidth"];
  std::string lookStr = stateJson["m_lookDirection"];
  if (lookStr.compare("right") == 0) {
    m_lookDirection = UsgsAstroSarSensorModel::RIGHT;
  } else if (lookStr.compare("left") == 0) {
    m_lookDirection = UsgsAstroSarSensorModel::LEFT;
  } else {
    std::string message = "Could not determine look direction from state";
    MESSAGE_LOG(message);
    throw csm::Error(csm::Error::INVALID_SENSOR_MODEL_STATE, message,
                     "UsgsAstroSarSensorModel::replaceModelState");
  }
  m_wavelength = stateJson["m_wavelength"];
  m_startingEphemerisTime = stateJson["m_startingEphemerisTime"];
  m_centerEphemerisTime = stateJson["m_centerEphemerisTime"];
  m_endingEphemerisTime = stateJson["m_endingEphemerisTime"];
  m_majorAxis = stateJson["m_majorAxis"];
  m_minorAxis = stateJson["m_minorAxis"];
  m_platformIdentifier = stateJson["m_platformIdentifier"].get<string>();
  m_sensorIdentifier = stateJson["m_sensorIdentifier"].get<string>();
  m_minElevation = stateJson["m_minElevation"];
  m_maxElevation = stateJson["m_maxElevation"];
  m_dtEphem = stateJson["m_dtEphem"];
  m_t0Ephem = stateJson["m_t0Ephem"];
  m_scaleConversionCoefficients =
      stateJson["m_scaleConversionCoefficients"].get<vector<double>>();
  m_scaleConversionTimes =
      stateJson["m_scaleConversionTimes"].get<vector<double>>();
  m_positions = stateJson["m_positions"].get<vector<double>>();
  m_velocities = stateJson["m_velocities"].get<vector<double>>();
  m_currentParameterValue =
      stateJson["m_currentParameterValue"].get<vector<double>>();
  m_referencePointXyz.x = stateJson["m_referencePointXyz"][0];
  m_referencePointXyz.y = stateJson["m_referencePointXyz"][1];
  m_referencePointXyz.z = stateJson["m_referencePointXyz"][2];
  m_covariance = stateJson["m_covariance"].get<vector<double>>();
  m_sunPosition = stateJson["m_sunPosition"].get<vector<double>>();
  m_sunVelocity = stateJson["m_sunVelocity"].get<vector<double>>();

  // If sensor model is being created for the first time, this routine will set
  // the reference point
  if (m_referencePointXyz.x == 0 && m_referencePointXyz.y == 0 &&
      m_referencePointXyz.z == 0) {
    MESSAGE_LOG("Updating State")

    double lineCtr = m_nLines / 2.0;
    double sampCtr = m_nSamples / 2.0;
    csm::ImageCoord ip(lineCtr, sampCtr);
    MESSAGE_LOG("updateState: center image coordinate set to {} {}", lineCtr,
                sampCtr)

    double refHeight = 0;
    m_referencePointXyz = imageToGround(ip, refHeight);
    MESSAGE_LOG("updateState: reference point (x, y, z) {} {} {}",
                m_referencePointXyz.x, m_referencePointXyz.y,
                m_referencePointXyz.z)
  }
}

string UsgsAstroSarSensorModel::getModelState() const {
  json state = {};

  state["m_modelName"] = _SENSOR_MODEL_NAME;
  state["m_imageIdentifier"] = m_imageIdentifier;
  state["m_sensorName"] = m_sensorName;
  state["m_platformName"] = m_platformName;
  state["m_nLines"] = m_nLines;
  state["m_nSamples"] = m_nSamples;
  state["m_referencePointXyz"] = {m_referencePointXyz.x, m_referencePointXyz.y,
                                  m_referencePointXyz.z};
  state["m_sunPosition"] = m_sunPosition;
  state["m_sunVelocity"] = m_sunVelocity;
  state["m_centerEphemerisTime"] = m_centerEphemerisTime;
  state["m_startingEphemerisTime"] = m_startingEphemerisTime;
  state["m_endingEphemerisTime"] = m_endingEphemerisTime;
  state["m_exposureDuration"] = m_exposureDuration;
  state["m_dtEphem"] = m_dtEphem;
  state["m_t0Ephem"] = m_t0Ephem;
  state["m_positions"] = m_positions;
  state["m_velocities"] = m_velocities;
  state["m_currentParameterValue"] = m_currentParameterValue;
  state["m_minorAxis"] = m_minorAxis;
  state["m_majorAxis"] = m_majorAxis;
  state["m_platformIdentifier"] = m_platformIdentifier;
  state["m_sensorIdentifier"] = m_sensorIdentifier;
  state["m_minElevation"] = m_minElevation;
  state["m_maxElevation"] = m_maxElevation;
  state["m_scaledPixelWidth"] = m_scaledPixelWidth;
  if (m_lookDirection == 0) {
    state["m_lookDirection"] = "left";
  } else if (m_lookDirection == 1) {
    state["m_lookDirection"] = "right";
  } else {
    std::string message = "Could not parse look direction from json state.";
    MESSAGE_LOG(message);
    throw csm::Error(csm::Error::INVALID_SENSOR_MODEL_STATE, message,
                     "UsgsAstroSarSensorModel::getModelState");
  }
  state["m_wavelength"] = m_wavelength;
  state["m_scaleConversionCoefficients"] = m_scaleConversionCoefficients;
  state["m_scaleConversionTimes"] = m_scaleConversionTimes;
  state["m_covariance"] = m_covariance;

  return state.dump();
}

csm::ImageCoord UsgsAstroSarSensorModel::groundToImage(
    const csm::EcefCoord& groundPt, double desiredPrecision,
    double* achievedPrecision, csm::WarningList* warnings) const {
  MESSAGE_LOG(
      "Computing groundToImage(ImageCoord) for {}, {}, {}, with desired "
      "precision {}"
      "No adjustments.",
      groundPt.x, groundPt.y, groundPt.z, desiredPrecision);

  csm::ImageCoord imagePt = groundToImage(
      groundPt, m_noAdjustments, desiredPrecision, achievedPrecision, warnings);
  return imagePt;
}

csm::ImageCoord UsgsAstroSarSensorModel::groundToImage(
    const csm::EcefCoord& groundPt, const std::vector<double> adj,
    double desiredPrecision, double* achievedPrecision,
    csm::WarningList* warnings) const {
  MESSAGE_LOG(
      "Computing groundToImage(ImageCoord) for {}, {}, {}, with desired "
      "precision {}, and "
      "adjustments.",
      groundPt.x, groundPt.y, groundPt.z, desiredPrecision);

  // Find time of closest approach to groundPt and the corresponding slant range
  // by finding the root of the doppler shift frequency
  try {
    double timeTolerance = m_exposureDuration * desiredPrecision / 2.0;

    double time = dopplerShift(groundPt, timeTolerance, adj);
    double slantRangeValue = slantRange(groundPt, time, adj);

    // Find the ground range, based on the ground-range-to-slant-range
    // polynomial defined by the range coefficient set, with a time closest to
    // the calculated time of closest approach
    double groundTolerance = m_scaledPixelWidth * desiredPrecision / 2.0;
    double groundRange = slantRangeToGroundRange(
        groundPt, time, slantRangeValue, groundTolerance);

    double line = (time - m_startingEphemerisTime) / m_exposureDuration + 0.5;
    double sample = groundRange / m_scaledPixelWidth;
    return csm::ImageCoord(line, sample);
  } catch (std::exception& error) {
    std::string message = "Could not calculate groundToImage, with error [";
    message += error.what();
    message += "]";
    MESSAGE_LOG(message);
    throw csm::Error(csm::Error::UNKNOWN_ERROR, message, "groundToImage");
  }
}

// Calculate the root
double UsgsAstroSarSensorModel::dopplerShift(
    csm::EcefCoord groundPt, double tolerance,
    const std::vector<double> adj) const {
  MESSAGE_LOG("Calculating doppler shift with: {}, {}, {}, and tolerance {}.",
              groundPt.x, groundPt.y, groundPt.z, tolerance);
  csm::EcefVector groundVec(groundPt.x, groundPt.y, groundPt.z);
  std::function<double(double)> dopplerShiftFunction = [this, groundVec,
                                                        adj](double time) {
    csm::EcefVector spacecraftPosition =
        getAdjustedSpacecraftPosition(time, adj);
    csm::EcefVector spacecraftVelocity = getAdjustedSensorVelocity(time, adj);
    csm::EcefVector lookVector = spacecraftPosition - groundVec;

    double slantRange = norm(lookVector);

    double dopplerShift = -2.0 * dot(lookVector, spacecraftVelocity) /
                          (slantRange * m_wavelength);

    return dopplerShift;
  };

  // Do root-finding for "dopplerShift"
  double timePadding = m_exposureDuration * m_nLines * 0.25;
  return brentRoot(m_startingEphemerisTime - timePadding,
                   m_endingEphemerisTime + timePadding, dopplerShiftFunction,
                   tolerance);
}

double UsgsAstroSarSensorModel::slantRange(csm::EcefCoord surfPt, double time,
                                           std::vector<double> adj) const {
  MESSAGE_LOG("Calculating slant range with: {}, {}, {}, and time {}.",
              surfPt.x, surfPt.y, surfPt.z, time);
  csm::EcefVector surfVec(surfPt.x, surfPt.y, surfPt.z);
  csm::EcefVector spacecraftPosition = getAdjustedSpacecraftPosition(time, adj);
  return norm(spacecraftPosition - surfVec);
}

double UsgsAstroSarSensorModel::slantRangeToGroundRange(
    const csm::EcefCoord& groundPt, double time, double slantRange,
    double tolerance) const {
  MESSAGE_LOG(
      "Calculating slant range to ground range with: {}, {}, {}, {}, {}, {}",
      groundPt.x, groundPt.y, groundPt.z, time, slantRange, tolerance);

  std::vector<double> coeffs = getRangeCoefficients(time);

  // Need to come up with an initial guess when solving for ground
  // range given slant range. Naively use the middle of the image.
  double guess = 0.5 * m_nSamples * m_scaledPixelWidth;

  // Tolerance to 1/20th of a pixel for now.
  coeffs[0] -= slantRange;
  return polynomialRoot(coeffs, guess, tolerance);
}

double UsgsAstroSarSensorModel::groundRangeToSlantRange(
    double groundRange, const std::vector<double>& coeffs) const {
  return evaluatePolynomial(coeffs, groundRange);
}

csm::ImageCoordCovar UsgsAstroSarSensorModel::groundToImage(
    const csm::EcefCoordCovar& groundPt, double desiredPrecision,
    double* achievedPrecision, csm::WarningList* warnings) const {
  MESSAGE_LOG("Calculating groundToImage with: {}, {}, {}, {}", groundPt.x,
              groundPt.y, groundPt.z, desiredPrecision);
  // Ground to image with error propagation
  // Compute corresponding image point
  csm::EcefCoord gp(groundPt);

  csm::ImageCoord ip =
      groundToImage(gp, desiredPrecision, achievedPrecision, warnings);
  csm::ImageCoordCovar result(ip.line, ip.samp);

  // Compute partials ls wrt XYZ
  std::vector<double> prt(6, 0.0);
  prt = computeGroundPartials(groundPt);

  // Error propagation
  double ltx, lty, ltz;
  double stx, sty, stz;
  ltx = prt[0] * groundPt.covariance[0] + prt[1] * groundPt.covariance[3] +
        prt[2] * groundPt.covariance[6];
  lty = prt[0] * groundPt.covariance[1] + prt[1] * groundPt.covariance[4] +
        prt[2] * groundPt.covariance[7];
  ltz = prt[0] * groundPt.covariance[2] + prt[1] * groundPt.covariance[5] +
        prt[2] * groundPt.covariance[8];
  stx = prt[3] * groundPt.covariance[0] + prt[4] * groundPt.covariance[3] +
        prt[5] * groundPt.covariance[6];
  sty = prt[3] * groundPt.covariance[1] + prt[4] * groundPt.covariance[4] +
        prt[5] * groundPt.covariance[7];
  stz = prt[3] * groundPt.covariance[2] + prt[4] * groundPt.covariance[5] +
        prt[5] * groundPt.covariance[8];

  double gp_cov[4];  // Input gp cov in image space
  gp_cov[0] = ltx * prt[0] + lty * prt[1] + ltz * prt[2];
  gp_cov[1] = ltx * prt[3] + lty * prt[4] + ltz * prt[5];
  gp_cov[2] = stx * prt[0] + sty * prt[1] + stz * prt[2];
  gp_cov[3] = stx * prt[3] + sty * prt[4] + stz * prt[5];

  std::vector<double> unmodeled_cov = getUnmodeledError(ip);
  double sensor_cov[4];  // sensor cov in image space
  determineSensorCovarianceInImageSpace(gp, sensor_cov);

  result.covariance[0] = gp_cov[0] + unmodeled_cov[0] + sensor_cov[0];
  result.covariance[1] = gp_cov[1] + unmodeled_cov[1] + sensor_cov[1];
  result.covariance[2] = gp_cov[2] + unmodeled_cov[2] + sensor_cov[2];
  result.covariance[3] = gp_cov[3] + unmodeled_cov[3] + sensor_cov[3];

  return result;
}

csm::EcefCoord UsgsAstroSarSensorModel::imageToGround(
    const csm::ImageCoord& imagePt, double height, double desiredPrecision,
    double* achievedPrecision, csm::WarningList* warnings) const {
  MESSAGE_LOG("Calculating imageToGround with: {}, {}, {}, {}", imagePt.samp,
              imagePt.line, height, desiredPrecision);
  double time =
      m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration;
  double groundRange = imagePt.samp * m_scaledPixelWidth;
  std::vector<double> coeffs = getRangeCoefficients(time);
  double slantRange = groundRangeToSlantRange(groundRange, coeffs);

  // Compute the in-track, cross-track, nadir, coordinate system to solve in
  csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
  double positionMag = norm(spacecraftPosition);
  csm::EcefVector spacecraftVelocity = getSensorVelocity(time);
  // In-track unit vector
  csm::EcefVector vHat = normalized(spacecraftVelocity);
  // Nadir unit vector
  csm::EcefVector tHat = normalized(rejection(spacecraftPosition, vHat));
  // Cross-track unit vector
  csm::EcefVector uHat = cross(vHat, tHat);

  // Compute the spacecraft position in the new coordinate system
  //   The cross-track unit vector is orthogonal to the position so we ignore it
  double nadirComp = dot(spacecraftPosition, tHat);

  // Iterate to find proper radius value
  double pointRadius = m_majorAxis + height;
  double radiusSqr;
  double pointHeight;
  csm::EcefVector groundVec;
  do {
    radiusSqr = pointRadius * pointRadius;
    double alpha =
        (radiusSqr - slantRange * slantRange - positionMag * positionMag) /
        (2 * nadirComp);
    double beta = sqrt(slantRange * slantRange - alpha * alpha);
    if (m_lookDirection == LEFT) {
      beta *= -1;
    }
    groundVec = alpha * tHat + beta * uHat + spacecraftPosition;
    pointHeight = computeEllipsoidElevation(
        groundVec.x, groundVec.y, groundVec.z, m_majorAxis, m_minorAxis);
    pointRadius -= (pointHeight - height);
  } while (fabs(pointHeight - height) > desiredPrecision);

  csm::EcefCoord groundPt(groundVec.x, groundVec.y, groundVec.z);

  return groundPt;
}

csm::EcefCoordCovar UsgsAstroSarSensorModel::imageToGround(
    const csm::ImageCoordCovar& imagePt, double height, double heightVariance,
    double desiredPrecision, double* achievedPrecision,
    csm::WarningList* warnings) const {
  MESSAGE_LOG("Calculating imageToGroundWith: {}, {}, {}, {}, {}", imagePt.samp,
              imagePt.line, height, heightVariance, desiredPrecision);
  // Image to ground with error propagation
  // Use numerical partials
  const double DELTA_IMAGE = 1.0;
  const double DELTA_GROUND = m_scaledPixelWidth;
  csm::ImageCoord ip(imagePt.line, imagePt.samp);

  csm::EcefCoord gp =
      imageToGround(ip, height, desiredPrecision, achievedPrecision, warnings);

  // Compute numerical partials xyz wrt to lsh
  ip.line = imagePt.line + DELTA_IMAGE;
  ip.samp = imagePt.samp;
  csm::EcefCoord gpl = imageToGround(ip, height, desiredPrecision);
  double xpl = (gpl.x - gp.x) / DELTA_IMAGE;
  double ypl = (gpl.y - gp.y) / DELTA_IMAGE;
  double zpl = (gpl.z - gp.z) / DELTA_IMAGE;

  ip.line = imagePt.line;
  ip.samp = imagePt.samp + DELTA_IMAGE;
  csm::EcefCoord gps = imageToGround(ip, height, desiredPrecision);
  double xps = (gps.x - gp.x) / DELTA_IMAGE;
  double yps = (gps.y - gp.y) / DELTA_IMAGE;
  double zps = (gps.z - gp.z) / DELTA_IMAGE;

  ip.line = imagePt.line;
  ip.samp = imagePt.samp;  // +DELTA_IMAGE;
  csm::EcefCoord gph =
      imageToGround(ip, height + DELTA_GROUND, desiredPrecision);
  double xph = (gph.x - gp.x) / DELTA_GROUND;
  double yph = (gph.y - gp.y) / DELTA_GROUND;
  double zph = (gph.z - gp.z) / DELTA_GROUND;

  // Convert sensor covariance to image space
  double sCov[4];
  determineSensorCovarianceInImageSpace(gp, sCov);

  std::vector<double> unmod = getUnmodeledError(imagePt);

  double iCov[4];
  iCov[0] = imagePt.covariance[0] + sCov[0] + unmod[0];
  iCov[1] = imagePt.covariance[1] + sCov[1] + unmod[1];
  iCov[2] = imagePt.covariance[2] + sCov[2] + unmod[2];
  iCov[3] = imagePt.covariance[3] + sCov[3] + unmod[3];

  // Temporary matrix product
  double t00, t01, t02, t10, t11, t12, t20, t21, t22;
  t00 = xpl * iCov[0] + xps * iCov[2];
  t01 = xpl * iCov[1] + xps * iCov[3];
  t02 = xph * heightVariance;
  t10 = ypl * iCov[0] + yps * iCov[2];
  t11 = ypl * iCov[1] + yps * iCov[3];
  t12 = yph * heightVariance;
  t20 = zpl * iCov[0] + zps * iCov[2];
  t21 = zpl * iCov[1] + zps * iCov[3];
  t22 = zph * heightVariance;

  // Ground covariance
  csm::EcefCoordCovar result;
  result.x = gp.x;
  result.y = gp.y;
  result.z = gp.z;

  result.covariance[0] = t00 * xpl + t01 * xps + t02 * xph;
  result.covariance[1] = t00 * ypl + t01 * yps + t02 * yph;
  result.covariance[2] = t00 * zpl + t01 * zps + t02 * zph;
  result.covariance[3] = t10 * xpl + t11 * xps + t12 * xph;
  result.covariance[4] = t10 * ypl + t11 * yps + t12 * yph;
  result.covariance[5] = t10 * zpl + t11 * zps + t12 * zph;
  result.covariance[6] = t20 * xpl + t21 * xps + t22 * xph;
  result.covariance[7] = t20 * ypl + t21 * yps + t22 * yph;
  result.covariance[8] = t20 * zpl + t21 * zps + t22 * zph;

  return result;
}

csm::EcefLocus UsgsAstroSarSensorModel::imageToProximateImagingLocus(
    const csm::ImageCoord& imagePt, const csm::EcefCoord& groundPt,
    double desiredPrecision, double* achievedPrecision,
    csm::WarningList* warnings) const {
  // Compute the slant range
  double time =
      m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration;
  double groundRange = imagePt.samp * m_scaledPixelWidth;
  std::vector<double> coeffs = getRangeCoefficients(time);
  double slantRange = groundRangeToSlantRange(groundRange, coeffs);

  // Project the sensor to ground point vector onto the 0 doppler plane
  // then compute the closest point at the slant range to that
  csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
  csm::EcefVector spacecraftVelocity = getSensorVelocity(time);
  csm::EcefVector groundVec(groundPt.x, groundPt.y, groundPt.z);
  csm::EcefVector lookVec =
      normalized(rejection(groundVec - spacecraftPosition, spacecraftVelocity));
  csm::EcefVector closestVec = spacecraftPosition + slantRange * lookVec;

  // Compute the tangent at the closest point
  csm::EcefVector tangent;
  if (m_lookDirection == LEFT) {
    tangent = cross(spacecraftVelocity, lookVec);
  } else {
    tangent = cross(lookVec, spacecraftVelocity);
  }
  tangent = normalized(tangent);

  return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, tangent.x,
                        tangent.y, tangent.z);
}

csm::EcefLocus UsgsAstroSarSensorModel::imageToRemoteImagingLocus(
    const csm::ImageCoord& imagePt, double desiredPrecision,
    double* achievedPrecision, csm::WarningList* warnings) const {
  // Compute the slant range
  double time =
      m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration;
  double groundRange = imagePt.samp * m_scaledPixelWidth;
  std::vector<double> coeffs = getRangeCoefficients(time);
  double slantRange = groundRangeToSlantRange(groundRange, coeffs);

  // Project the negative sensor position vector onto the 0 doppler plane
  // then compute the closest point at the slant range to that
  csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
  csm::EcefVector spacecraftVelocity = getSensorVelocity(time);
  csm::EcefVector lookVec =
      normalized(rejection(-1 * spacecraftPosition, spacecraftVelocity));
  csm::EcefVector closestVec = spacecraftPosition + slantRange * lookVec;

  // Compute the tangent at the closest point
  csm::EcefVector tangent;
  if (m_lookDirection == LEFT) {
    tangent = cross(spacecraftVelocity, lookVec);
  } else {
    tangent = cross(lookVec, spacecraftVelocity);
  }
  tangent = normalized(tangent);

  return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, tangent.x,
                        tangent.y, tangent.z);
}

csm::ImageCoord UsgsAstroSarSensorModel::getImageStart() const {
  return csm::ImageCoord(0.0, 0.0);
}

csm::ImageVector UsgsAstroSarSensorModel::getImageSize() const {
  return csm::ImageVector(m_nLines, m_nSamples);
}

pair<csm::ImageCoord, csm::ImageCoord>
UsgsAstroSarSensorModel::getValidImageRange() const {
  csm::ImageCoord start = getImageStart();
  csm::ImageVector size = getImageSize();
  return make_pair(
      start, csm::ImageCoord(start.line + size.line, start.samp + size.samp));
}

pair<double, double> UsgsAstroSarSensorModel::getValidHeightRange() const {
  return make_pair(m_minElevation, m_maxElevation);
}

csm::EcefVector UsgsAstroSarSensorModel::getIlluminationDirection(
    const csm::EcefCoord& groundPt) const {
  csm::EcefVector groundVec(groundPt.x, groundPt.y, groundPt.z);
  csm::EcefVector sunPosition =
      getSunPosition(getImageTime(groundToImage(groundPt)));
  csm::EcefVector illuminationDirection = normalized(groundVec - sunPosition);
  return illuminationDirection;
}

double UsgsAstroSarSensorModel::getImageTime(
    const csm::ImageCoord& imagePt) const {
  return m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration;
}

csm::EcefVector UsgsAstroSarSensorModel::getSpacecraftPosition(
    double time) const {
  MESSAGE_LOG("getSpacecraftPosition at {} without adjustments", time)
  csm::EcefCoord spacecraftPosition = getSensorPosition(time);
  return csm::EcefVector(spacecraftPosition.x, spacecraftPosition.y,
                         spacecraftPosition.z);
}

csm::EcefVector UsgsAstroSarSensorModel::getAdjustedSpacecraftPosition(
    double time, std::vector<double> adj) const {
  MESSAGE_LOG("getSpacecraftPosition at {} with adjustments", time)
  csm::EcefCoord spacecraftPosition = getAdjustedSensorPosition(time, adj);
  return csm::EcefVector(spacecraftPosition.x, spacecraftPosition.y,
                         spacecraftPosition.z);
}

csm::EcefCoord UsgsAstroSarSensorModel::getSensorPosition(double time) const {
  MESSAGE_LOG("getSensorPosition at {}.", time)
  csm::EcefCoord sensorPosition =
      getAdjustedSensorPosition(time, m_noAdjustments);
  return sensorPosition;
}

csm::EcefCoord UsgsAstroSarSensorModel::getSensorPosition(
    const csm::ImageCoord& imagePt) const {
  MESSAGE_LOG("getSensorPosition at {}, {}.", imagePt.samp, imagePt.line);
  double time = getImageTime(imagePt);
  return getSensorPosition(time);
}

csm::EcefCoord UsgsAstroSarSensorModel::getAdjustedSensorPosition(
    double time, std::vector<double> adj) const {
  MESSAGE_LOG("getSensorPosition at {}. With adjustments: {}.", time);
  int numPositions = m_positions.size();
  csm::EcefVector spacecraftPosition = csm::EcefVector();

  // If there are multiple positions, use Lagrange interpolation
  if ((numPositions / 3) > 1) {
    double position[3];
    lagrangeInterp(numPositions / 3, &m_positions[0], m_t0Ephem, m_dtEphem,
                   time, 3, 8, position);
    spacecraftPosition.x = position[0] + getValue(0, adj);
    spacecraftPosition.y = position[1] + getValue(1, adj);
    spacecraftPosition.z = position[2] + getValue(2, adj);
  } else {
    spacecraftPosition.x = m_positions[0] + getValue(0, adj);
    spacecraftPosition.y = m_positions[1] + getValue(1, adj);
    spacecraftPosition.z = m_positions[2] + getValue(2, adj);
  }
  return csm::EcefCoord(spacecraftPosition.x, spacecraftPosition.y,
                        spacecraftPosition.z);
}

csm::EcefVector UsgsAstroSarSensorModel::getSensorVelocity(
    const csm::ImageCoord& imagePt) const {
  MESSAGE_LOG("getSensorVelocity at {}, {}. No adjustments.", imagePt.samp,
              imagePt.line);
  double time = getImageTime(imagePt);
  return getSensorVelocity(time);
}

csm::EcefVector UsgsAstroSarSensorModel::getSensorVelocity(double time) const {
  MESSAGE_LOG("getSensorVelocity at {}. Without adjustments.", time);
  csm::EcefVector spacecraftVelocity =
      getAdjustedSensorVelocity(time, m_noAdjustments);
  return spacecraftVelocity;
}

csm::EcefVector UsgsAstroSarSensorModel::getAdjustedSensorVelocity(
    double time, std::vector<double> adj) const {
  MESSAGE_LOG("getSensorVelocity at {}. With adjustments.", time);
  int numVelocities = m_velocities.size();
  csm::EcefVector spacecraftVelocity = csm::EcefVector();

  // If there are multiple positions, use Lagrange interpolation
  if ((numVelocities / 3) > 1) {
    double velocity[3];
    lagrangeInterp(numVelocities / 3, &m_velocities[0], m_t0Ephem, m_dtEphem,
                   time, 3, 8, velocity);
    spacecraftVelocity.x = velocity[0] + getValue(3, adj);
    spacecraftVelocity.y = velocity[1] + getValue(4, adj);
    spacecraftVelocity.z = velocity[2] + getValue(5, adj);
  } else {
    spacecraftVelocity.x = m_velocities[0] + getValue(3, adj);
    spacecraftVelocity.y = m_velocities[1] + getValue(4, adj);
    spacecraftVelocity.z = m_velocities[2] + getValue(5, adj);
  }
  return spacecraftVelocity;
}

csm::RasterGM::SensorPartials UsgsAstroSarSensorModel::computeSensorPartials(
    int index, const csm::EcefCoord& groundPt, double desiredPrecision,
    double* achievedPrecision, csm::WarningList* warnings) const {
  MESSAGE_LOG(
      "Calculating computeSensorPartials for ground point {}, {}, {} with "
      "desired precision {}",
      groundPt.x, groundPt.y, groundPt.z, desiredPrecision)

  // Compute image coordinate first
  csm::ImageCoord imgPt =
      groundToImage(groundPt, desiredPrecision, achievedPrecision);

  // Call overloaded function
  return computeSensorPartials(index, imgPt, groundPt, desiredPrecision,
                               achievedPrecision, warnings);
}

csm::RasterGM::SensorPartials UsgsAstroSarSensorModel::computeSensorPartials(
    int index, const csm::ImageCoord& imagePt, const csm::EcefCoord& groundPt,
    double desiredPrecision, double* achievedPrecision,
    csm::WarningList* warnings) const {
  MESSAGE_LOG(
      "Calculating computeSensorPartials (with image points {}, {}) for ground "
      "point {}, {}, "
      "{} with desired precision {}",
      imagePt.line, imagePt.samp, groundPt.x, groundPt.y, groundPt.z,
      desiredPrecision)

  // Compute numerical partials wrt specific parameter

  const double DELTA = m_scaledPixelWidth;
  std::vector<double> adj(UsgsAstroSarSensorModel::NUM_PARAMETERS, 0.0);
  adj[index] = DELTA;

  csm::ImageCoord adjImgPt = groundToImage(groundPt, adj, desiredPrecision,
                                           achievedPrecision, warnings);

  double linePartial = (adjImgPt.line - imagePt.line) / DELTA;
  double samplePartial = (adjImgPt.samp - imagePt.samp) / DELTA;

  return csm::RasterGM::SensorPartials(linePartial, samplePartial);
}

vector<double> UsgsAstroSarSensorModel::computeGroundPartials(
    const csm::EcefCoord& groundPt) const {
  double GND_DELTA = m_scaledPixelWidth;
  // Partial of line, sample wrt X, Y, Z
  double x = groundPt.x;
  double y = groundPt.y;
  double z = groundPt.z;

  csm::ImageCoord ipB = groundToImage(groundPt);
  csm::ImageCoord ipX = groundToImage(csm::EcefCoord(x + GND_DELTA, y, z));
  csm::ImageCoord ipY = groundToImage(csm::EcefCoord(x, y + GND_DELTA, z));
  csm::ImageCoord ipZ = groundToImage(csm::EcefCoord(x, y, z + GND_DELTA));

  std::vector<double> partials(6, 0.0);
  partials[0] = (ipX.line - ipB.line) / GND_DELTA;
  partials[3] = (ipX.samp - ipB.samp) / GND_DELTA;
  partials[1] = (ipY.line - ipB.line) / GND_DELTA;
  partials[4] = (ipY.samp - ipB.samp) / GND_DELTA;
  partials[2] = (ipZ.line - ipB.line) / GND_DELTA;
  partials[5] = (ipZ.samp - ipB.samp) / GND_DELTA;

  return partials;
}

const csm::CorrelationModel& UsgsAstroSarSensorModel::getCorrelationModel()
    const {
  return _NO_CORR_MODEL;
}

vector<double> UsgsAstroSarSensorModel::getUnmodeledCrossCovariance(
    const csm::ImageCoord& pt1, const csm::ImageCoord& pt2) const {
  return vector<double>(4, 0.0);
}

csm::EcefCoord UsgsAstroSarSensorModel::getReferencePoint() const {
  return m_referencePointXyz;
}

void UsgsAstroSarSensorModel::setReferencePoint(
    const csm::EcefCoord& groundPt) {
  m_referencePointXyz = groundPt;
}

int UsgsAstroSarSensorModel::getNumParameters() const { return NUM_PARAMETERS; }

string UsgsAstroSarSensorModel::getParameterName(int index) const {
  return PARAMETER_NAME[index];
}

string UsgsAstroSarSensorModel::getParameterUnits(int index) const {
  return "m";
}

bool UsgsAstroSarSensorModel::hasShareableParameters() const { return false; }

bool UsgsAstroSarSensorModel::isParameterShareable(int index) const {
  return false;
}

csm::SharingCriteria UsgsAstroSarSensorModel::getParameterSharingCriteria(
    int index) const {
  return csm::SharingCriteria();
}

double UsgsAstroSarSensorModel::getParameterValue(int index) const {
  return m_currentParameterValue[index];
}

void UsgsAstroSarSensorModel::setParameterValue(int index, double value) {
  m_currentParameterValue[index] = value;
}

csm::param::Type UsgsAstroSarSensorModel::getParameterType(int index) const {
  return m_parameterType[index];
}

void UsgsAstroSarSensorModel::setParameterType(int index,
                                               csm::param::Type pType) {
  m_parameterType[index] = pType;
}

double UsgsAstroSarSensorModel::getParameterCovariance(int index1,
                                                       int index2) const {
  return m_covariance[index1 * NUM_PARAMETERS + index2];
}

void UsgsAstroSarSensorModel::setParameterCovariance(int index1, int index2,
                                                     double covariance)

{
  m_covariance[index1 * NUM_PARAMETERS + index2] = covariance;
}

int UsgsAstroSarSensorModel::getNumGeometricCorrectionSwitches() const {
  return 0;
}

string UsgsAstroSarSensorModel::getGeometricCorrectionName(int index) const {
  throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.",
                   "UsgsAstroSarSensorModel::getGeometricCorrectionName");
}

void UsgsAstroSarSensorModel::setGeometricCorrectionSwitch(
    int index, bool value, csm::param::Type pType) {
  throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.",
                   "UsgsAstroSarSensorModel::setGeometricCorrectionSwitch");
}

bool UsgsAstroSarSensorModel::getGeometricCorrectionSwitch(int index) const {
  throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.",
                   "UsgsAstroSarSensorModel::getGeometricCorrectionSwitch");
}

vector<double> UsgsAstroSarSensorModel::getCrossCovarianceMatrix(
    const csm::GeometricModel& comparisonModel, csm::param::Set pSet,
    const csm::GeometricModel::GeometricModelList& otherModels) const {
  // Return covariance matrix
  if (&comparisonModel == this) {
    vector<int> paramIndices = getParameterSetIndices(pSet);
    int numParams = paramIndices.size();
    vector<double> covariances(numParams * numParams, 0.0);
    for (int i = 0; i < numParams; i++) {
      for (int j = 0; j < numParams; j++) {
        covariances[i * numParams + j] =
            getParameterCovariance(paramIndices[i], paramIndices[j]);
      }
    }
    return covariances;
  }
  // No correlation between models.
  const vector<int>& indices = getParameterSetIndices(pSet);
  size_t num_rows = indices.size();
  const vector<int>& indices2 = comparisonModel.getParameterSetIndices(pSet);
  size_t num_cols = indices.size();

  return vector<double>(num_rows * num_cols, 0.0);
}

csm::Version UsgsAstroSarSensorModel::getVersion() const {
  return csm::Version(1, 0, 0);
}

string UsgsAstroSarSensorModel::getModelName() const {
  return _SENSOR_MODEL_NAME;
}

string UsgsAstroSarSensorModel::getPedigree() const { return "USGS_SAR"; }

string UsgsAstroSarSensorModel::getImageIdentifier() const {
  return m_imageIdentifier;
}

void UsgsAstroSarSensorModel::setImageIdentifier(const string& imageId,
                                                 csm::WarningList* warnings) {
  m_imageIdentifier = imageId;
}

string UsgsAstroSarSensorModel::getSensorIdentifier() const {
  return m_sensorIdentifier;
}

string UsgsAstroSarSensorModel::getPlatformIdentifier() const {
  return m_platformIdentifier;
}

string UsgsAstroSarSensorModel::getCollectionIdentifier() const {
  return m_collectionIdentifier;
}

string UsgsAstroSarSensorModel::getTrajectoryIdentifier() const {
  return m_trajectoryIdentifier;
}

string UsgsAstroSarSensorModel::getSensorType() const { return "SAR"; }

string UsgsAstroSarSensorModel::getSensorMode() const { return "STRIP"; }

string UsgsAstroSarSensorModel::getReferenceDateAndTime() const {
  csm::ImageCoord referencePointImage = groundToImage(m_referencePointXyz);
  double relativeTime = getImageTime(referencePointImage);
  time_t ephemTime = m_centerEphemerisTime + relativeTime;
  struct tm t = {0};  // Initalize to all 0's
  t.tm_year = 100;    // This is year-1900, so 100 = 2000
  t.tm_mday = 1;
  time_t timeSinceEpoch = mktime(&t);
  time_t finalTime = ephemTime + timeSinceEpoch;
  char buffer[16];
  strftime(buffer, 16, "%Y%m%dT%H%M%S", localtime(&finalTime));
  buffer[15] = '\0';

  return buffer;
}

csm::Ellipsoid UsgsAstroSarSensorModel::getEllipsoid() const {
  return csm::Ellipsoid(m_majorAxis, m_minorAxis);
}

void UsgsAstroSarSensorModel::setEllipsoid(const csm::Ellipsoid& ellipsoid) {
  m_majorAxis = ellipsoid.getSemiMajorRadius();
  m_minorAxis = ellipsoid.getSemiMinorRadius();
}

void UsgsAstroSarSensorModel::determineSensorCovarianceInImageSpace(
    csm::EcefCoord& gp, double sensor_cov[4]) const {
  int i, j, totalAdjParams;
  totalAdjParams = getNumParameters();

  std::vector<csm::RasterGM::SensorPartials> sensor_partials =
      computeAllSensorPartials(gp);

  sensor_cov[0] = 0.0;
  sensor_cov[1] = 0.0;
  sensor_cov[2] = 0.0;
  sensor_cov[3] = 0.0;

  for (i = 0; i < totalAdjParams; i++) {
    for (j = 0; j < totalAdjParams; j++) {
      sensor_cov[0] += sensor_partials[i].first * getParameterCovariance(i, j) *
                       sensor_partials[j].first;
      sensor_cov[1] += sensor_partials[i].second *
                       getParameterCovariance(i, j) * sensor_partials[j].first;
      sensor_cov[2] += sensor_partials[i].first * getParameterCovariance(i, j) *
                       sensor_partials[j].second;
      sensor_cov[3] += sensor_partials[i].second *
                       getParameterCovariance(i, j) * sensor_partials[j].second;
    }
  }
}

std::vector<double> UsgsAstroSarSensorModel::getRangeCoefficients(
    double time) const {
  int numCoeffs = m_scaleConversionCoefficients.size();
  std::vector<double> interpCoeffs;

  double endTime = m_scaleConversionTimes.back();
  if ((numCoeffs / 4) > 1) {
    double coefficients[4];
    double dtEphem = (endTime - m_scaleConversionTimes[0]) /
                     (m_scaleConversionCoefficients.size() / 4);
    lagrangeInterp(m_scaleConversionCoefficients.size() / 4,
                   &m_scaleConversionCoefficients[0], m_scaleConversionTimes[0],
                   dtEphem, time, 4, 8, coefficients);
    interpCoeffs.push_back(coefficients[0]);
    interpCoeffs.push_back(coefficients[1]);
    interpCoeffs.push_back(coefficients[2]);
    interpCoeffs.push_back(coefficients[3]);
  } else {
    interpCoeffs.push_back(m_scaleConversionCoefficients[0]);
    interpCoeffs.push_back(m_scaleConversionCoefficients[1]);
    interpCoeffs.push_back(m_scaleConversionCoefficients[2]);
    interpCoeffs.push_back(m_scaleConversionCoefficients[3]);
  }
  return interpCoeffs;
}

csm::EcefVector UsgsAstroSarSensorModel::getSunPosition(
    const double imageTime) const {
  int numSunPositions = m_sunPosition.size();
  int numSunVelocities = m_sunVelocity.size();
  csm::EcefVector sunPosition = csm::EcefVector();

  // If there are multiple positions, use Lagrange interpolation
  if ((numSunPositions / 3) > 1) {
    double sunPos[3];
    double endTime = m_t0Ephem + m_nLines * m_exposureDuration;
    double sun_dtEphem = (endTime - m_t0Ephem) / (numSunPositions / 3);
    lagrangeInterp(numSunPositions / 3, &m_sunPosition[0], m_t0Ephem,
                   sun_dtEphem, imageTime, 3, 8, sunPos);
    sunPosition.x = sunPos[0];
    sunPosition.y = sunPos[1];
    sunPosition.z = sunPos[2];
  } else if ((numSunVelocities / 3) >= 1) {
    // If there is one position triple with at least one velocity triple
    //  then the illumination direction is calculated via linear extrapolation.
    sunPosition.x = (imageTime * m_sunVelocity[0] + m_sunPosition[0]);
    sunPosition.y = (imageTime * m_sunVelocity[1] + m_sunPosition[1]);
    sunPosition.z = (imageTime * m_sunVelocity[2] + m_sunPosition[2]);
  } else {
    // If there is one position triple with no velocity triple, then the
    //  illumination direction is the difference of the original vectors.
    sunPosition.x = m_sunPosition[0];
    sunPosition.y = m_sunPosition[1];
    sunPosition.z = m_sunPosition[2];
  }
  return sunPosition;
}

double UsgsAstroSarSensorModel::getValue(
    int index, const std::vector<double>& adjustments) const {
  return m_currentParameterValue[index] + adjustments[index];
}