/**
 * @file
 * $Revision: 1.7 $
 * $Date: 2010/06/17 18:59:11 $
 *
 *   Unless noted otherwise, the portions of Isis written by the USGS are
 *   public domain. See individual third-party library and package descriptions
 *   for intellectual property information, user agreements, and related
 *   information.
 *
 *   Although Isis has been used by the USGS, no warranty, expressed or
 *   implied, is made by the USGS as to the accuracy and functioning of such
 *   software and related material nor shall the fact of distribution
 *   constitute any such warranty, and no responsibility is assumed by the
 *   USGS in connection therewith.
 *
 *   For additional information, launch
 *   $ISISROOT/doc//documents/Disclaimers/Disclaimers.html
 *   in a browser or see the Privacy &amp; Disclaimers page on the Isis website,
 *   http://isis.astrogeology.usgs.gov, and the USGS privacy and disclaimers on
 *   http://www.usgs.gov/privacy.html.
 */

#include "SampleScanCameraGroundMap.h"

#include <iostream>
#include <iomanip>

#include <QTime>
#include <QList>
#include <QFile>
#include <QTextStream>

#include "IException.h"
#include "IString.h"
#include "Camera.h"
#include "CameraDistortionMap.h"
#include "CameraFocalPlaneMap.h"
#include "Distance.h"
#include "SampleScanCameraDetectorMap.h"
#include "iTime.h"
#include "Latitude.h"
#include "Longitude.h"
#include "Statistics.h"
#include "SurfacePoint.h"
#include "FunctionTools.h"


using namespace std;
using namespace Isis;

//bool ptXLessThan(const QList<double> l1, const QList<double> l2);

/**
 * @author 2012-05-09 Orrin Thomas
 *
 * @internal
 */
