#include "Utilities.h"


#include <Error.h>
#include <cmath>
#include <stack>
#include <stdexcept>
#include <utility>
#include <ctime>
#include <iostream>
#include <fstream>

#include "ale/Distortion.h"

using json = nlohmann::json;

// Calculates a rotation matrix from Euler angles
// in - euler[3]
// out - rotationMatrix[9]
void calculateRotationMatrixFromEuler(double euler[], double rotationMatrix[]) {
  double cos_a = cos(euler[0]);
  double sin_a = sin(euler[0]);
  double cos_b = cos(euler[1]);
  double sin_b = sin(euler[1]);
  double cos_c = cos(euler[2]);
  double sin_c = sin(euler[2]);

  rotationMatrix[0] = cos_b * cos_c;
  rotationMatrix[1] = -cos_a * sin_c + sin_a * sin_b * cos_c;
  rotationMatrix[2] = sin_a * sin_c + cos_a * sin_b * cos_c;
  rotationMatrix[3] = cos_b * sin_c;
  rotationMatrix[4] = cos_a * cos_c + sin_a * sin_b * sin_c;
  rotationMatrix[5] = -sin_a * cos_c + cos_a * sin_b * sin_c;
  rotationMatrix[6] = -sin_b;
  rotationMatrix[7] = sin_a * cos_b;
  rotationMatrix[8] = cos_a * cos_b;
}

// uses a quaternion to calculate a rotation matrix.
// in - q[4]
// out - rotationMatrix[9]
void calculateRotationMatrixFromQuaternions(double q[4],
                                            double rotationMatrix[9]) {
  double norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
  q[0] /= norm;
  q[1] /= norm;
  q[2] /= norm;
  q[3] /= norm;

  rotationMatrix[0] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
  rotationMatrix[1] = 2 * (q[0] * q[1] - q[2] * q[3]);
  rotationMatrix[2] = 2 * (q[0] * q[2] + q[1] * q[3]);
  rotationMatrix[3] = 2 * (q[0] * q[1] + q[2] * q[3]);
  rotationMatrix[4] = -q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
  rotationMatrix[5] = 2 * (q[1] * q[2] - q[0] * q[3]);
  rotationMatrix[6] = 2 * (q[0] * q[2] - q[1] * q[3]);
  rotationMatrix[7] = 2 * (q[1] * q[2] + q[0] * q[3]);
  rotationMatrix[8] = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
}

// Compute the distorted focal plane coordinate for a given image pixel
// in - line
// in - sample
// in - sampleOrigin - the origin of the ccd coordinate system relative to the
// top left of the ccd in - lineOrigin - the origin of the ccd coordinate system
// relative to the top left of the ccd in - sampleSumming in - startingSample -
// first ccd sample for the image in - iTransS[3] - the transformation from
// focal plane to ccd samples in - iTransL[3] - the transformation from focal
// plane to ccd lines out - distortedX out - distortedY
void computeDistortedFocalPlaneCoordinates(
    const double &line, const double &sample, const double &sampleOrigin,
    const double &lineOrigin, const double &sampleSumming,
    const double &lineSumming, const double &startingSample,
    const double &startingLine, const double iTransS[], const double iTransL[],
    double &distortedX, double &distortedY) {
  double detSample = sample * sampleSumming + startingSample;
  double detLine = line * lineSumming + startingLine;
  double m11 = iTransL[1];
  double m12 = iTransL[2];
  double m21 = iTransS[1];
  double m22 = iTransS[2];
  double t1 = detLine - lineOrigin - iTransL[0];
  double t2 = detSample - sampleOrigin - iTransS[0];
  double determinant = m11 * m22 - m12 * m21;
  double p11 = m22 / determinant;
  double p12 = -m12 / determinant;
  double p21 = -m21 / determinant;
  double p22 = m11 / determinant;

  distortedX = p11 * t1 + p12 * t2;
  distortedY = p21 * t1 + p22 * t2;
}


// Compute the de-jittered pixel coordinate given a jittered image coordinate
// a set of jitter coefficients and line exposure times for rolling shutter.
// Jitter coefficients are in largest power first order. There is no constant
// coefficient. For example {1, 2, 3} would correspond to 1*t^3 + 2*t&2 + 3*t.
void removeJitter(
    const double &line, const double &sample,
    const std::vector<double> lineJitterCoeffs, const std::vector<double> sampleJitterCoeffs,
    const std::vector<double> lineTimes,
    double &dejitteredLine, double &dejitteredSample) {
  // Check input
  if (lineJitterCoeffs.size() != sampleJitterCoeffs.size() ||
      lineTimes.size() == 0) {
    throw csm::Error(
        csm::Error::INDEX_OUT_OF_RANGE,
        "Jitter coefficient vectors must be the same size.",
        "removeJitter");
  }
  if (lineTimes.size() == 0) {
    throw csm::Error(
        csm::Error::INDEX_OUT_OF_RANGE,
        "Line exposure times must be non-empty.",
        "removeJitter");
  }
  double lineJitter = 0;
  double sampleJitter = 0;
  // Bound line index to the vector of line exposure times;
  double time = lineTimes[std::max(std::min((int)std::round(line), (int)lineTimes.size()), 1) - 1];
  for (unsigned int n = 0; n < lineJitterCoeffs.size(); n++) {
    double timeTerm = pow(time, lineJitterCoeffs.size() - n);
    lineJitter += lineJitterCoeffs[n] * timeTerm;
    sampleJitter += sampleJitterCoeffs[n] * timeTerm;
  }
  dejitteredLine = line - lineJitter;
  dejitteredSample = sample - sampleJitter;

  return;
}


