/**
 * @file
 * $Revision: 1.2 $
 * $Date: 2008/11/24 16:40:30 $
 *
 *   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 <cmath>

#include "IString.h"
#include "MarciDistortionMap.h"

namespace Isis {
  /** Camera distortion map constructor
   *
   * Create a camera distortion map.  This class maps between distorted
   * and undistorted focal plane x/y's.  The default mapping is the
   * identity, that is, the focal plane x/y and undistorted focal plane
   * x/y will be identical.
   *
   * @param parent        the parent camera that will use this distortion map
   * @param zDirection    the direction of the focal plane Z-axis
   *                      (either 1 or -1)
   *
   */
  MarciDistortionMap::MarciDistortionMap(Camera *parent, int naifIkCode) : CameraDistortionMap(parent) {
    QString odkkey = "INS" + toString(naifIkCode) + "_DISTORTION_COEFFS";

    for(int i = 0; i < 4; i++) {
      p_odk.push_back(p_camera->getDouble(odkkey, i));
    }
  }

  /** Compute undistorted focal plane x/y
   *
   * Compute undistorted focal plane x/y given a distorted focal plane x/y.
   *
   * @param dx distorted focal plane x in millimeters
   * @param dy distorted focal plane y in millimeters
   *
   * @return if the conversion was successful
   * @see SetDistortion
   * @todo Generalize polynomial equation
   */
  bool MarciDistortionMap::SetFocalPlane(const double dx, const double dy) {
    p_focalPlaneX = dx;
    p_focalPlaneY = dy;

    double dxPix = p_focalPlaneX / p_camera->PixelPitch();
    double dyPix = p_focalPlaneY / p_camera->PixelPitch();

    // Get the distance from the focal plane center and if we are close
    // then skip the distortion
    double radialDist2 = (dxPix * dxPix) + (dyPix * dyPix);

    if(radialDist2 <= 1.0E-3) {
      p_undistortedFocalPlaneX = dx;
      p_undistortedFocalPlaneY = dy;
      return true;
    }

    // Ok we need to apply distortion correction
    double radialDist4 = radialDist2 * radialDist2;
    double radialDist6 = radialDist4 * radialDist2;

    double uRadialDist = p_odk[0] + radialDist2 * p_odk[1] +
                         radialDist4 * p_odk[2] +
                         radialDist6 * p_odk[3];

    // double radialDist = sqrt(radialDist2);
    double uxPix = dxPix * uRadialDist;
    double uyPix = dyPix * uRadialDist;


    p_undistortedFocalPlaneX = uxPix * p_camera->PixelPitch();
    p_undistortedFocalPlaneY = uyPix * p_camera->PixelPitch();

    return true;
  }

  /** Compute distorted focal plane x/y
   *
   * Compute distorted focal plane x/y given an undistorted focal plane x/y.
   *
   * @param ux undistorted focal plane x in millimeters
   * @param uy undistorted focal plane y in millimeters
   *
   * @return if the conversion was successful
   * @see SetDistortion
   * @todo Generalize polynomial equation
   * @todo Figure out a better solution for divergence condition
   */
  bool MarciDistortionMap::SetUndistortedFocalPlane(const double ux,
      const double uy) {
    p_undistortedFocalPlaneX = ux;
    p_undistortedFocalPlaneY = uy;

    double uxPix = ux / p_camera->PixelPitch();
    double uyPix = uy / p_camera->PixelPitch();

    double dxPix = GuessDx(uxPix);
    double dyPix = uyPix;

    // Get the distance from the focal plane center and if we are close
    // then skip the distortion
    double Ru = sqrt((uxPix * uxPix) + (uyPix * uyPix));

    if(Ru <= 1.0E-6) {
      p_focalPlaneX = ux;
      p_focalPlaneY = uy;
      return true;
    }

    double delta = 1.0;
    int iter = 0;

    double Rd = sqrt((dxPix * dxPix) + (dyPix * dyPix));

    while(fabs(delta) > 1E-9) {
      if(fabs(delta) > 1E30 || iter > 50) {
        return false;
      }

      double Rd2 = Rd * Rd;
      double Rd3 = Rd2 * Rd;
      double Rd4 = Rd3 * Rd;
      double Rd5 = Rd4 * Rd;
      double Rd6 = Rd5 * Rd;

      double fRd = p_odk[0] + Rd2 * p_odk[1] +
                   Rd4 * p_odk[2] +
                   Rd6 * p_odk[3] - Ru * (1.0 / Rd);

      double fRd2 = 2 * p_odk[1] * Rd +
                    4 * p_odk[2] * Rd3 +
                    6 * p_odk[3] * Rd5 +
                    Ru * (1.0 / Rd2);

      delta = fRd / fRd2;

      Rd = Rd - delta;

      iter ++;
    }

    dxPix = uxPix * (Rd / Ru);
    dyPix = uyPix * (Rd / Ru);

    p_focalPlaneX = dxPix * p_camera->PixelPitch();
    p_focalPlaneY = dyPix * p_camera->PixelPitch();

    return true;
  }

  double MarciDistortionMap::GuessDx(double uX) {
    // We're using natural log fits, but if uX < 1 the fit doesnt work
    if(fabs(uX) < 1) return uX;

    if(p_filter == 0) { // BLUE FILTER
      return (1.4101 * log(fabs(uX)));
    }

    else if(p_filter == 1) { // GREEN FILTER
      return (1.1039 * log(fabs(uX)));
    }

    else if(p_filter == 2) { // ORANGE FILTER
      return (0.8963 * log(fabs(uX)) + 2.1644);
    }

    else if(p_filter == 3) { // RED FILTER
      return (1.1039 * log(fabs(uX)));
    }

    else if(p_filter == 4) { // NIR FILTER
      return (1.4101 * log(fabs(uX)));
    }

    return uX;
  }
}