class SampleOffsetFunctor :
  public std::unary_function<double, double > {
  public:

    SampleOffsetFunctor(Isis::Camera *camera, const Isis::SurfacePoint &surPt) {
      m_camera = camera;
      surfacePoint = surPt;
    }


    ~SampleOffsetFunctor() {}


    /** Compute the number of samples between the current sample (i.e., the sample imaged at the et
     *  as set in the camera model) and the sample number where the argument et would hit the focal
     *  plane.
     *  
     * @param et The et at the new postion
     *
     * @return Sample off (see description)
     */
    double operator()(double et) {
      double lookC[3] = {0.0, 0.0, 0.0};
      double ux = 0.0;
      double uy = 0.0;
      double dx = 0.0;
      double dy = 0.0;

      // Verify the time is with the cache bounds
      double startTime = m_camera->cacheStartTime().Et();
      double endTime = m_camera->cacheEndTime().Et();
      if (et < startTime || et > endTime) {
        IString msg = "Ephemeris time passed to SampleOffsetFunctor is not within the image "
                      "cache bounds";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }
     
      m_camera->Sensor::setTime(et);
 
      // Set ground
      if (!m_camera->Sensor::SetGround(surfacePoint, false)) {
        IString msg = "Sensor::SetGround failed for surface point in SampleScanCameraGroundMap.cpp"
                      " SampleOffsetFunctor";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }
   
      // Calculate the undistorted focal plane coordinates

      m_camera->Sensor::LookDirection(lookC);
      ux = m_camera->FocalLength() * lookC[0] / lookC[2];
      uy = m_camera->FocalLength() * lookC[1] / lookC[2];

      // Try to use SetUndistortedFocalPlane, if that does not work use the distorted x,y
      // under the assumption (bad|good) that extrapolating the distortion
      // is causing the distorted x to be way off the sensor, and thus not very good anyway.
      if (m_camera->DistortionMap()->SetUndistortedFocalPlane(ux, uy)) {
        // Get the natural (distorted focal plane coordinates)
        dx = m_camera->DistortionMap()->FocalPlaneX();
        dy = m_camera->DistortionMap()->FocalPlaneY();
      }
      else {
        dx = ux;
        dy = uy;
      }

//      if (!m_camera->FocalPlaneMap()->SetFocalPlane(dx, dy)) {
//        IString msg = "FocalPlaneMap::SetFocalPlane failed for surface point in "
//                      "SampleScanCameraGroundMap.cpp SampleOffsetFunctor";
//        throw IException(IException::Programmer, msg, _FILEINFO_);
//      }

      // return sample offset in pixels
      return (dx/0.005);
    }

/*
    double operator()(double et) {
      double lookC[3] = {0.0, 0.0, 0.0};
      double ux = 0.0;
      double uy = 0.0;
      double dx = 0.0;
      double dy = 0.0;

      // Verify the time is with the cache bounds
      double startTime = m_camera->cacheStartTime().Et();
      double endTime = m_camera->cacheEndTime().Et();
      if (et < startTime || et > endTime) {
        IString msg = "Ephemeris time passed to SampleOffsetFunctor is not within the image "
                      "cache bounds";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }

      m_camera->Sensor::setTime(et);

      // Set ground
      if (!m_camera->Sensor::SetGround(surfacePoint, false)) {
        IString msg = "Sensor::SetGround failed for surface point in SampleScanCameraGroundMap.cpp"
                      " SampleOffsetFunctor";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }

      // Calculate the undistorted focal plane coordinates

      m_camera->Sensor::LookDirection(lookC);
      ux = m_camera->FocalLength() * lookC[0] / lookC[2];
      uy = m_camera->FocalLength() * lookC[1] / lookC[2];

      // Try to use SetUndistortedFocalPlane, if that does not work use the distorted x,y
      // under the assumption (bad|good) that extrapolating the distortion
      // is causing the distorted x to be way off the sensor, and thus not very good anyway.
      if (m_camera->DistortionMap()->SetUndistortedFocalPlane(ux, uy)) {
        // Get the natural (distorted focal plane coordinates)
        dx = m_camera->DistortionMap()->FocalPlaneX();
        dy = m_camera->DistortionMap()->FocalPlaneY();
      }
      else {
        dx = ux;
        dy = uy;
      }

      if (!m_camera->FocalPlaneMap()->SetFocalPlane(dx, dy)) {
        IString msg = "FocalPlaneMap::SetFocalPlane failed for surface point in "
                      "SampleScanCameraGroundMap.cpp SampleOffsetFunctor";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }

      // Return the offset
//      return (m_camera->FocalPlaneMap()->DetectorSampleOffset() -
//              m_camera->FocalPlaneMap()->DetectorSample());
      return (m_camera->FocalPlaneMap()->DetectorSampleOffset() -
              m_camera->FocalPlaneMap()->DetectorSample());
    }
*/
  private:
    SurfacePoint surfacePoint;
    Camera* m_camera;
};


/**
 * @author 2012-05-09 Orrin Thomas
 *
 * @internal
 */
class SensorSurfacePointDistanceFunctor : 
  public std::unary_function<double, double > {

  public:
    SensorSurfacePointDistanceFunctor(Isis::Camera *camera, const Isis::SurfacePoint &surPt) {
      m_camera = camera;
      surfacePoint = surPt;
    }


    ~SensorSurfacePointDistanceFunctor() {}


    double operator()(double et) {
      double s[3], p[3];

      //verify the time is with the cache bounds
      double startTime = m_camera->cacheStartTime().Et();
      double endTime = m_camera->cacheEndTime().Et();

      if (et < startTime || et > endTime) {
        IString msg = "Ephemeris time passed to SensorSurfacePointDistanceFunctor is not within the image "
                      "cache bounds";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }
      m_camera->Sensor::setTime(et);
      if(!m_camera->Sensor::SetGround(surfacePoint, false)) {
         IString msg = "Sensor::SetGround failed for surface point in SampleScanCameraGroundMap.cpp"
                       "SensorSurfacePointDistanceFunctor";
      }
      m_camera->instrumentPosition(s);
      m_camera->Coordinate(p);
      return sqrt((s[0] - p[0]) * (s[0] - p[0]) +
                  (s[1] - p[1]) * (s[1] - p[1]) +
                  (s[2] - p[2]) * (s[2] - p[2]) );  //distance
    }

  private:
    SurfacePoint surfacePoint;
    Camera* m_camera;
};