// Compute the jittered pixel coordinate given a de-jittered image coordinate
// a set of jitter coefficients and line exposure times for rolling shutter.
// Jitter coefficients are in largest power first order. There is no constant
// coefficient. For example {1, 2, 3} would correspond to 1*t^3 + 2*t&2 + 3*t.
// This uses an iterative method so a tolerance and maximum number of iteration
// are required to determine when to stop iterating.
void addJitter(
    const double &line, const double &sample,
    const double &tolerance, const int &maxIts,
    const std::vector<double> lineJitterCoeffs, const std::vector<double> sampleJitterCoeffs,
    const std::vector<double> lineTimes,
    double &jitteredLine, double &jitteredSample) {
  int iteration = 0;
  double dejitteredLine = line - 1;
  double dejitteredSample = sample - 1;
  double currentLine = line;
  double currentSample =  sample;

  while (iteration < maxIts) {
    removeJitter(
        currentLine, currentSample,
        lineJitterCoeffs, sampleJitterCoeffs, lineTimes,
        dejitteredLine, dejitteredSample);

    if (fabs(dejitteredLine - line) < tolerance &&
        fabs(dejitteredSample - sample) < tolerance) {
      break;
    }

    currentLine = line + currentLine - dejitteredLine;
    currentSample = sample + currentSample - dejitteredSample;
    iteration++;
  }

  jitteredLine = currentLine;
  jitteredSample = currentSample;

  return;
}


// Compute the image pixel for a distorted focal plane coordinate
// in - line
// in - sample
// in - sampleOrigin - the origin of the ccd coordinate system relative to the
// top left of the ccd in - lineOrigin - the origin of the ccd coordinate system
// relative to the top left of the ccd in - sampleSumming in - startingSample -
// first ccd sample for the image in - iTransS[3] - the transformation from
// focal plane to ccd samples in - iTransL[3] - the transformation from focal
// plane to ccd lines out - natFocalPlane
void computePixel(const double &distortedX, const double &distortedY,
                  const double &sampleOrigin, const double &lineOrigin,
                  const double &sampleSumming, const double &lineSumming,
                  const double &startingSample, const double &startingLine,
                  const double iTransS[], const double iTransL[], double &line,
                  double &sample) {
  double centeredSample =
      iTransS[0] + iTransS[1] * distortedX + iTransS[2] * distortedY;
  double centeredLine =
      iTransL[0] + iTransL[1] * distortedX + iTransL[2] * distortedY;
  double detSample = centeredSample + sampleOrigin;
  double detLine = centeredLine + lineOrigin;
  sample = (detSample - startingSample) / sampleSumming;
  line = (detLine - startingLine) / lineSumming;
}

// Define imaging ray in image space (In other words, create a look vector in
// camera space) in - undistortedFocalPlaneX in - undistortedFocalPlaneY in -
// zDirection - Either 1 or -1. The direction of the boresight in - focalLength
// out - cameraLook[3]
void createCameraLookVector(const double &undistortedFocalPlaneX,
                            const double &undistortedFocalPlaneY,
                            const double &zDirection, const double &focalLength,
                            double cameraLook[]) {
  cameraLook[0] = -undistortedFocalPlaneX * zDirection;
  cameraLook[1] = -undistortedFocalPlaneY * zDirection;
  cameraLook[2] = -focalLength;
  double magnitude =
      sqrt(cameraLook[0] * cameraLook[0] + cameraLook[1] * cameraLook[1] +
           cameraLook[2] * cameraLook[2]);
  cameraLook[0] /= magnitude;
  cameraLook[1] /= magnitude;
  cameraLook[2] /= magnitude;
}

// Lagrange Interpolation for equally spaced data
void lagrangeInterp(const int &numTime, const double *valueArray,
                    const double &startTime, const double &delTime,
                    const double &time, const int &vectorLength,
                    const int &i_order, double *valueVector) {
  // Lagrange interpolation for uniform post interval.
  // Largest order possible is 8th. Points far away from
  // data center are handled gracefully to avoid failure.

  if (numTime < 2) {
    throw csm::Error(
        csm::Error::INDEX_OUT_OF_RANGE,
        "At least 2 points are required to perform Lagrange interpolation.",
        "lagrangeInterp");
  }

  // Compute index

  double fndex = (time - startTime) / delTime;
  int index = static_cast<int>(fndex);

  if (index < 0) {
    index = 0;
  }
  if (index > numTime - 2) {
    index = numTime - 2;
  }

  // Define order, max is 8

  int order;
  if (index >= 3 && index < numTime - 4) {
    order = 8;
  } else if (index >= 2 && index < numTime - 3) {
    order = 6;
  } else if (index >= 1 && index < numTime - 2) {
    order = 4;
  } else {
    order = 2;
  }
  if (order > i_order) {
    order = i_order;
  }

  // Compute interpolation coefficients
  double tp3, tp2, tp1, tm1, tm2, tm3, tm4, d[8];
  double t0 = fndex - index;
  if (order == 2) {
    tm1 = t0 - 1;
    d[0] = -tm1;
    d[1] = t0;
  } else if (order == 4) {
    tp1 = t0 + 1;
    tm1 = t0 - 1;
    tm2 = t0 - 2;
    d[0] = -t0 * tm1 * tm2 / 6.0;
    d[1] = tp1 * tm1 * tm2 / 2.0;
    d[2] = -tp1 * t0 * tm2 / 2.0;
    d[3] = tp1 * t0 * tm1 / 6.0;
  } else if (order == 6) {
    tp2 = t0 + 2;
    tp1 = t0 + 1;
    tm1 = t0 - 1;
    tm2 = t0 - 2;
    tm3 = t0 - 3;
    d[0] = -tp1 * t0 * tm1 * tm2 * tm3 / 120.0;
    d[1] = tp2 * t0 * tm1 * tm2 * tm3 / 24.0;
    d[2] = -tp2 * tp1 * tm1 * tm2 * tm3 / 12.0;
    d[3] = tp2 * tp1 * t0 * tm2 * tm3 / 12.0;
    d[4] = -tp2 * tp1 * t0 * tm1 * tm3 / 24.0;
    d[5] = tp2 * tp1 * t0 * tm1 * tm2 / 120.0;
  } else if (order == 8) {
    tp3 = t0 + 3;
    tp2 = t0 + 2;
    tp1 = t0 + 1;
    tm1 = t0 - 1;
    tm2 = t0 - 2;
    tm3 = t0 - 3;
    tm4 = t0 - 4;
    // The denominators are hard-coded because the sampling is uniform
    // hence they can be computed explicitly.
    // Note: Trying pre-compute the many repeated multiplications
    // below does not speed things up.
    d[0] = -tp2 * tp1 * t0 * tm1 * tm2 * tm3 * tm4 / 5040.0;
    d[1] = tp3 * tp1 * t0 * tm1 * tm2 * tm3 * tm4  / 720.0;
    d[2] = -tp3 * tp2 * t0 * tm1 * tm2 * tm3 * tm4 / 240.0;
    d[3] = tp3 * tp2 * tp1 * tm1 * tm2 * tm3 * tm4 / 144.0;
    d[4] = -tp3 * tp2 * tp1 * t0 * tm2 * tm3 * tm4 / 144.0;
    d[5] = tp3 * tp2 * tp1 * t0 * tm1 * tm3 * tm4  / 240.0;
    d[6] = -tp3 * tp2 * tp1 * t0 * tm1 * tm2 * tm4 / 720.0;
    d[7] = tp3 * tp2 * tp1 * t0 * tm1 * tm2 * tm3  / 5040.0;
  }

  // Compute interpolated point
  int indx0 = index - order / 2 + 1;
  for (int i = 0; i < vectorLength; i++) {
    valueVector[i] = 0.0;
  }

  for (int i = 0; i < order; i++) {
    int jndex = vectorLength * (indx0 + i);
    for (int j = 0; j < vectorLength; j++) {
      valueVector[j] += d[i] * valueArray[jndex + j];
    }
  }
}

