/*****************************************************************************/

// Copyright 2008 Adobe Systems Incorporated

// All Rights Reserved.

//

// NOTICE:  Adobe permits you to use, modify, and distribute this file in

// accordance with the terms of the Adobe license agreement accompanying it.

/*****************************************************************************/

/* $Id: //mondo/dng_sdk_1_4/dng_sdk/source/dng_lens_correction.cpp#1 $ */ 

/* $DateTime: 2012/05/30 13:28:51 $ */

/* $Change: 832332 $ */

/* $Author: tknoll $ */

/*****************************************************************************/

#include <cfloat>

#include <limits.h>

#include "dng_1d_table.h"

#include "dng_assertions.h"

#include "dng_bottlenecks.h"

#include "dng_exceptions.h"

#include "dng_filter_task.h"

#include "dng_globals.h"

#include "dng_host.h"

#include "dng_image.h"

#include "dng_lens_correction.h"

#include "dng_negative.h"

#include "dng_safe_arithmetic.h"

#include "dng_sdk_limits.h"

#include "dng_tag_values.h"

#include "dng_utils.h"

/*****************************************************************************/

dng_warp_params::dng_warp_params ()

  : fPlanes (1)

  , fCenter (0.5, 0.5)

  {

  }

/*****************************************************************************/

dng_warp_params::dng_warp_params (uint32 planes,

                  const dng_point_real64 &center)

  : fPlanes (planes)

  , fCenter (center)

  {

  DNG_ASSERT (planes >= 1,         "Too few planes." );

  DNG_ASSERT (planes <= kMaxColorPlanes, "Too many planes.");

  DNG_ASSERT (fCenter.h >= 0.0 && fCenter.h <= 1.0, 

        "Center (horizontal) out of range.");

  DNG_ASSERT (fCenter.v >= 0.0 && fCenter.v <= 1.0, 

        "Center (vertical) out of range.");

  }

/*****************************************************************************/

dng_warp_params::~dng_warp_params ()

  {

  }

/*****************************************************************************/

bool dng_warp_params::IsNOPAll () const

  {

  return IsRadNOPAll () && 

       IsTanNOPAll ();

  }

/*****************************************************************************/

bool dng_warp_params::IsNOP (uint32 plane) const

  {

  return IsRadNOP (plane) &&

       IsTanNOP (plane);

  }

/*****************************************************************************/

bool dng_warp_params::IsRadNOPAll () const

  {

  for (uint32 plane = 0; plane < fPlanes; plane++)

    {

    if (!IsRadNOP (plane))

      {

      return false;

      }

    }

  return true;

  }

/*****************************************************************************/

bool dng_warp_params::IsRadNOP (uint32 /* plane */) const

  {

  return false;

  }

/*****************************************************************************/

bool dng_warp_params::IsTanNOPAll () const

  {

  for (uint32 plane = 0; plane < fPlanes; plane++)

    {

    if (!IsTanNOP (plane))

      {

      return false;

      }

    }

  return true;

  }

/*****************************************************************************/

bool dng_warp_params::IsTanNOP (uint32 /* plane */) const

  {

  return false;

  }

/*****************************************************************************/

bool dng_warp_params::IsValid () const

  {

  if (fPlanes < 1 || fPlanes > kMaxColorPlanes)

    {

    return false;

    }

  if (fCenter.h < 0.0 || 

    fCenter.h > 1.0 ||

    fCenter.v < 0.0 ||

    fCenter.v > 1.0)

    {

    return false;

    }

  return true;

  }

/*****************************************************************************/

bool dng_warp_params::IsValidForNegative (const dng_negative &negative) const

  {

  if (!IsValid ())

    {

    return false;

    }

  if ((fPlanes != 1) &&

    (fPlanes != negative.ColorChannels ()))

    {

    return false;

    }

  return true;

  }

/*****************************************************************************/

real64 dng_warp_params::EvaluateInverse (uint32 plane,

                     real64 y) const

  {

  const uint32 kMaxIterations = 30;

  const real64 kNearZero    = 1.0e-10;

  real64 x0 = 0.0;

  real64 y0 = Evaluate (plane,

              x0);

  real64 x1 = 1.0;

  real64 y1 = Evaluate (plane,

              x1);

  for (uint32 iteration = 0; iteration < kMaxIterations; iteration++)   

    {

    if (Abs_real64 (y1 - y0) < kNearZero)

      {

      break;

      }

    const real64 x2 = Pin_real64 (0.0, 

                    x1 + (y - y1) * (x1 - x0) / (y1 - y0),

                    1.0);

    const real64 y2 = Evaluate (plane,

                  x2);

    x0 = x1;

    y0 = y1;

    x1 = x2;

    y1 = y2;

    }

  return x1;

  }