namespace Isis {

  /** Constructor
   *
   * @param cam pointer to camera model
   */
  SampleScanCameraGroundMap::SampleScanCameraGroundMap(Camera *cam) : CameraGroundMap(cam) {}


  /** Destructor
   *
   */
  SampleScanCameraGroundMap::~SampleScanCameraGroundMap() {}

  /** Compute undistorted focal plane coordinate from ground position
   *
   * @param lat planetocentric latitude in degrees
   * @param lon planetocentric longitude in degrees
   *
   * @return conversion was successful
   */
  bool SampleScanCameraGroundMap::SetGround(const Latitude &lat,
      const Longitude &lon) {
    Distance radius(p_camera->LocalRadius(lat, lon));

    if (radius.isValid()) {
      return SetGround(SurfacePoint(lat, lon, radius));
    }
    else {
      return false;
    }
  }


  /** Compute undistorted focal plane coordinate from ground position
   *
   * @param lat planetocentric latitude in degrees
   * @param lon planetocentric longitude in degrees
   * @param radius local radius in meters
   *
   * @return conversion was successful
   */
  bool SampleScanCameraGroundMap::SetGround(const SurfacePoint &surfacePoint,
                                            const int &approxSample) {
    FindFocalPlaneStatus status = FindFocalPlane(approxSample, surfacePoint);
    if (status == Success)
      return true;

    return false;
  }
  

  /** Compute undistorted focal plane coordinate from ground position
   *
   * @param surfacePoint 3D point on the surface of the planet
   *
   * @return conversion was successful
   */
  bool SampleScanCameraGroundMap::SetGround(const SurfacePoint &surfacePoint) {
    FindFocalPlaneStatus status = FindFocalPlane(-1, surfacePoint);

    if (status == Success)
      return true;

    return false;
  }


  double SampleScanCameraGroundMap::FindSpacecraftDistance(int sample,
      const SurfacePoint &surfacePoint) {

    CameraDetectorMap *detectorMap = p_camera->DetectorMap();
    detectorMap->SetParent(sample, p_camera->ParentLines() / 2);
    if (!p_camera->Sensor::SetGround(surfacePoint, false)) {
      return DBL_MAX;
    }

    return p_camera->SlantDistance();
  }