double brentRoot(double lowerBound, double upperBound,
                 std::function<double(double)> func, double epsilon) {
  double counterPoint = lowerBound;
  double currentPoint = upperBound;
  double counterFunc = func(counterPoint);
  double currentFunc = func(currentPoint);
  if (counterFunc * currentFunc > 0.0) {
    throw std::invalid_argument(
        "Function values at the boundaries have the same sign [brentRoot].");
  }
  if (fabs(counterFunc) < fabs(currentFunc)) {
    std::swap(counterPoint, currentPoint);
    std::swap(counterFunc, currentFunc);
  }

  double previousPoint = counterPoint;
  double previousFunc = counterFunc;
  double evenOlderPoint = previousPoint;
  double nextPoint;
  double nextFunc;
  int iteration = 0;
  bool bisected = true;

  do {
    // Inverse quadratic interpolation
    if (counterFunc != previousFunc && counterFunc != currentFunc &&
        currentFunc != previousFunc) {
      nextPoint = (counterPoint * currentFunc * previousFunc) /
                  ((counterFunc - currentFunc) * (counterFunc - previousFunc));
      nextPoint += (currentPoint * counterFunc * previousFunc) /
                   ((currentFunc - counterFunc) * (currentFunc - previousFunc));
      nextPoint +=
          (previousPoint * currentFunc * counterFunc) /
          ((previousFunc - counterFunc) * (previousFunc - currentFunc));
    } else {
      // Secant method
      nextPoint = currentPoint - currentFunc * (currentPoint - counterPoint) /
                                     (currentFunc - counterFunc);
    }

    // Bisection method
    if (((currentPoint - nextPoint) *
             (nextPoint - (3 * counterPoint + currentPoint) / 4) <
         0) ||
        (bisected && fabs(nextPoint - currentPoint) >=
                         fabs(currentPoint - previousPoint) / 2) ||
        (!bisected && fabs(nextPoint - currentPoint) >=
                          fabs(previousPoint - evenOlderPoint) / 2) ||
        (bisected && fabs(currentPoint - previousPoint) < epsilon) ||
        (!bisected && fabs(previousPoint - evenOlderPoint) < epsilon)) {
      nextPoint = (currentPoint + counterPoint) / 2;
      bisected = true;
    } else {
      bisected = false;
    }

    // Setup for next iteration
    evenOlderPoint = previousPoint;
    previousPoint = currentPoint;
    previousFunc = currentFunc;
    nextFunc = func(nextPoint);

    // This is a bugfix. Without it, the code gets lost and can't find the solution.
    // See also the implementation at https://en.wikipedia.org/wiki/Brent%27s_method
    if (nextFunc == 0)
      return nextPoint;

    if (counterFunc * nextFunc < 0) {
      currentPoint = nextPoint;
      currentFunc = nextFunc;
    } else {
      counterPoint = nextPoint;
      counterFunc = nextFunc;
    }
  } while (++iteration < 30 && fabs(counterPoint - currentPoint) > epsilon);

  return nextPoint;
}

double evaluatePolynomial(const std::vector<double> &coeffs, double x) {
  if (coeffs.empty()) {
    throw std::invalid_argument("Polynomial coeffs must be non-empty.");
  }
  auto revIt = coeffs.crbegin();
  double value = *revIt;
  ++revIt;
  for (; revIt != coeffs.crend(); ++revIt) {
    value *= x;
    value += *revIt;
  }
  return value;
}

double evaluatePolynomialDerivative(const std::vector<double> &coeffs,
                                    double x) {
  if (coeffs.empty()) {
    throw std::invalid_argument("Polynomial coeffs must be non-empty.");
  }
  int i = coeffs.size() - 1;
  double value = i * coeffs[i];
  --i;
  for (; i > 0; --i) {
    value *= x;
    value += i * coeffs[i];
  }
  return value;
}

double polynomialRoot(const std::vector<double> &coeffs, double guess,
                      double threshold, int maxIterations) {
  double root = guess;
  double polyValue = evaluatePolynomial(coeffs, root);
  double polyDeriv = 0.0;
  for (int iteration = 0; iteration < maxIterations + 1; iteration++) {
    if (fabs(polyValue) < threshold) {
      return root;
    }
    polyDeriv = evaluatePolynomialDerivative(coeffs, root);
    if (fabs(polyDeriv) < 1e-15) {
      throw std::invalid_argument("Derivative at guess (" +
                                  std::to_string(guess) +
                                  ") is too close to 0.");
    }
    root -= polyValue / polyDeriv;
    polyValue = evaluatePolynomial(coeffs, root);
  }
  throw std::invalid_argument("Root finder did not converge after " +
                              std::to_string(maxIterations) + " iterations");
}