/*****************************************************************************/

dng_point_real64 dng_warp_params::EvaluateTangential2 (uint32 plane,

                             const dng_point_real64 &diff) const

  {

  const real64 dvdv = diff.v * diff.v;

  const real64 dhdh = diff.h * diff.h;

  const real64 rr = dvdv + dhdh;

  dng_point_real64 diffSqr (dvdv,

                dhdh);

  return EvaluateTangential (plane,

                 rr,

                 diff,

                 diffSqr);

  }

/*****************************************************************************/

dng_point_real64 dng_warp_params::EvaluateTangential3 (uint32 plane,

                             real64 r2,

                             const dng_point_real64 &diff) const

  {

  dng_point_real64 diffSqr (diff.v * diff.v,

                diff.h * diff.h);

  return EvaluateTangential (plane,

                 r2,

                 diff,

                 diffSqr);

  }

/*****************************************************************************/

void dng_warp_params::Dump () const

  {

  #if qDNGValidate

  printf ("Planes: %u\n", (unsigned) fPlanes);

  printf ("  Optical center:\n"

      "    h = %.6lf\n"

      "    v = %.6lf\n",

      fCenter.h,

      fCenter.v);

  #endif

  }

/*****************************************************************************/

dng_warp_params_rectilinear::dng_warp_params_rectilinear ()

  : dng_warp_params ()

  {

  for (uint32 plane = 0; plane < kMaxColorPlanes; plane++)

    {

    fRadParams [plane] = dng_vector (4);

    fTanParams [plane] = dng_vector (2);

    fRadParams [plane][0] = 1.0;

    }

  }

/*****************************************************************************/

dng_warp_params_rectilinear::dng_warp_params_rectilinear (uint32 planes,

                              const dng_vector radParams [],

                              const dng_vector tanParams [],

                              const dng_point_real64 &center)

  : dng_warp_params (planes, 

             center)

  {

  for (uint32 i = 0; i < fPlanes; i++)

    {

    fRadParams [i] = radParams [i];

    fTanParams [i] = tanParams [i];

    }

  }

/*****************************************************************************/

dng_warp_params_rectilinear::~dng_warp_params_rectilinear ()

  {

  }

/*****************************************************************************/

bool dng_warp_params_rectilinear::IsRadNOP (uint32 plane) const

  {

  DNG_ASSERT (plane < fPlanes, "plane out of range.");

  const dng_vector &r = fRadParams [plane];

  return (r [0] == 1.0 &&

      r [1] == 0.0 &&

      r [2] == 0.0 &&

      r [3] == 0.0);

  }

/*****************************************************************************/

bool dng_warp_params_rectilinear::IsTanNOP (uint32 plane) const

  {

  DNG_ASSERT (plane < fPlanes, "plane out of range.");

  const dng_vector &t = fTanParams [plane];

  return (t [0] == 0.0 &&

      t [1] == 0.0);

  }

/*****************************************************************************/

bool dng_warp_params_rectilinear::IsValid () const

  {

  for (uint32 plane = 0; plane < fPlanes; plane++)

    {

    if (fRadParams [plane].Count () != 4)

      {

      return false;

      }

    if (fTanParams [plane].Count () < 2)

      {

      return false;

      }

    }

  return dng_warp_params::IsValid ();

  }

/*****************************************************************************/

void dng_warp_params_rectilinear::PropagateToAllPlanes (uint32 totalPlanes)

  {

  for (uint32 plane = fPlanes; plane < totalPlanes; plane++)

    {

    fRadParams [plane] = fRadParams [0];

    fTanParams [plane] = fTanParams [0];

    }

  }

/*****************************************************************************/

real64 dng_warp_params_rectilinear::Evaluate (uint32 plane,

                        real64 x) const

  {

  const dng_vector &K = fRadParams [plane]; // Coefficients.

  const real64 x2 = x * x;

  return x * (K [0] + x2 * (K [1] + x2 * (K [2] + x2 * K [3])));

  }

/*****************************************************************************/