  SampleScanCameraGroundMap::FindFocalPlaneStatus
      SampleScanCameraGroundMap::FindFocalPlane(const int &approxSample,
                                                const SurfacePoint &surfacePoint) {

    double approxTime=0;
    double approxOffset=0;
    double lookC[3] = {0.0, 0.0, 0.0};
    double ux = 0.0;
    double uy = 0.0;
    const double cacheStart = p_camera->Spice::cacheStartTime().Et();
    const double cacheEnd = p_camera->Spice::cacheEndTime().Et();

    double sampleRate = ((SampleScanCameraDetectorMap *)p_camera->DetectorMap())->SampleRate();

    if (sampleRate == 0.0)
      return Failure;

    SampleOffsetFunctor offsetFunc(p_camera,surfacePoint);
    SensorSurfacePointDistanceFunctor distanceFunc(p_camera,surfacePoint);

    // #1 try use secant method using midsample as start point

    // if an approximate point is given use that as a start point for the secant method root search
    //if (approxSample > 0) {
      //convert the approxSample to an approximate time and offset
      double midSample = p_camera->ParentSamples()/2.0;
      double midLine = p_camera->ParentLines()/2.0;
      p_camera->DetectorMap()->SetParent(midSample, midLine);
      approxTime = p_camera->time().Et();

      approxOffset = offsetFunc(approxTime);

      if (fabs(approxOffset) < 1e-2) { //no need to iteratively improve this root, it's good enough
        p_camera->Sensor::setTime(approxTime);
        //check to make sure the point isn't behind the planet
        if (!p_camera->Sensor::SetGround(surfacePoint, true)) {
          return Failure;
        }
        p_camera->Sensor::LookDirection(lookC);
        ux = p_camera->FocalLength() * lookC[0] / lookC[2];
        uy = p_camera->FocalLength() * lookC[1] / lookC[2];

        p_focalPlaneX = ux;
        p_focalPlaneY = uy;

        return Success;
      }

      double fl, fh, xl, xh;

      //starting times for the secant method, kept within the domain of the cache
      xh = approxTime;
      if (xh + sampleRate < cacheEnd) {
        xl = xh + sampleRate;
      }
      else {
        xl = xh - sampleRate;
      }
double etGuess;
      //starting offsets
      fh = approxOffset;  //the first is already calculated
      fl = offsetFunc(xl);
      double fprevious = approxOffset;

      // Iterate to refine the given approximate time that the instrument imaged the ground point
      for (int j=0; j < 10; j++) {
        if (fl-fh == 0.0)
          break;

        etGuess = xl + (xh - xl) * fl / (fl - fh);

        if (etGuess < cacheStart)
          etGuess = cacheStart;

        if (etGuess > cacheEnd)
          etGuess = cacheEnd;

        double f = offsetFunc(etGuess);

        // if f is not changing anymore, we stop iterating
        double convergedcheck;
        if (f != 0.0)
          convergedcheck = fabs(f-fprevious)/fabs(f);
        else
          convergedcheck = fabs(f-fprevious);
//      if (fabs(convergedcheck) < 1e-5) {
        if (fabs(convergedcheck) < 1e-3) {
          // now, if f is less than our tolerance, we're done
          // if f is not less than our tolerance, we jump down to the next step, determining the
          // coefficients of a quadratic with a "reasonable" interval, which we have hopefully
          // narrowed down in the secant method
//        if (fabs(f) < 1e-2) {
          if (fabs(f) < 1.0) {
            p_camera->Sensor::setTime(etGuess);

            // check to make sure the point isn't behind the planet
            if (!p_camera->Sensor::SetGround(surfacePoint, true))
              break;

            p_camera->Sensor::LookDirection(lookC);
            ux = p_camera->FocalLength() * lookC[0] / lookC[2];
            uy = p_camera->FocalLength() * lookC[1] / lookC[2];

            p_focalPlaneX = ux;
            p_focalPlaneY = uy;

            return Success;
          }
          else
            break;
        }

        // eliminate node farthest away from current best guess and continue
        if (fabs( xl- etGuess) > fabs( xh - etGuess)) {
          xl = etGuess;
          fl = f;
        }
        else {
          xh = etGuess;
          fh = f;
        }
        fprevious = f;
      }

      return Failure;
  }