double computeEllipsoidElevation(double x, double y, double z, double semiMajor,
                                 double semiMinor, double desired_precision,
                                 double *achieved_precision) {
  // Compute elevation given xyz
  // Requires semi-major-axis and eccentricity-square
  const int MKTR = 10;
  double ecc_sqr = 1.0 - semiMinor * semiMinor / semiMajor / semiMajor;
  double ep2 = 1.0 - ecc_sqr;
  double d2 = x * x + y * y;
  double d = sqrt(d2);
  double h = 0.0;
  int ktr = 0;
  double hPrev, r;

  // Suited for points near equator
  if (d >= z) {
    double tt, zz, n;
    double tanPhi = z / d;
    do {
      hPrev = h;
      tt = tanPhi * tanPhi;
      r = semiMajor / sqrt(1.0 + ep2 * tt);
      zz = z + r * ecc_sqr * tanPhi;
      n = r * sqrt(1.0 + tt);
      h = sqrt(d2 + zz * zz) - n;
      tanPhi = zz / d;
      ktr++;
    } while (MKTR > ktr && fabs(h - hPrev) > desired_precision);
  } else {
    // Suited for points near the poles
    double cc, dd, nn;
    double cotPhi = d / z;
    do {
      hPrev = h;
      cc = cotPhi * cotPhi;
      r = semiMajor / sqrt(ep2 + cc);
      dd = d - r * ecc_sqr * cotPhi;
      nn = r * sqrt(1.0 + cc) * ep2;
      h = sqrt(dd * dd + z * z) - nn;
      cotPhi = dd / z;
      ktr++;
    } while (MKTR > ktr && fabs(h - hPrev) > desired_precision);
  }

  if (achieved_precision) {
    *achieved_precision = fabs(h - hPrev);
  }

  return h;
}

csm::EcefVector operator*(double scalar, const csm::EcefVector &vec) {
  return csm::EcefVector(scalar * vec.x, scalar * vec.y, scalar * vec.z);
}

csm::EcefVector operator*(const csm::EcefVector &vec, double scalar) {
  return scalar * vec;
}

csm::EcefVector operator/(const csm::EcefVector &vec, double scalar) {
  return 1.0 / scalar * vec;
}

csm::EcefVector operator+(const csm::EcefVector &vec1,
                          const csm::EcefVector &vec2) {
  return csm::EcefVector(vec1.x + vec2.x, vec1.y + vec2.y, vec1.z + vec2.z);
}

csm::EcefVector operator-(const csm::EcefVector &vec1,
                          const csm::EcefVector &vec2) {
  return csm::EcefVector(vec1.x - vec2.x, vec1.y - vec2.y, vec1.z - vec2.z);
}

double dot(const csm::EcefVector &vec1, const csm::EcefVector &vec2) {
  return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
}

csm::EcefVector cross(const csm::EcefVector &vec1,
                      const csm::EcefVector &vec2) {
  return csm::EcefVector(vec1.y * vec2.z - vec1.z * vec2.y,
                         vec1.z * vec2.x - vec1.x * vec2.z,
                         vec1.x * vec2.y - vec1.y * vec2.x);
}

double norm(const csm::EcefVector &vec) {
  return sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
}

csm::EcefVector normalized(const csm::EcefVector &vec) {
  return vec / norm(vec);
}

csm::EcefVector projection(const csm::EcefVector &vec1,
                           const csm::EcefVector &vec2) {
  return dot(vec1, vec2) / dot(vec2, vec2) * vec2;
}

csm::EcefVector rejection(const csm::EcefVector &vec1,
                          const csm::EcefVector &vec2) {
  return vec1 - projection(vec1, vec2);
}

// convert a measurement
double metric_conversion(double val, std::string from, std::string to) {
  json typemap = {{"m", 0}, {"km", 3}};

  // everything to lowercase
  std::transform(from.begin(), from.end(), from.begin(), ::tolower);
  std::transform(to.begin(), to.end(), to.begin(), ::tolower);
  return val * pow(10, typemap[from].get<int>() - typemap[to].get<int>());
}

std::string getSensorModelName(json isd, csm::WarningList *list) {
  std::string name = "";
  try {
    name = isd.at("name_model");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the sensor model name.",
                                   "Utilities::getSensorModelName()"));
    }
  }
  return name;
}

std::string getImageId(json isd, csm::WarningList *list) {
  std::string id = "";
  try {
    id = isd.at("image_identifier");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the image identifier.",
                                   "Utilities::getImageId()"));
    }
  }
  return id;
}

std::string getSensorName(json isd, csm::WarningList *list) {
  std::string name = "";
  try {
    name = isd.at("name_sensor");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the sensor name.",
                                   "Utilities::getSensorName()"));
    }
  }
  return name;
}

std::string getPlatformName(json isd, csm::WarningList *list) {
  std::string name = "";
  try {
    name = isd.at("name_platform");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the platform name.",
                                   "Utilities::getPlatformName()"));
    }
  }
  return name;
}

std::string getLogFile(nlohmann::json isd, csm::WarningList *list) {
  std::string file = "";
  try {
    file = isd.at("log_file");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the log filename.",
                                   "Utilities::getLogFile()"));
    }
  }
  return file;
}

int getTotalLines(json isd, csm::WarningList *list) {
  int lines = 0;
  try {
    lines = isd.at("image_lines");
  } catch (...) {
    if (list) {
      list->push_back(
          csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                       "Could not parse the number of lines in the image.",
                       "Utilities::getTotalLines()"));
    }
  }
  return lines;
}