real64 dng_warp_params_rectilinear::EvaluateRatio (uint32 plane,

                           real64 r2) const

  {

  const dng_vector &K = fRadParams [plane]; // Coefficients.

  return K [0] + r2 * (K [1] + r2 * (K [2] + r2 * K [3]));

  }

/*****************************************************************************/

dng_point_real64 dng_warp_params_rectilinear::EvaluateTangential (uint32 plane,

                                  real64 r2,

                                  const dng_point_real64 &diff,

                                  const dng_point_real64 &diff2) const

  {

  const real64 kt0 = fTanParams [plane][0];

  const real64 kt1 = fTanParams [plane][1];

  const real64 dh = diff.h;

  const real64 dv = diff.v;

  const real64 dhdh = diff2.h;

  const real64 dvdv = diff2.v;

  return dng_point_real64 (kt0 * (r2 + 2.0 * dvdv) + (2.0 * kt1 * dh * dv),  // v

               kt1 * (r2 + 2.0 * dhdh) + (2.0 * kt0 * dh * dv)); // h

  }

/*****************************************************************************/

real64 dng_warp_params_rectilinear::MaxSrcRadiusGap (real64 maxDstGap) const

  {

  real64 maxSrcGap = 0.0;

  for (uint32 plane = 0; plane < fPlanes; plane++)

    {

    const dng_vector &coefs = fRadParams [plane];

    const real64 k3 = coefs [1];

    const real64 k5 = coefs [2];

    const real64 k7 = coefs [3];

    //

    //  Let f (r) be the radius warp function. Consider the function

    //

    //    gap (r) = f (r + maxDstGap) - f (r)

    //

    //  We wish to maximize gap (r) over the domain [0, 1 - maxDstGap]. This will

    //  give us a reasonable upper bound on the src tile size, independent of

    //  dstBounds.

    //

    //  As usual, we maximize gap (r) by examining its critical points, i.e., by

    //  considering the roots of its derivative which lie in the domain [0, 1 -

    //  maxDstGap]. gap' (r) is a 5th-order polynomial. One of its roots is

    //  -maxDstGap / 2, which is negative and hence lies outside the domain of

    //  interest. This leaves 4 other possible roots. We solve for these

    //  analytically below.

    //

    real64 roots [4];

    uint32 numRoots = 0;

    if (k7 == 0.0)

      {

      if (k5 == 0.0)

        {

        // No roots in [0,1].

        }

      else

        {

        // k7 is zero, but k5 is non-zero. At most two real roots. 

        const real64 discrim = 25.0 * (-6.0 * k3 * k5 - 5.0 * k5 * maxDstGap * maxDstGap);

        if (discrim >= 0.0)

          {

          // Two real roots.

          const real64 scale    =  0.1 * k5;

          const real64 offset   = -5.0 * maxDstGap * k5;

          const real64 sDiscrim =  sqrt (discrim);

          roots [numRoots++] = scale * (offset + sDiscrim);

          roots [numRoots++] = scale * (offset - sDiscrim);

          }

        }

      }

    else

      {

      // k7 is non-zero. Up to 4 real roots.

      const real64 d  = maxDstGap;

      const real64 d2 = d  * d;

      const real64 d4 = d2 * d2;

      const real64 discrim = 25.0 * k5 * k5 

                 - 63.0 * k3 * k7 

                 + 35.0 * d2 * k5 * k7 

                 + 49.0 * d4 * k7 * k7;

      if (discrim >= 0.0)

        {

        const real64 sDiscrim = 4.0 * k7 * sqrt (discrim);

        const real64 offset = -20.0 * k5 * k7 - 35.0 * d2 * k7 * k7;

        const real64 discrim1 = offset - sDiscrim;

        const real64 discrim2 = offset + sDiscrim;

        const real64 scale = sqrt (21.0) / (42.0 * k7);

        if (discrim1 >= 0.0)

          {

          const real64 offset1 = -d * 0.5;

          const real64 sDiscrim1 = scale * sqrt (discrim1);

          roots [numRoots++] = offset1 + sDiscrim1;

          roots [numRoots++] = offset1 - sDiscrim1;

          }

        if (discrim2 >= 0.0)

          {

          const real64 offset2 = -d * 0.5;

          const real64 sDiscrim2 = scale * sqrt (discrim2);

          roots [numRoots++] = offset2 + sDiscrim2;

          roots [numRoots++] = offset2 - sDiscrim2;

          }

        }

      }

    real64 planeMaxSrcGap = 0.0;

    // Examine the endpoints.

      {

      // Check left endpoint: f (maxDstGap) - f (0). Remember that f (0) == 0.

      const real64 gap1 = Evaluate (plane, maxDstGap);

      planeMaxSrcGap = Max_real64 (planeMaxSrcGap, gap1);

      // Check right endpoint: f (1) - f (1 - maxDstGap).

      const real64 gap2 = Evaluate (plane, 1.0)

                - Evaluate (plane, 1.0 - maxDstGap);

      planeMaxSrcGap = Max_real64 (planeMaxSrcGap, gap2);

      }

    // Examine the roots we found, if any.

    for (uint32 i = 0; i < numRoots; i++)

      {

      const real64 r = roots [i];

      if (r > 0.0 && r < 1.0 - maxDstGap)

        {

        const real64 gap = Evaluate (plane, r + maxDstGap)

                 - Evaluate (plane, r);

        planeMaxSrcGap = Max_real64 (planeMaxSrcGap, gap);

        }

      }

    maxSrcGap = Max_real64 (maxSrcGap,

                planeMaxSrcGap);

    }

  return maxSrcGap;

  }