  /* KLE 2016-09-12 below is the rest of the code from LineScanCameraGroundMap, hoping secant method
 * is initially good enough for pan
    //
    // no estimate given for the approximate line--quadratic approximation root finding
    //
    // The offsets are typically quadratic, so three points will be used to approximate a quadratic
    // as a first order attempt to find the root location(s)

    // The three nodes to be used to approximate the quadratic
    double offsetNodes[3];
    double timeNodes[3];
    double timeAverage;
    double scale;
    QList<double> root;
    QList<double> offset;
    QList<double> dist;

    timeNodes[0] = cacheStart;
    timeNodes[2] = cacheEnd;
    timeNodes[1] = (cacheStart+cacheEnd) / 2.0; //middle time

    double quadPoly[3];
    double temp;

    for (int i=0; i<3; i++) {
      offsetNodes[i] = offsetFunc(timeNodes[i]);
    }
   
    //centralize and normalize the data for stability in root finding
    timeAverage = (timeNodes[0] + timeNodes[1] + timeNodes[2]) / 3.0;
    timeNodes[0] -= timeAverage;
    timeNodes[1] -= timeAverage;
    timeNodes[2] -= timeAverage;

    scale = 1.0 / sqrt((timeNodes[0] - timeNodes[2]) * 
                       (timeNodes[0] - timeNodes[2]) + 
                       (offsetNodes[0] - offsetNodes[2]) * 
                       (offsetNodes[0] - offsetNodes[2]));

    timeNodes[0] *= scale;
    timeNodes[1] *= scale;
    timeNodes[2] *= scale;

    offsetNodes[0] *= scale;
    offsetNodes[1] *= scale;
    offsetNodes[2] *= scale;

    // Use lagrange interpolating polynomials to find the coefficients of the quadratic, 
    // there are many ways to do this; I chose to do it this way because it is pretty straight 
    // forward and cheap
    quadPoly[0] = quadPoly[1] = quadPoly[2] = 0.0;

    temp = offsetNodes[0] / ((timeNodes[0] - timeNodes[1]) * (timeNodes[0] - timeNodes[2]));
    quadPoly[0] += temp;
    quadPoly[1] += temp * (-timeNodes[1] - timeNodes[2]);
    quadPoly[2] += temp * timeNodes[1] * timeNodes[2];

    temp = offsetNodes[1] / ((timeNodes[1] - timeNodes[0]) * (timeNodes[1] - timeNodes[2]));
    quadPoly[0] += temp;
    quadPoly[1] += temp * (-timeNodes[0] - timeNodes[2]);
    quadPoly[2] += temp * timeNodes[0] * timeNodes[2];
      
    temp = offsetNodes[2] / ((timeNodes[2] - timeNodes[0]) * (timeNodes[2] - timeNodes[1]));
    quadPoly[0] += temp;
    quadPoly[1] += temp * (-timeNodes[0] - timeNodes[1]);
    quadPoly[2] += temp * timeNodes[0] * timeNodes[1];

    // Now that we have the coefficients of the quadratic look for roots 
    // (see Numerical Recipes Third Edition page 227)
    temp = quadPoly[1] * quadPoly[1] - 4.0 * quadPoly[0] * quadPoly[2];  //discriminant
    if (temp < 0.0) {
      return Failure;  //there are apparently not any real roots on this image
    }

    if (quadPoly[1] >= 0.0) {
      temp = -0.5 * (quadPoly[1] + sqrt(temp));
    }
    else {
      temp = -0.5 * (quadPoly[1] - sqrt(temp));
    }

    if (quadPoly[0] != 0.0) {
      root.push_back(temp/quadPoly[0]);
    }

    if (quadPoly[2] != 0.0) {
      root.push_back(quadPoly[2]/temp);
    }

    //check to see if the roots are in the time interval of the cache
    for (int i=root.size()-1; i>=0; i--) {
      if ( root[i] < timeNodes[0] || root[i] > timeNodes[2] ) {
        root.removeAt(i);
      }
    }
     
    //return the calculated roots to the original system
    for (int i=0; i<root.size(); i++) {
      root[i] = root[i]/scale + timeAverage;
    }

    if (root.size() == 0) {
      return Failure;  //there are apparently not any roots on this image
    }

    // At the time of this writing ISIS made no attempt to support any sensors that were not "1 to 1".
    // Meaning that imaged the same point on the ground in multiple lines of the image
    // therefore we must somehow reduce multiple possible roots to a single one,  the legacy 
    // code (replaced with this code) did this based on distance from the sensor to the target
    // the shortest distance being the winner.  For legacy consistency I have used the same logic below.  

    for (int i=0; i<root.size(); i++) {  //Offset/dist calculation loop
      dist << distanceFunc(root[i]);
      offset << offsetFunc(root[i]);
    }

    // Save the root with the smallest dist
    int j=0;   
    for (int i=1, j=0; i<root.size(); i++) {
      if (dist[i] < dist[j]) j=i;
    }      
      
    approxTime = root[j];  // Now we have our time
    approxOffset = offset[j];  // The offsets are saved to avoid recalculating it later

    if (fabs(approxOffset) < 1.0e-2) { // No need to iteratively improve this root, it's good enough
      p_camera->Sensor::setTime(approxTime);

      // Check to make sure the point isn't behind the planet
      if (!p_camera->Sensor::SetGround(surfacePoint, true)) {
        return Failure;
      }
       
      p_camera->Sensor::LookDirection(lookC);
      ux = p_camera->FocalLength() * lookC[0] / lookC[2];
      uy = p_camera->FocalLength() * lookC[1] / lookC[2];
     
      p_focalPlaneX = ux;
      p_focalPlaneY = uy;

      return Success;
    }

    //
    // no estimate given for the approximate line, quadratic approximation insufficient, use Brent's
    // method
    //
    // The offsets are typically quadratic, so three points will be used to approximate a quadratic
    // as a first order attempt to find the root location(s)

    // The above sections are sufficient for finding the correct times for the vast majority of 
    // back projection solutions.  The following section exists for those few particularly
    // stuborn problems.  They are typically characterised by being significantly non-quadratic.
    // Examples mostly include images with very long exposure times.
    //
    // Further, while the preceeding sections is intended to be fast, this section is intended to be
    // thurough.  Brents method (Numerical Recipies 454 - 456) will be used to find all the roots,
    // that are bracketed by the five points defined in the quadratic solution method above.
    // The root with the shortest distance to the camera will be returned

    // Get everything ordered for iteration combine and sort the five already defined points
    QList <QList <double> > pts;

    for (int i=0; i<3; i++) {
      QList <double> pt;
      pt << timeNodes[i] / scale + timeAverage;
      pt << offsetNodes[i] / scale;
      pts << pt; 
    }
 
    for (int i=0; i<root.size(); i++) {
      QList <double> pt;
      pt << root[i];
      pt << offset[i];
      pts << pt; 
    }
  
    qSort(pts.begin(), pts.end(), ptXLessThan);
    
    root.clear();
    for (int i=1; i<pts.size(); i++) {
      // If the signs of the two offsets are not the same they bracket at least one root
      if ( (pts[i-1][1] > 0) - (pts[i-1][1] < 0) != (pts[i][1] > 0) - (pts[i][1] < 0) ) {
        double temp;
        if (FunctionTools::brentsRootFinder <LineOffsetFunctor> (offsetFunc, pts[i-1], pts[i],
                                                                 1.0e-3, 200, temp)) {
          root << temp;

        }
      }      
    }

    // Discard any roots that are looking through the planet
    for (int i = root.size()-1; i>=0; i--) {
      p_camera->Sensor::setTime(root[i]);
      //check to make sure the point isn't behind the planet
      if (!p_camera->Sensor::SetGround(surfacePoint, true)) {
        root.removeAt(i);
      }
    }

    // If none of the roots remain...
    if (root.size() == 0) {
      return Failure;
    }

    // Choose from the remaining roots, the solution with the smallest distance to target
    dist.clear();
    offset.clear();
    for (int i=0; i<root.size(); i++) {  // Offset/dist calculation loop
      dist << distanceFunc(root[i]);
      offset << offsetFunc(root[i]);
    }

    // Save the root with the smallest dist
    for (int i=1, j=0; i<root.size(); i++) {
      if (dist[i] < dist[j]) j=i;
    }

    p_camera->Sensor::setTime(root[j]);

    // No need to make sure the point isn't behind the planet, it was done above
    p_camera->Sensor::LookDirection(lookC);
    ux = p_camera->FocalLength() * lookC[0] / lookC[2];
    uy = p_camera->FocalLength() * lookC[1] / lookC[2];
     
    p_focalPlaneX = ux;
    p_focalPlaneY = uy;
       
    return Success;
  }
*/
}


//bool ptXLessThan(const QList<double> l1, const QList<double> l2) {
//  return l1[0] < l2[0];
//}