int getTotalSamples(json isd, csm::WarningList *list) {
  int samples = 0;
  try {
    samples = isd.at("image_samples");
  } catch (...) {
    if (list) {
      list->push_back(
          csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                       "Could not parse the number of samples in the image.",
                       "Utilities::getTotalSamples()"));
    }
  }
  return samples;
}

double getStartingTime(json isd, csm::WarningList *list) {
  double time = 0.0;
  try {
    time = isd.at("starting_ephemeris_time");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the image start time.",
                                   "Utilities::getStartingTime()"));
    }
  }
  return time;
}

double getCenterTime(json isd, csm::WarningList *list) {
  double time = 0.0;
  try {
    time = isd.at("center_ephemeris_time");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the center image time.",
                                   "Utilities::getCenterTime()"));
    }
  }
  return time;
}

double getEndingTime(json isd, csm::WarningList *list) {
  double time = 0.0;
  try {
    time = isd.at("ending_ephemeris_time");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the ending image time.",
                                   "Utilities::getEndingTime()"));
    }
  }
  return time;
}

std::vector<double> getIntegrationStartLines(
    std::vector<std::vector<double>> lineScanRate, csm::WarningList *list) {
  std::vector<double> lines;
  try {
    for (auto &scanRate : lineScanRate) {
      if (scanRate.size() != 3) {
        throw csm::Error(
            csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE,
            "Unable to parse integration start lines from line "
            "scan rate due to malformed vector. Expected vector size of 3.",
            "Utilities::getIntegrationStartLines()");
      }
      lines.push_back(scanRate[0]);
    }
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the integration start "
                                   "lines in the integration time table.",
                                   "Utilities::getIntegrationStartLines()"));
    }
  }
  return lines;
}

std::vector<double> getIntegrationStartTimes(
    std::vector<std::vector<double>> lineScanRate, csm::WarningList *list) {
  std::vector<double> times;
  try {
    for (auto &scanRate : lineScanRate) {
      if (scanRate.size() != 3) {
        throw csm::Error(
            csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE,
            "Unable to parse integration start times from line "
            "scan rate due to malformed vector. Expected vector size of 3.",
            "Utilities::getIntegrationStartTimes()");
      }
      times.push_back(scanRate[1]);
    }
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the integration start "
                                   "times in the integration time table.",
                                   "Utilities::getIntegrationStartTimes()"));
    }
  }
  return times;
}

std::vector<double> getIntegrationTimes(
    std::vector<std::vector<double>> lineScanRate, csm::WarningList *list) {
  std::vector<double> times;
  try {
    for (auto &scanRate : lineScanRate) {
      if (scanRate.size() != 3) {
        throw csm::Error(
            csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE,
            "Unable to parse integration times from line "
            "scan rate due to malformed vector. Expected vector size of 3.",
            "Utilities::getIntegrationTimes()");
      }
      times.push_back(scanRate[2]);
    }
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the integration times in "
                                   "the integration time table.",
                                   "Utilities::getIntegrationTimes()"));
    }
  }
  return times;
}

int getSampleSumming(json isd, csm::WarningList *list) {
  int summing = 0;
  try {
    summing = isd.at("detector_sample_summing");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(
          csm::Warning::DATA_NOT_AVAILABLE,
          "Could not parse the sample direction detector pixel summing.",
          "Utilities::getSampleSumming()"));
    }
  }
  return summing;
}

int getLineSumming(json isd, csm::WarningList *list) {
  int summing = 0;
  try {
    summing = isd.at("detector_line_summing");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(
          csm::Warning::DATA_NOT_AVAILABLE,
          "Could not parse the line direction detector pixel summing.",
          "Utilities::getLineSumming()"));
    }
  }
  return summing;
}

double getFocalLength(json isd, csm::WarningList *list) {
  double length = 0.0;
  try {
    length = isd.at("focal_length_model").at("focal_length");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the focal length.",
                                   "Utilities::getFocalLength()"));
    }
  }
  return length;
}

double getFocalLengthEpsilon(json isd, csm::WarningList *list) {
  double epsilon = 0.0;
  try {
    epsilon = isd.at("focal_length_model").at("focal_epsilon");
  } catch (...) {
    if (list) {
      list->push_back(
          csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                       "Could not parse the focal length uncertainty.",
                       "Utilities::getFocalLengthEpsilon()"));
    }
  }
  return epsilon;
}

std::vector<double> getFocal2PixelLines(json isd, csm::WarningList *list) {
  std::vector<double> transformation;
  try {
    transformation = isd.at("focal2pixel_lines").get<std::vector<double>>();
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the focal plane coordinate "
                                   "to detector lines transformation.",
                                   "Utilities::getFocal2PixelLines()"));
    }
  }
  return transformation;
}

std::vector<double> getFocal2PixelSamples(json isd, csm::WarningList *list) {
  std::vector<double> transformation;
  try {
    transformation = isd.at("focal2pixel_samples").get<std::vector<double>>();
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the focal plane coordinate "
                                   "to detector samples transformation.",
                                   "Utilities::getFocal2PixelSamples()"));
    }
  }
  return transformation;
}

double getDetectorCenterLine(json isd, csm::WarningList *list) {
  double line;
  try {
    line = isd.at("detector_center").at("line");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the detector center line.",
                                   "Utilities::getDetectorCenterLine()"));
    }
  }
  return line;
}

double getDetectorCenterSample(json isd, csm::WarningList *list) {
  double sample;
  try {
    sample = isd.at("detector_center").at("sample");
  } catch (...) {
    if (list) {
      list->push_back(
          csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                       "Could not parse the detector center sample.",
                       "Utilities::getDetectorCenterSample()"));
    }
  }
  return sample;
}

double getDetectorStartingLine(json isd, csm::WarningList *list) {
  double line;
  try {
    line = isd.at("starting_detector_line");
  } catch (...) {
    if (list) {
      list->push_back(
          csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                       "Could not parse the detector starting line.",
                       "Utilities::getDetectorStartingLine()"));
    }
  }
  return line;
}