/*****************************************************************************/

dng_point_real64 dng_warp_params_rectilinear::MaxSrcTanGap (dng_point_real64 minDst,

                              dng_point_real64 maxDst) const

  {

  const real64 v [] = { minDst.v, maxDst.v, 0.0 };

  const real64 h [] = { minDst.h, maxDst.h, 0.0 };

  dng_point_real64 maxGap;

  for (uint32 plane = 0; plane < fPlanes; plane++)

    {

    real64 hMin = +FLT_MAX;

    real64 hMax = -FLT_MAX;

    real64 vMin = +FLT_MAX;

    real64 vMax = -FLT_MAX;

    for (uint32 i = 0; i < 3; i++)

      {

      for (uint32 j = 0; j < 3; j++)

        {

        dng_point_real64 dstDiff (v [i], 

                      h [j]);

        dng_point_real64 srcDiff = EvaluateTangential2 (plane,

                                dstDiff);

        hMin = Min_real64 (hMin, srcDiff.h);

        hMax = Max_real64 (hMax, srcDiff.h);

        vMin = Min_real64 (vMin, srcDiff.v);

        vMax = Max_real64 (vMax, srcDiff.v);

        }

      }

    const real64 hGap = hMax - hMin;

    const real64 vGap = vMax - vMin;

    maxGap.h = Max_real64 (maxGap.h, hGap);

    maxGap.v = Max_real64 (maxGap.v, vGap);

    }

  return maxGap;

  }

/*****************************************************************************/

void dng_warp_params_rectilinear::Dump () const

  {

  #if qDNGValidate

  dng_warp_params::Dump ();

  for (uint32 plane = 0; plane < fPlanes; plane++)

    {

    printf ("  Plane %u:\n", (unsigned) plane);

    printf ("    Radial params:     %.6lf, %.6lf, %.6lf, %.6lf\n",

        fRadParams [plane][0],

        fRadParams [plane][1],

        fRadParams [plane][2],

        fRadParams [plane][3]);

    printf ("    Tangential params: %.6lf, %.6lf\n",

        fTanParams [plane][0],

        fTanParams [plane][1]);

    }   

  #endif

  }

/*****************************************************************************/

dng_warp_params_fisheye::dng_warp_params_fisheye ()

  : dng_warp_params ()

  {

  for (uint32 plane = 0; plane < kMaxColorPlanes; plane++)

    {

    fRadParams [plane] = dng_vector (4);

    }

  }

/*****************************************************************************/

dng_warp_params_fisheye::dng_warp_params_fisheye (uint32 planes,

                          const dng_vector radParams [],

                          const dng_point_real64 &center)

  : dng_warp_params (planes, center)

  {

  for (uint32 i = 0; i < fPlanes; i++)

    {

    fRadParams [i] = radParams [i];

    }

  }

/*****************************************************************************/

dng_warp_params_fisheye::~dng_warp_params_fisheye ()

  {

  }

/*****************************************************************************/

bool dng_warp_params_fisheye::IsRadNOP (uint32 /* plane */) const

  {

  return false;

  }

/*****************************************************************************/

bool dng_warp_params_fisheye::IsTanNOP (uint32 /* plane */) const

  {

  return true;

  }

