// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2015 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

/*
The implementation is based on

    [1] "Anatomy of the SIFT Method."
        I. Rey Otero  and  M. Delbracio
        Image Processing Online, 2013.
        http://www.ipol.im/pub/algo/rd_anatomy_sift/
*/

#ifndef OPENMVG_FEATURES_SIFT_SIFT_DESCRIPTOR_EXTRACTOR_HPP
#define OPENMVG_FEATURES_SIFT_SIFT_DESCRIPTOR_EXTRACTOR_HPP

#include <algorithm>
#include <limits>
#include <vector>

#include "openMVG/features/feature.hpp"
#include "openMVG/features/sift/hierarchical_gaussian_scale_space.hpp"
#include "openMVG/features/sift/sift_keypoint.hpp"
#include "openMVG/image/image_container.hpp"

namespace openMVG{
namespace features{
namespace sift{

/**
 ** @brief Class managing SIFT descriptor computation (orientation and description)
 **/
class Sift_DescriptorExtractor
{
public:
  /**
   ** @brief Constructor
   ** @param nb_orientation_histogram_bin Number of orientation histogram bin to compute orientations of a keypoint
   ** @param m_orientation_scale Sets the width of the orientation patch
   ** @param nb_smoothing_step Number of smoothing step applied to orientation histogram before computing orientations of a keypoint
   ** @param percentage_keep Percentage of highest orientation histogram above which a new orientation is computed
   ** @param b_root_sift Compute the root sift normalized histogram
   ** @param descriptor_scale Scale of descriptor box computation
   ** @param nb_split2d Number of split (in x and in y) applied to descriptor histogram
   ** @param nb_split_angle Number of split (in z) applied to descriptor histogram
   ** @param clip_value Value above this threshold are clamped before computing final descriptor
   **/
  Sift_DescriptorExtractor
  (
    // Orientation computation parameters
    const int nb_orientation_histogram_bin = 36,
    const float orientation_scale = 1.5f,
    const int nb_smoothing_step = 6,
    const float percentage_keep = 0.8f,
    // Descriptor computation parameters
    const bool b_root_sift = true,
    const float descriptor_scale = 6.0f,
    const int nb_split2d = 4,
    const int nb_split_angle = 8,
    const float clip_value = 0.2f
  ):
  m_nb_orientation_histogram_bin(nb_orientation_histogram_bin),
  m_orientation_scale(orientation_scale),
  m_nb_smoothing_step(nb_smoothing_step),
  m_percentage_keep(percentage_keep),
  m_b_root_sift(b_root_sift),
  m_descriptor_scale(descriptor_scale),
  m_nb_split2d(nb_split2d),
  m_nb_split_angle(nb_split_angle),
  m_clip_value(clip_value)
  {
  }

  /**
  * @brief Compute the local orientation and descriptor of a list of given Keypoints
  * @param[in] octave A Gaussian Octave
  * @param[in,out] keypoints The list of found extrema as Keypoints with updated orientation and descriptor
  */
  void operator()
  (
    const Octave & octave,
    std::vector<Keypoint> & keypoints
  )
  {
    Compute_Gradients(octave);
    Keypoints_orientations(keypoints);
    Keypoints_description(keypoints);
  }

  void Compute_Orientations
  (
    const Octave & octave,
    std::vector<Keypoint> & keypoints
  )
  {
    Compute_Gradients(octave);
    Keypoints_orientations(keypoints);
  }

protected:

  /**
  * @brief Compute the gradient of a given Gaussian Octave
  * @param[in] octave A gaussian Octave
  */
  void Compute_Gradients(const Octave & octave)
  {
    const int nSca = octave.slices.size();
    m_xgradient.slices.resize(nSca);
    m_ygradient.slices.resize(nSca);
    m_xgradient.delta = octave.delta;
    m_ygradient.delta = octave.delta;
    m_xgradient.octave_level = octave.octave_level;
    m_ygradient.octave_level = octave.octave_level;
    for (int s = 1; s < nSca-1; ++s)
    {
      // only in range [1; n-1] (since first and last images were only used for non max suppression)
      image::ImageXDerivative( octave.slices[s], m_xgradient.slices[s]);
      image::ImageYDerivative( octave.slices[s], m_ygradient.slices[s]);
    }
  }

  // Compute the x modulus y
  // Modulo than handle negative values (for circular way)
  static inline float modulus(float x, float y)
  {
    float z = x;
    if (z < 0)
    {
      z += ((int)( -z / y) + 1) * y;
    }
    z -= (int)(z / y) * y;
    return z;
  }

  static inline int ori_to_bin(float ori, int nbins)
  {
    if (ori < 0)
      ori += 2 * M_PI;
    return static_cast<int>((ori/(2*M_PI)*nbins+0.5))%nbins;
  }