double getDetectorStartingSample(json isd, csm::WarningList *list) {
  double sample;
  try {
    sample = isd.at("starting_detector_sample");
  } catch (...) {
    if (list) {
      list->push_back(
          csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                       "Could not parse the detector starting sample.",
                       "Utilities::getDetectorStartingSample()"));
    }
  }
  return sample;
}

double getMinHeight(json isd, csm::WarningList *list) {
  double height = 0.0;
  try {
    json referenceHeight = isd.at("reference_height");
    json minHeight = referenceHeight.at("minheight");
    json unit = referenceHeight.at("unit");
    height =
        metric_conversion(minHeight.get<double>(), unit.get<std::string>());
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(
          csm::Warning::DATA_NOT_AVAILABLE,
          "Could not parse the minimum height above the reference ellipsoid.",
          "Utilities::getMinHeight()"));
    }
  }
  return height;
}

double getMaxHeight(json isd, csm::WarningList *list) {
  double height = 0.0;
  try {
    json referenceHeight = isd.at("reference_height");
    json maxHeight = referenceHeight.at("maxheight");
    json unit = referenceHeight.at("unit");
    height =
        metric_conversion(maxHeight.get<double>(), unit.get<std::string>());
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(
          csm::Warning::DATA_NOT_AVAILABLE,
          "Could not parse the maximum height above the reference ellipsoid.",
          "Utilities::getMaxHeight()"));
    }
  }
  return height;
}

double getSemiMajorRadius(json isd, csm::WarningList *list) {
  double radius = 0.0;
  try {
    json radii = isd.at("radii");
    json semiMajor = radii.at("semimajor");
    json unit = radii.at("unit");
    radius =
        metric_conversion(semiMajor.get<double>(), unit.get<std::string>());
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(
          csm::Warning::DATA_NOT_AVAILABLE,
          "Could not parse the reference ellipsoid semimajor radius.",
          "Utilities::getSemiMajorRadius()"));
    }
  }
  return radius;
}

double getSemiMinorRadius(json isd, csm::WarningList *list) {
  double radius = 0.0;
  try {
    json radii = isd.at("radii");
    json semiMinor = radii.at("semiminor");
    json unit = radii.at("unit");
    radius =
        metric_conversion(semiMinor.get<double>(), unit.get<std::string>());
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(
          csm::Warning::DATA_NOT_AVAILABLE,
          "Could not parse the reference ellipsoid semiminor radius.",
          "Utilities::getSemiMinorRadius()"));
    }
  }
  return radius;
}

// Converts the distortion model name from the ISD (string) to the enumeration
// type. Defaults to transverse
DistortionType getDistortionModel(json isd, csm::WarningList *list) {
  try {
    json distoriton_subset = isd.at("optical_distortion");

    json::iterator it = distoriton_subset.begin();

    std::string distortion = (std::string)it.key();

    if (distortion.compare("transverse") == 0) {
      return DistortionType::TRANSVERSE;
    } else if (distortion.compare("radial") == 0) {
      return DistortionType::RADIAL;
    } else if (distortion.compare("kaguyalism") == 0) {
      return DistortionType::KAGUYALISM;
    } else if (distortion.compare("dawnfc") == 0) {
      return DistortionType::DAWNFC;
    } else if (distortion.compare("lrolrocnac") == 0) {
      return DistortionType::LROLROCNAC;
    }
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the distortion model.",
                                   "Utilities::getDistortionModel()"));
    }
  }
  return DistortionType::TRANSVERSE;
}

DistortionType getDistortionModel(int aleDistortionModel,
                                  csm::WarningList *list) {
  try {
    ale::DistortionType aleDistortionType;
    aleDistortionType = (ale::DistortionType)aleDistortionModel;

    if (aleDistortionType == ale::DistortionType::TRANSVERSE) {
      return DistortionType::TRANSVERSE;
    } else if (aleDistortionType == ale::DistortionType::RADIAL) {
      return DistortionType::RADIAL;
    } else if (aleDistortionType == ale::DistortionType::KAGUYALISM) {
      return DistortionType::KAGUYALISM;
    } else if (aleDistortionType == ale::DistortionType::DAWNFC) {
      return DistortionType::DAWNFC;
    } else if (aleDistortionType == ale::DistortionType::LROLROCNAC) {
      return DistortionType::LROLROCNAC;
    }else if (aleDistortionType == ale::DistortionType::CAHVOR) {
      return DistortionType::CAHVOR;
    }
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the distortion model.",
                                   "Utilities::getDistortionModel()"));
    }
  }
  return DistortionType::TRANSVERSE;
}

