text: #include <cmath>
  #include <iostream>
  
  #include "WarpTransform.h"
  
  namespace Isis {
    WarpTransform::WarpTransform(BasisFunction &basisLine,
                                 BasisFunction &basisSamp, bool weighted, std::vector<double> &inputLine,
                                 std::vector<double> &inputSample, std::vector<double> &outputLine,
                                 std::vector<double> &outputSample, int inputLines, int inputSamples,
                                 int outputLines, int outputSamples) {
      // Determine the size of the output cube if necessary
      // We do this by solving for output position given an input position
      // then walk the edge of the input file to find the maximum output
      // line/sample
      if ((outputLines == 0) || (outputSamples == 0)) {
        LeastSquares lsqOutputLine(basisLine);
        LeastSquares lsqOutputSamp(basisSamp);
        std::vector<double> known;
        known.resize(2);
        for (int i = 0; i < (int)inputLine.size(); i++) {
          known[0] = inputLine[i];
          known[1] = inputSample[i];
          lsqOutputLine.AddKnown(known, outputLine[i]);
          lsqOutputSamp.AddKnown(known, outputSample[i]);
        }
  
        lsqOutputLine.Solve();
        lsqOutputSamp.Solve();
  
        outputLines = 0;
        outputSamples = 0;
        // Walk top and bottom edge
        for (int samp = 1; samp <= inputSamples; samp++) {
          known[0] = 1.0;
          known[1] = samp;
          int oline = (int)(lsqOutputLine.Evaluate(known) + 0.5);
          int osamp = (int)(lsqOutputSamp.Evaluate(known) + 0.5);
          if (oline > outputLines)
            outputLines = oline;
          if (osamp > outputSamples)
            outputSamples = osamp;
  
          known[0] = inputLines;
          known[1] = samp;
          oline = (int)(lsqOutputLine.Evaluate(known) + 0.5);
          osamp = (int)(lsqOutputSamp.Evaluate(known) + 0.5);
          if (oline > outputLines)
            outputLines = oline;
          if (osamp > outputSamples)
            outputSamples = osamp;
        }
  
        // Walk left and right edge
        for (int line = 1; line <= inputLines; line++) {
          known[0] = line;
          known[1] = 1.0;
          int oline = (int)(lsqOutputLine.Evaluate(known) + 0.5);
          int osamp = (int)(lsqOutputSamp.Evaluate(known) + 0.5);
          if (oline > outputLines)
            outputLines = oline;
          if (osamp > outputSamples)
            outputSamples = osamp;
  
          known[0] = line;
          known[1] = inputSamples;
          oline = (int)(lsqOutputLine.Evaluate(known) + 0.5);
          osamp = (int)(lsqOutputSamp.Evaluate(known) + 0.5);
          if (oline > outputLines)
            outputLines = oline;
          if (osamp > outputSamples)
            outputSamples = osamp;
        }
      }
  
      p_outputLines = outputLines;
      p_outputSamples = outputSamples;
  
      // Create the equations for the control points using a least squares fit
      p_lsqInputSamp = new LeastSquares(basisSamp);
      p_lsqInputLine = new LeastSquares(basisLine);
      std::vector<double> known;
      known.resize(2);
      for (int i = 0; i < (int)inputLine.size(); i++) {
        known[0] = outputLine[i];
        known[1] = outputSample[i];
        p_lsqInputLine->AddKnown(known, inputLine[i]);
        p_lsqInputSamp->AddKnown(known, inputSample[i]);
      }
  
      p_lsqInputLine->Solve();
      p_lsqInputSamp->Solve();
  
      p_weighted = weighted;
      if (weighted) {
        p_outputLine = outputLine;
        p_outputSample = outputSample;
      }
    }
  
    WarpTransform::~WarpTransform() {
      if (p_lsqInputLine != NULL)
        delete p_lsqInputLine;
      if (p_lsqInputSamp != NULL)
        delete p_lsqInputSamp;
    }
  
  
    // Convert the requested output samp/line to an input samp/line
    bool WarpTransform::Xform(double &inSample, double &inLine,
                              const double outSample, const double outLine) {
      if (p_weighted) {
        for (int i = 0; i < (int) p_outputLine.size(); i++) {
          double dist = (outLine - p_outputLine[i]) *
                        (outLine - p_outputLine[i]) +
                        (outSample - p_outputSample[i]) *
                        (outSample - p_outputSample[i]);
          dist = sqrt(dist);
          double weight = 1.0;
          if (dist >= 0.001)
            weight = 1.0 / dist;
          p_lsqInputLine->Weight(i, weight);
          p_lsqInputSamp->Weight(i, weight);
        }
        p_lsqInputLine->Solve();
        p_lsqInputSamp->Solve();
      }
  
      static std::vector<double> vars;
      if (vars.size() != 2)
        vars.resize(2);
      vars[0] = outLine;
      vars[1] = outSample;
      inLine = p_lsqInputLine->Evaluate(vars);
      inSample = p_lsqInputSamp->Evaluate(vars);
      return true;
    }
  
    PvlGroup WarpTransform::Residuals() {
      PvlGroup errs("Residuals");
      for (int i = 0; i < p_lsqInputLine->Knowns(); i++) {
        PvlKeyword p("POINT" + toString(i + 1));
        p += toString(p_lsqInputLine->Residual(i));
        p += toString(p_lsqInputSamp->Residual(i));
        errs += p;
      }
      return errs;
    }
  }
  