  static inline float bin_to_ori(float bin, int nbins)
  {
    float ori = (bin+0.5)*2*M_PI/(float)nbins;
    if (ori > M_PI)
      ori -= 2*M_PI;
    return ori;
  }

  /**
  * @brief Accumulate gradient orientation histogram around a keypoint
  * @param key The keypoint for which orientation histogram
  * @param[out] hist The orientation histogram
  */
  void Keypoint_orientation_histogram
  (
    const Keypoint & key,
    openMVG::Vecf & hist) const
  {
    hist.resize(m_nb_orientation_histogram_bin);
    hist.fill(0.0f);

    // conversion to the octave's coordinates (sampling)
    const float delta  = m_xgradient.delta;
    const float x = key.x / delta;
    const float y = key.y / delta;
    const float sigma = key.sigma / delta;

    const int w = m_xgradient.slices[1].Width(); // 1 since 0 is empty
    const int h = m_xgradient.slices[1].Height();

    const image::Image<float> & xgradient = m_xgradient.slices[key.s];
    const image::Image<float> & ygradient = m_ygradient.slices[key.s];

    // Contributing pixels are inside a patch [siMin;siMax] X [sjMin;sjMax]
    // of width w 6*lambda_ori*sigma_key (=9*sigma_key)
    const float R = 3 * m_orientation_scale * sigma;
    const int siMin = std::max(0, (int)(x - R + 0.5));
    const int sjMin = std::max(0, (int)(y - R + 0.5));
    const int siMax = std::min((int)(x + R + 0.5), w-1);
    const int sjMax = std::min((int)(y + R + 0.5), h-1);

    // For each pixel neighborhood patch
    for (int si = siMin; si <= siMax; ++si)
    {
      for (int sj = sjMin; sj <= sjMax; ++sj)
      {
        // gradient orientation (theta)
        const float dx = xgradient(sj, si);
        const float dy = ygradient(sj, si);
        const float ori = modulus(atan2(dy, dx), 2*M_PI);

        // gradient magnitude with Gaussian weighting
        const float sX = (si - x) / sigma;
        const float sY = (sj - y) / sigma;
        const float r2 = Square(sX) + Square(sY);
        const float M = hypot(dx, dy) * exp(-r2/(2*Square(m_orientation_scale)));

        /// Determine the bin index in the circular histogram
        const int gamma = ori_to_bin(ori, m_nb_orientation_histogram_bin);

        /// Add the contribution to the orientation histogram
        hist[gamma] += M;
      }
    }
  }

  /**
  * @brief Iterative box filter based of a triangular length 3 kernel
  * @param[in] iter The number of filtering operation
  * @param[in,out] orientation_histogram The histogram to smooth
  */
  static void smooth_circular_histogram
  (
    int niter,
    openMVG::Vecf & orientation_histogram
  )
  {
    int i,i_prev,i_next;
    openMVG::Vecf tmp = orientation_histogram;
    const int nbins = tmp.rows();
    // Convolution with box filters
    for (; niter > 0; --niter){
      tmp = orientation_histogram;
      for (i = 0; i < nbins; ++i){
        i_prev = (i-1+nbins)%nbins;
        i_next = (i+1)%nbins;
        orientation_histogram[i] = (tmp[i_prev]/4+tmp[i]/2+tmp[i_next]/4);
      }
    }
  }

  /**
  * @brief Quadratic interpolation
  * @retval The quadratic interpolated value
  */
  static float interpolate_peak(float h1, float h2, float h3)
  {
    return (h1-h3)/(2*(h1+h3-2*h2));
  }

  /**
  * @brief Extract the principal orientations from a gradient orientation histogram
  *        Local maxima are considered secondary
  *        principal orientation if they exceed
  *        threshold times the absolute maximum (expressed in percentage of the max value)
  * @param[in] orientation_histogram The orientation histogram
  * @param[out] principal_orientations the principal orientation value (as bin index)
  * @retval the number of found principal orientation
  */
  int Extract_principal_orientations
  (
    openMVG::Vecf & orientation_histogram,
    std::vector<float> & principal_orientations
  ) const
  {
    // number of principal orientations (the return value).
    int o = 0;

    // Smooth histogram : iterated box filters
    smooth_circular_histogram(m_nb_smoothing_step, orientation_histogram);

    // What is the value of the global maximum
    const float max_value = orientation_histogram.maxCoeff();
    // Search for local extrema in the histogram
    const int nbins = orientation_histogram.rows();
    for (int i = 0; i < nbins; ++i){
      const int i_prev = (i-1+nbins)%nbins;
      const int i_next = (i+1)%nbins;
      if ( (orientation_histogram[i] > m_percentage_keep * max_value)
        && (orientation_histogram[i]>orientation_histogram[i_prev])
        && (orientation_histogram[i]>orientation_histogram[i_next]))
      {
        // Quadratic interpolation of the position of each local maximum
        const float offset = interpolate_peak(orientation_histogram[i_prev], orientation_histogram[i], orientation_histogram[i_next]);
        // Add to vector of principal orientations (expressed in [0,2pi]
        principal_orientations[o] = bin_to_ori((float)i + offset, nbins);
        ++o;
      }
    }
    // return the number of principal orientations
    return o;
  }