std::vector<double> getDistortionCoeffs(json isd, csm::WarningList *list) {
  std::vector<double> coefficients;

  DistortionType distortion = getDistortionModel(isd);

  switch (distortion) {
    case DistortionType::TRANSVERSE: {
      try {
        std::vector<double> coefficientsX, coefficientsY;

        coefficientsX = isd.at("optical_distortion")
                            .at("transverse")
                            .at("x")
                            .get<std::vector<double>>();
        coefficientsX.resize(10, 0.0);

        coefficientsY = isd.at("optical_distortion")
                            .at("transverse")
                            .at("y")
                            .get<std::vector<double>>();
        coefficientsY.resize(10, 0.0);

        coefficients = coefficientsX;

        coefficients.insert(coefficients.end(), coefficientsY.begin(),
                            coefficientsY.end());
        return coefficients;
      } catch (...) {
        if (list) {
          list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                       "Could not parse a set of transverse "
                                       "distortion model coefficients.",
                                       "Utilities::getDistortion()"));
        }
        coefficients = std::vector<double>(20, 0.0);
        coefficients[1] = 1.0;
        coefficients[12] = 1.0;
      }
    } break;
    case DistortionType::RADIAL: {
      try {
        coefficients = isd.at("optical_distortion")
                           .at("radial")
                           .at("coefficients")
                           .get<std::vector<double>>();

        return coefficients;
      } catch (...) {
        if (list) {
          list->push_back(csm::Warning(
              csm::Warning::DATA_NOT_AVAILABLE,
              "Could not parse the radial distortion model coefficients.",
              "Utilities::getDistortion()"));
        }
        coefficients = std::vector<double>(3, 0.0);
      }
    } break;
    case DistortionType::KAGUYALISM: {
      try {
        std::vector<double> coefficientsX = isd.at("optical_distortion")
                                                .at("kaguyalism")
                                                .at("x")
                                                .get<std::vector<double>>();
        std::vector<double> coefficientsY = isd.at("optical_distortion")
                                                .at("kaguyalism")
                                                .at("y")
                                                .get<std::vector<double>>();
        double boresightX = isd.at("optical_distortion")
                                .at("kaguyalism")
                                .at("boresight_x")
                                .get<double>();
        double boresightY = isd.at("optical_distortion")
                                .at("kaguyalism")
                                .at("boresight_y")
                                .get<double>();

        coefficientsX.resize(4, 0.0);
        coefficientsY.resize(4, 0.0);
        coefficientsX.insert(coefficientsX.begin(), boresightX);
        coefficientsY.insert(coefficientsY.begin(), boresightY);
        coefficientsX.insert(coefficientsX.end(), coefficientsY.begin(),
                             coefficientsY.end());

        return coefficientsX;
      } catch (...) {
        if (list) {
          list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                       "Could not parse a set of Kaguya LISM "
                                       "distortion model coefficients.",
                                       "Utilities::getDistortion()"));
        }
        coefficients = std::vector<double>(8, 0.0);
      }
    } break;
    case DistortionType::DAWNFC: {
      try {
        coefficients = isd.at("optical_distortion")
                           .at("dawnfc")
                           .at("coefficients")
                           .get<std::vector<double>>();

        return coefficients;
      } catch (...) {
        if (list) {
          list->push_back(csm::Warning(
              csm::Warning::DATA_NOT_AVAILABLE,
              "Could not parse the dawn radial distortion model coefficients.",
              "Utilities::getDistortion()"));
        }
        coefficients = std::vector<double>(1, 0.0);
      }
    } break;
    case DistortionType::LROLROCNAC: {
      try {
        coefficients = isd.at("optical_distortion")
                           .at("lrolrocnac")
                           .at("coefficients")
                           .get<std::vector<double>>();
        return coefficients;
      } catch (...) {
        if (list) {
          list->push_back(csm::Warning(
              csm::Warning::DATA_NOT_AVAILABLE,
              "Could not parse the lrolrocnac distortion model coefficients.",
              "Utilities::getDistortion()"));
        }
        coefficients = std::vector<double>(1, 0.0);
      }
    } break;
    case DistortionType::CAHVOR: {
      try {
        coefficients = isd.at("optical_distortion")
                           .at("cahvor")
                           .at("coefficients")
                           .get<std::vector<double>>();

        return coefficients;
      } catch (...) {
        if (list) {
          list->push_back(csm::Warning(
              csm::Warning::DATA_NOT_AVAILABLE,
              "Could not parse the radial distortion model coefficients.",
              "Utilities::getDistortion()"));
        }
        coefficients = std::vector<double>(6, 0.0);
      }
    } break;
  }
  if (list) {
    list->push_back(
        csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                     "Could not parse the distortion model coefficients.",
                     "Utilities::getDistortion()"));
  }

  return coefficients;
}

std::vector<double> getSunPositions(json isd, csm::WarningList *list) {
  std::vector<double> positions;
  try {
    json jayson = isd.at("sun_position");
    json unit = jayson.at("unit");
    for (auto &location : jayson.at("positions")) {
      positions.push_back(metric_conversion(location[0].get<double>(),
                                            unit.get<std::string>()));
      positions.push_back(metric_conversion(location[1].get<double>(),
                                            unit.get<std::string>()));
      positions.push_back(metric_conversion(location[2].get<double>(),
                                            unit.get<std::string>()));
    }
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the sun positions.",
                                   "Utilities::getSunPositions()"));
    }
  }
  return positions;
}

std::vector<double> getSunVelocities(json isd, csm::WarningList *list) {
  std::vector<double> velocities;
  try {
    json jayson = isd.at("sun_position");
    json unit = jayson.at("unit");
    for (auto &location : jayson.at("velocities")) {
      velocities.push_back(metric_conversion(location[0].get<double>(),
                                             unit.get<std::string>()));
      velocities.push_back(metric_conversion(location[1].get<double>(),
                                             unit.get<std::string>()));
      velocities.push_back(metric_conversion(location[2].get<double>(),
                                             unit.get<std::string>()));
    }
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the sun velocities.",
                                   "Utilities::getSunVelocities()"));
    }
  }
  return velocities;
}

std::vector<double> getSensorPositions(json isd, csm::WarningList *list) {
  std::vector<double> positions;
  try {
    json jayson = isd.at("sensor_position");
    json unit = jayson.at("unit");
    for (auto &location : jayson.at("positions")) {
      positions.push_back(metric_conversion(location[0].get<double>(),
                                            unit.get<std::string>()));
      positions.push_back(metric_conversion(location[1].get<double>(),
                                            unit.get<std::string>()));
      positions.push_back(metric_conversion(location[2].get<double>(),
                                            unit.get<std::string>()));
    }
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the sensor positions.",
                                   "Utilities::getSensorPositions()"));
    }
  }
  return positions;
}

std::vector<double> getSensorVelocities(json isd, csm::WarningList *list) {
  std::vector<double> velocities;
  try {
    json jayson = isd.at("sensor_position");
    json unit = jayson.at("unit");
    for (auto &velocity : jayson.at("velocities")) {
      velocities.push_back(metric_conversion(velocity[0].get<double>(),
                                             unit.get<std::string>()));
      velocities.push_back(metric_conversion(velocity[1].get<double>(),
                                             unit.get<std::string>()));
      velocities.push_back(metric_conversion(velocity[2].get<double>(),
                                             unit.get<std::string>()));
    }
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the sensor velocities.",
                                   "Utilities::getSensorVelocities()"));
    }
  }
  return velocities;
}