/*****************************************************************************/

bool dng_warp_params_fisheye::IsValid () const

  {

  for (uint32 plane = 0; plane < fPlanes; plane++)

    {

    if (fRadParams [plane].Count () != 4)

      {

      return false;

      }

    }

  return dng_warp_params::IsValid ();

  }

/*****************************************************************************/

void dng_warp_params_fisheye::PropagateToAllPlanes (uint32 totalPlanes)

  {

  for (uint32 plane = fPlanes; plane < totalPlanes; plane++)

    {

    fRadParams [plane] = fRadParams [0];

    }

  }

/*****************************************************************************/

real64 dng_warp_params_fisheye::Evaluate (uint32 plane,

                      real64 r) const

  {

  const real64 t = atan (r);

  const dng_vector &K = fRadParams [plane];

  const real64 t2 = t * t;

  return t * (K [0] + t2 * (K [1] + t2 * (K [2] + t2 * K [3])));

  }

/*****************************************************************************/

real64 dng_warp_params_fisheye::EvaluateRatio (uint32 plane,

                         real64 rSqr) const

  {

  const real64 eps = 1.0e-12;

  if (rSqr < eps)

    {

    // r is very close to zero.

    return 1.0;

    }

  const real64 r = sqrt (rSqr);

  return Evaluate (plane, r) / r;

  }

/*****************************************************************************/

dng_point_real64 dng_warp_params_fisheye::EvaluateTangential (uint32 /* plane */,

                                real64 /* r2 */,

                                const dng_point_real64 & /* diff */,

                                const dng_point_real64 & /* diff2 */) const

  {

  // This fisheye model does not support tangential warping.

  ThrowProgramError ();

  return dng_point_real64 (0.0, 0.0);

  }

/*****************************************************************************/

real64 dng_warp_params_fisheye::MaxSrcRadiusGap (real64 maxDstGap) const

  {

  //

  //  Let f (r) be the radius warp function. Consider the function

  //

  //    gap (r) = f (r + maxDstGap) - f (r)

  //

  //  We wish to maximize gap (r) over the domain [0, 1 - maxDstGap]. This will

  //  give us a reasonable upper bound on the src tile size, independent of

  //  dstBounds.

  //

  //  Ideally, we'd like to maximize gap (r) by examining its critical points,

  //  i.e., by considering the roots of its derivative which lie in the domain [0,

  //  1 - maxDstGap]. However, gap' (r) is too complex to find its roots

  //  analytically.

  //

  real64 maxSrcGap = 0.0;

  DNG_REQUIRE (maxDstGap > 0.0, "maxDstGap must be positive.");

  const real64 kMaxValue = 1.0 - maxDstGap;

  const uint32 kSteps = 128;

  const real64 kNorm = kMaxValue / real64 (kSteps - 1);

  for (uint32 plane = 0; plane < fPlanes; plane++)

    {

    for (uint32 i = 0; i < kSteps; i++)

      {

      const real64 tt = i * kNorm;

      const real64 gap = Evaluate (plane, tt + maxDstGap)

               - Evaluate (plane, tt);

      maxSrcGap = Max_real64 (maxSrcGap,

                  gap);

      }

    }

  return maxSrcGap;

  }

/*****************************************************************************/

dng_point_real64 dng_warp_params_fisheye::MaxSrcTanGap (dng_point_real64 /* minDst */,

                            dng_point_real64 /* maxDst */) const

  {

  // This fisheye model does not support tangential distortion.

  return dng_point_real64 (0.0, 0.0);

  }

/*****************************************************************************/

void dng_warp_params_fisheye::Dump () const

  {

  #if qDNGValidate

  dng_warp_params::Dump ();

  for (uint32 plane = 0; plane < fPlanes; plane++)

    {

    printf ("  Plane %u:\n", (unsigned) plane);

    printf ("    Radial params:     %.6lf, %.6lf, %.6lf, %.6lf\n",

        fRadParams [plane][0],

        fRadParams [plane][1],

        fRadParams [plane][2],

        fRadParams [plane][3]);

    }   

  #endif

  }

/*****************************************************************************/

class dng_filter_warp: public dng_filter_task

  {

  protected:

    AutoPtr<dng_warp_params> fParams;

    dng_point_real64 fCenter;

    dng_resample_weights_2d fWeights;

    real64 fNormRadius;

    real64 fInvNormRadius;