  /**
  * @brief Compute the orientations of a list of Keypoints
  * @param[in,out] keypoints The list of found Keypoints
  */
  void Keypoints_orientations
  (
    std::vector<Keypoint> & keypoints
  ) const
  {
    std::vector<Keypoint> kps;
    kps.reserve(keypoints.size());
#ifdef OPENMVG_USE_OPENMP
#pragma omp parallel for schedule(dynamic)
#endif
    for (int i_key = 0; i_key < keypoints.size(); ++i_key)
    {
      const Keypoint & key = keypoints[i_key];

      openMVG::Vecf orientation_histogram;
      Keypoint_orientation_histogram(key, orientation_histogram);

      // Compute principal orientation(s)
      std::vector<float> principal_orientations(m_nb_orientation_histogram_bin);
      const int n_prOri = Extract_principal_orientations(orientation_histogram, principal_orientations);

      // Updating keypoints and save it in the new list
#ifdef OPENMVG_USE_OPENMP
      #pragma omp critical
#endif
      for (int n = 0; n < n_prOri; ++n)
      {
        Keypoint kp = key;
        kp.theta = principal_orientations[n];
        kps.emplace_back(kp);
      }
    }
    keypoints = std::move(kps);
    keypoints.shrink_to_fit();
  }

  /// Fast approximation of atan2
  /// http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
  static inline float atan2_fast
  (
    float y,
    float x
  )
  {
    float angle, r;
    const float c3 = 0.1821f;
    const float c1 = 0.9675f;
    const float abs_y = std::abs(y) + std::numeric_limits<float>::min();

    if (x >= 0) {
      r = (x - abs_y) / (x + abs_y);
      angle = (float)(M_PI / 4);
    }
    else {
      r = (x + abs_y) / (abs_y - x);
      angle = (float)(3 * M_PI / 4);
    }
    angle += (c3 * r * r - c1) * r;
    return (y < 0) ? -angle : angle;
  }

  static inline float mod_2pi_fast
  (
    float x
  )
  {
    while (x >(float)(2 * M_PI)) x -= (float)(2 * M_PI);
    while (x < 0.0f) x += (float)(2 * M_PI);
    return x;
  }


  /**
  * @brief Extract a SIFT descriptor for a given Keypoint
  * @param[in] k The keypoint for which the descriptor is required
  * @param[out] descr The found SIFT descriptor
  */
  void Extract_sift_feature_vector
  (
    const Keypoint &k,
    openMVG::Vecf & descr
  ) const
  {
    descr.fill(0.0f);
    const float delta = m_xgradient.delta;
    const int w = m_xgradient.slices[1].Width();
    const int h = m_xgradient.slices[1].Height();
    const float x = k.x / delta;
    const float y = k.y / delta;
    const float sigma = k.sigma / delta;

    const float c = cos(-k.theta);
    const float s = sin(-k.theta);

    const image::Image<float> & xgradient = m_xgradient.slices[k.s];
    const image::Image<float> & ygradient = m_ygradient.slices[k.s];

    // Local patch size is [siMin;siMax]X[sjMin;sjMax] which width W
    // W = 2*lambda_descr*sigma_key*(m_nb_split2d+1)/m_nb_split2d
    const float R =  (1+1/(float)m_nb_split2d)* m_descriptor_scale * sigma;
    const float Rp =  1.41421356237309504880 * R;
    const int siMin = std::max(0, (int)(x - Rp + 0.5f));
    const int sjMin = std::max(0, (int)(y - Rp + 0.5f));
    const int siMax = std::min((int)(x + Rp + 0.5f), w-1);
    const int sjMax = std::min((int)(y + Rp + 0.5f), h-1);
    /// For each pixel inside the patch.
    for (int si = siMin; si < siMax; ++si)
    {
      for (int sj = sjMin; sj < sjMax; ++sj)
      {
        // Compute pixel coordinates (sX,sY) on keypoint's invariant referential.
        const float Xref = si - x;
        const float Yref = sj - y;
        const float X = c * Xref - s * Yref;
        const float Y = s * Xref + c * Yref;
        // Does this sample fall inside the descriptor area ?
        if (std::max(std::abs(X), std::abs(Y)) < R)
        {
          // Compute the gradient orientation (theta) on keypoint referential.
          const float dx = xgradient(sj, si);
          const float dy = ygradient(sj, si);
          const float atan2val = atan2_fast(dy, dx);
          const float ori = mod_2pi_fast(atan2val - k.theta);

          // Compute the gradient magnitude and apply a Gaussian weighting to give less emphasis to distant sample
          const float t = m_descriptor_scale * sigma;
          const float M = hypot(dx, dy) * exp(-(Square(X)+Square(Y))/(2*Square(t)));

          // bin indices, Compute the (tri)linear weightings ...
          const float alpha = X/(2 * m_descriptor_scale * sigma / m_nb_split2d) + (m_nb_split2d-1.0)/2.0;
          const float beta  = Y/(2 * m_descriptor_scale * sigma / m_nb_split2d) + (m_nb_split2d-1.0)/2.0;
          const float gamma = ori / (2*M_PI) * m_nb_split_angle;
          //    ...and add contributions to respective bins in different histograms.
          // a loop with 1 or two elements
          const int i0 = std::floor(alpha);
          const int j0 = std::floor(beta);
          for (int i = std::max(0,i0); i <= std::min(i0+1,m_nb_split2d-1); ++i)
          {
            for (int j = std::max(0,j0); j <= std::min(j0+1,m_nb_split2d-1); ++j)
            { // looping through all surrounding histograms.

              const int index = i*m_nb_split2d*m_nb_split_angle+j*m_nb_split_angle;
              // Contribution to left bin.
              const int k_left = ((int)gamma + m_nb_split_angle) % m_nb_split_angle;
              descr[index+k_left]
                += (1.0f-(gamma-floor(gamma)))
                   *(1.0f-std::abs((float)i-alpha))
                   *(1.0f-std::abs((float)j-beta))
                   *M;

              // Contribution to right bin.
              const int k_right = ((int)gamma + 1 + m_nb_split_angle) % m_nb_split_angle;
              descr[index+k_right]
                += (1.0f-(floor(gamma)+1.f-gamma))
                  *(1.0f-std::abs((float)i-alpha))
                  *(1.0f-std::abs((float)j-beta))
                  *M;
            }
          }
        }
      }
    }
  }