std::vector<double> getSensorOrientations(json isd, csm::WarningList *list) {
  std::vector<double> quaternions;
  try {
    for (auto &quaternion : isd.at("sensor_orientation").at("quaternions")) {
      quaternions.push_back(quaternion[0]);
      quaternions.push_back(quaternion[1]);
      quaternions.push_back(quaternion[2]);
      quaternions.push_back(quaternion[3]);
    }
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the sensor orientations.",
                                   "Utilities::getSensorOrientations()"));
    }
  }
  return quaternions;
}

double getExposureDuration(nlohmann::json isd, csm::WarningList *list) {
  double duration;
  try {
    duration = isd.at("line_exposure_duration");
  } catch (...) {
    if (list) {
      list->push_back(
          csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                       "Could not parse the line exposure duration.",
                       "Utilities::getExposureDuration()"));
    }
  }
  return duration;
}

double getScaledPixelWidth(nlohmann::json isd, csm::WarningList *list) {
  double width;
  try {
    width = isd.at("scaled_pixel_width");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the scaled pixel width.",
                                   "Utilities::getScaledPixelWidth()"));
    }
  }
  return width;
}

std::string getLookDirection(nlohmann::json isd, csm::WarningList *list) {
  std::string lookDirection = "";
  try {
    lookDirection = isd.at("look_direction");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the scaled pixel width.",
                                   "Utilities::getScaledPixelWidth()"));
    }
  }
  return lookDirection;
}

std::vector<double> getScaleConversionTimes(nlohmann::json isd,
                                            csm::WarningList *list) {
  std::vector<double> time;
  try {
    time = isd.at("range_conversion_times").get<std::vector<double>>();
  } catch (...) {
    if (list) {
      list->push_back(
          csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                       "Could not parse the range conversion times.",
                       "Utilities::getScaleConversionTimes()"));
    }
  }
  return time;
}

std::vector<double> getScaleConversionCoefficients(nlohmann::json isd,
                                                   csm::WarningList *list) {
  std::vector<double> coefficients;
  try {
    for (auto &location : isd.at("range_conversion_coefficients")) {
      coefficients.push_back(location[0]);
      coefficients.push_back(location[1]);
      coefficients.push_back(location[2]);
      coefficients.push_back(location[3]);
    }
  } catch (...) {
    if (list) {
      list->push_back(
          csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                       "Could not parse the range conversion coefficients.",
                       "Utilities::getScaleConversionCoefficients()"));
    }
  }
  return coefficients;
}

double getWavelength(json isd, csm::WarningList *list) {
  double wavelength = 0.0;
  try {
    wavelength = isd.at("wavelength");
  } catch (...) {
    if (list) {
      list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE,
                                   "Could not parse the wavelength.",
                                   "Utilities::getWavelength()"));
    }
  }
  return wavelength;
}

json stateAsJson(std::string modelState) {
  // Remove special characters from string
  sanitize(modelState);

  std::size_t foundFirst = modelState.find_first_of("{");
  std::size_t foundLast = modelState.find_last_of("}");

  if (foundFirst == std::string::npos) {
    foundFirst = 0;
  }
  return json::parse(modelState.begin() + foundFirst, modelState.begin() + foundLast + 1);
}

void sanitize(std::string &input){
  // Replaces characters from the string that are not printable with newlines
  std::replace_if(input.begin(), input.end(), [](int c){return !::isprint(c);}, '\n');
}

// Read a file's content in a single string
bool readFileInString(std::string const& filename, std::string & str) {

  str.clear(); // clear the output

  std::ifstream ifs(filename.c_str());
  if (!ifs.is_open()) {
    std::cout << "Cannot open file: " << filename << std::endl;
    return false;
  }

  ifs.seekg(0, std::ios::end);
  str.reserve(ifs.tellg());
  ifs.seekg(0, std::ios::beg);
  str.assign((std::istreambuf_iterator<char>(ifs)),
             std::istreambuf_iterator<char>());
  ifs.close();

  return true;
}

// Apply a rotation to a vector of quaternions.
void applyRotationToQuatVec(ale::Rotation const& r, std::vector<double> & quaternions) {
  int num_quat = quaternions.size();

  for (int it = 0; it < num_quat/4; it++) {

    double * q = &quaternions[4*it];

    // Note that quaternion in q is stored in order x, y, z, w, while
    // the rotation matrix wants it as w, x, y, z.

    std::vector<double> trans_q = (r * ale::Rotation(q[3], q[0], q[1], q[2])).toQuaternion();

    // Modify in-place
    q[0] = trans_q[1];
    q[1] = trans_q[2];
    q[2] = trans_q[3];
    q[3] = trans_q[0];
  }
}

// Apply a rotation and translation to a vector of xyz vectors.
void applyRotationTranslationToXyzVec(ale::Rotation const& r, ale::Vec3d const& t,
                                      std::vector<double> & xyz) {

  int num_xyz = xyz.size();
  for (int it = 0; it < num_xyz/3; it++) {

    double * p = &xyz[3*it];

    ale::Vec3d p_vec(p[0], p[1], p[2]);

    // Apply the rotation
    p_vec = r(p_vec);

    // Apply the translation
    p_vec += t;

    // Modify in-place
    p[0] = p_vec.x;
    p[1] = p_vec.y;
    p[2] = p_vec.z;

  }
}

// Convert ephemeris time, in seconds since January 1, 2000 12:00:00 AM GMT,
// to a calendar time string, such as 2000-01-01T00:16:40Z.
std::string ephemTimeToCalendarTime(double ephemTime) {

  // Care must be taken to not use mktime() or localtime() as their
  // precise value depends on if a location respects Daylight Savings Time.

  std::time_t y2k = 946684800; // January 1, 2000 12:00:00 AM GMT
  std::time_t finalTime = ephemTime + y2k;
  char buffer[22];
  strftime(buffer, 22, "%Y-%m-%dT%H:%M:%SZ", std::gmtime(&finalTime));
  buffer[21] = '\0';
  return buffer;
}