  /**
  * @brief Compute the SIFT descriptor of a list of Keypoints, apply the requested normalization and discretization [0;255]
  * @param[in,out] keypoints The list of keypoints that must have their SIFT descriptor computed
  */
  void Keypoints_description
  (
    std::vector<Keypoint> & keypoints
  ) const
  {
    const int n_descr = Square(m_nb_split2d) * m_nb_split_angle;
#ifdef OPENMVG_USE_OPENMP
#pragma omp parallel for schedule(dynamic)
#endif
    for (int i_key = 0; i_key < keypoints.size(); ++i_key)
    {
      Keypoint & key = keypoints[i_key];
      // Compute the SIFT descriptor
      key.descr.resize(n_descr);
      Extract_sift_feature_vector(key, key.descr);

      // Threshold bins
      float norm = key.descr.lpNorm<2>() + std::numeric_limits<float>::epsilon();
      for (int i = 0; i < key.descr.rows(); ++i){
        key.descr[i] = std::min(key.descr[i], m_clip_value * norm);
      }
      // Quantization
      if (m_b_root_sift)
      {
        // scaling(rootsift) = sqrt( sift / sum(sift) );
        norm = key.descr.lpNorm<1>() + std::numeric_limits<float>::epsilon();
        key.descr = 512.f * (key.descr / norm).array().sqrt();
      }
      else
      {
        // scaling(normalized descriptor)
        norm = key.descr.lpNorm<2>() + std::numeric_limits<float>::epsilon();
        key.descr = 512.f * (key.descr / norm).array();
      }
    }
  }
protected:

  Octave m_xgradient; // gradient along x direction
  Octave m_ygradient; // gradient along y direction

  // Orientation histogram params
  int m_nb_orientation_histogram_bin;   // Nb bin for computing orientation
  float m_orientation_scale;            // Sets the width of the orientation patch
  int m_nb_smoothing_step;              // Smoothing step before orientation assignment
  float m_percentage_keep;              // Percentage to keep an orientation (minimal value in the histogram)

  // Orientation & Descriptor extraction parameters
  bool m_b_root_sift;       // Apply or not root sift normalized histogram
  float m_descriptor_scale; // Scale of descriptor box computation (sets the width of the descriptor patch)
  int m_nb_split2d;         // Number of split (in x and in y) applied to descriptor histogram
  int m_nb_split_angle;     // Number of split (in z) applied to descriptor histogram
  float m_clip_value;       // Threshold value to clamp large descriptor peak
};

} // namespace sift
} // namespace features
} // namespace openMVG

#endif // OPENMVG_FEATURES_SIFT_SIFT_DESCRIPTOR_EXTRACTOR_HPP
