// Copyright (c) 2014 Romuald PERROT, 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/.

#ifndef OPENMVG_IMAGE_DESCRIPTION_MLDB_DESCRIPTOR_H
#define OPENMVG_IMAGE_DESCRIPTION_MLDB_DESCRIPTOR_H

#include "openMVG/features/descriptor.hpp"
#include "openMVG/features/feature.hpp"
#include "openMVG/image/image.hpp"
#include "openMVG/numeric/math_trait.hpp"

namespace openMVG {
namespace features {

  /**
  ** @brief Compute mean values (Li,Lx,Ly) in each subdivisions
  ** @param samples_Li input values on Li
  ** @param samples_Lx input values on Lx
  ** @param samples_Ly input values on Ly
  ** @param nb_subdiv number of subdivision (on each 2d axis)
  ** @param subdiv_size size of a subdivision (on each 2d axis)
  ** @param pattern_size source size (on each 2d axis)
  ** @param c cosinus of main orientation (for computing new derivatives values)
  ** @param s sinus of main orientation (for computing new derivatives values)
  ** @param mean_Li mean of Li in each subdivision
  ** @param mean_Lx mean of Lx in each subdivision
  ** @param mean_Ly mean of Ly in each subdivision
  **/
  template< typename Real>
  static inline void ComputeMeanValuesInSubdivisions(
      const Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & samples_Li ,
      const Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & samples_Lx ,
      const Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & samples_Ly ,
      const int nb_subdiv ,
      const int subdiv_size ,
      const int pattern_size ,
      const Real c , // cos( theta )
      const Real s , // sin( theta )
      Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & mean_Li ,
      Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & mean_Lx ,
      Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & mean_Ly )
  {
    mean_Li.resize( nb_subdiv , nb_subdiv ) ;
    mean_Lx.resize( nb_subdiv , nb_subdiv ) ;
    mean_Ly.resize( nb_subdiv , nb_subdiv ) ;

    const int max_w = samples_Li.cols() ;
    const int max_h = samples_Li.rows() ;

    for( int i = 0 ; i < nb_subdiv ; ++i )
    {
      for( int j = 0 ; j < nb_subdiv ; ++j )
      {
        // Compute subdivision extend
        const int min_x = j * subdiv_size ;
        const int min_y = i * subdiv_size ;
        const int max_x = std::min( ( j + 1 ) * subdiv_size , max_w ) ;
        const int max_y = std::min( ( i + 1 ) * subdiv_size , max_h ) ;

        // Sum every elements of this subdivision
        mean_Li( i , j ) = mean_Lx( i , j ) = mean_Ly( i , j ) = 0 ;

        size_t nb_elt = 0 ;
        for( int ii = min_y ; ii < max_y ; ++ii )
        {
          for( int jj = min_x ; jj < max_x ; ++jj )
          {
            mean_Li( i , j ) += samples_Li( ii , jj ) ;

            const Real dx = samples_Lx( ii , jj ) ;
            const Real dy = samples_Ly( ii , jj ) ;

            // Rotate derivatives
            // a is original angle, b is keypoint angle
            // Cos( a - b ) = cosA cosB + sinA sinB
            //              = dx * c + dy * s
            // Sin( a - b ) = sinA cosB - cosA sinB
            //              = dy * c - dx * s
            mean_Ly( i , j ) +=   dx * c + dy * s ;
            mean_Lx( i , j ) +=   dy * c - dx * s ;

            ++nb_elt ;
          }
        }

        mean_Li( i , j ) /= static_cast<Real>( nb_elt ) ;
        mean_Lx( i , j ) /= static_cast<Real>( nb_elt ) ;
        mean_Ly( i , j ) /= static_cast<Real>( nb_elt ) ;
      }
    }
  }

  /**
  ** @brief Compute binary description
  ** @param mean_Li input mean values of Li values (mean per subdivision)
  ** @param mean_Lx input mean values of Lx values (mean per subdivision)
  ** @param mean_Ly input mean values of Ly values (mean per subdivision)
  ** @param nb_subdiv Number of subdivision (in 2d so it's a nb_subdivxnb_subdiv pattern)
  ** @param outIndex input/ouput index to store description
  ** @param desc ouput vector (idealy a std::bitset) containing binary description of theses regions
  **/
  template< typename DescriptorType , typename Real>
  static inline void ComputeBinaryValues(
    const Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & mean_Li ,
    const Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & mean_Lx ,
    const Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> & mean_Ly ,
    const int nb_subdiv ,
    size_t & outIndex ,
    DescriptorType & desc )
  {
    // Binary comparisons (ie (0,0) with (0,1), (O,0) with (0,2), ... )
    for( int i = 0 ; i < ( nb_subdiv * nb_subdiv ) ; ++i )
    {
      for( int j = i + 1 ; j < ( nb_subdiv * nb_subdiv ) ; ++j )
      {
        const int src_i = i / nb_subdiv ;
        const int src_j = i % nb_subdiv  ;

        const int dst_i = j / nb_subdiv ;
        const int dst_j = j % nb_subdiv ;

        // Compare (src_i,src_j) with (dst_i,dst_j) on the three values
        desc[ outIndex++ ] = mean_Li( src_i , src_j ) > mean_Li( dst_i , dst_j ) ;
        desc[ outIndex++ ] = mean_Lx( src_i , src_j ) > mean_Lx( dst_i , dst_j ) ;
        desc[ outIndex++ ] = mean_Ly( src_i , src_j ) > mean_Ly( dst_i , dst_j ) ;
      }
    }
  }

  /**
    ** @brief Compute final keypoint (ie interest point + description) for a given interest point
    ** @param Li Input Octave slice
    ** @param Lx Input X-derivative
    ** @param Ly Input Y-derivative
    ** @param id_octave Id of current octave
    ** @param ipt Input interest point
    ** @param desc output descriptor (binary descriptor)
    **/
  template< typename Real>
  void ComputeMLDBDescriptor(
    const image::Image<Real> & Li,
    const image::Image<Real> &Lx,
    const image::Image<Real> &Ly,
    const int id_octave ,
    const SIOPointFeature & ipt ,
    Descriptor<bool, 486> & desc )
  {
    // // Note : in KAZE description we compute descriptor of previous slice and never the current one

    // See if it's necessary to change this value (pass it into a param ?)
    const int pattern_size = 10 ;

    // Sampling size according to the scale value
    const Real inv_octave_scale = static_cast<Real>( 1 ) / static_cast<Real>( 1 << id_octave ) ;
    const Real sigma_scale = MathTrait<Real>::round( ipt.scale() * inv_octave_scale ) ;

    // Get every samples inside 2pattern x 2pattern square region
    // Memory efficient (get samples then work in aligned)
    Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
      samples_Li( 2 * pattern_size + 1 , 2 * pattern_size + 1 ),
      samples_Lx( 2 * pattern_size + 1 , 2 * pattern_size + 1 ),
      samples_Ly( 2 * pattern_size + 1 , 2 * pattern_size + 1 );

    // Compute cos and sin values for this point orientation
    const Real c = MathTrait<Real>::cos( ipt.orientation() ) ;
    const Real s = MathTrait<Real>::sin( ipt.orientation() ) ;

    const Real cur_x = ipt.x() * inv_octave_scale ;
    const Real cur_y = ipt.y() * inv_octave_scale ;

    // Retrieve samples in oriented pattern
    for( int i = - pattern_size ; i <= pattern_size ; ++i )
    {
      for( int j = - pattern_size ; j <= pattern_size ; ++j )
      {
        // sample_y = yf + (l*scale*co + k*scale*si);
        // sample_x = xf + (-l*scale*si + k*scale*co);

        // Need to do a function for that rotate of angle -theta ( minus theta because image frame is Y down)
        const Real delta_y =   static_cast<Real>( j ) * c + static_cast<Real>( i ) * s ;
        const Real delta_x =  -static_cast<Real>( j ) * s + static_cast<Real>( i ) * c ;

        // Compute new real position
        const Real dx = cur_x + sigma_scale * delta_x ;
        const Real dy = cur_y + sigma_scale * delta_y ;

        // Compute new discrete position
        const int y = MathTrait<Real>::round( dy ) ;
        const int x = MathTrait<Real>::round( dx ) ;

        samples_Li( i + pattern_size , j + pattern_size ) = Li( y , x ) ;
        samples_Lx( i + pattern_size , j + pattern_size ) = Lx( y , x ) ;
        samples_Ly( i + pattern_size , j + pattern_size ) = Ly( y , x ) ;
      }
    }

    size_t outIndex = 0 ; // Index to store next binary value

    // Grid 1 : 2x2 subdivision
    int subdiv_size = pattern_size ;
    Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> sumLi , sumLx , sumLy ;
    ComputeMeanValuesInSubdivisions( samples_Li , samples_Lx , samples_Ly , 2 , subdiv_size , pattern_size , c , s , sumLi , sumLx , sumLy ) ;
    ComputeBinaryValues( sumLi , sumLx , sumLy , 2 , outIndex , desc ) ;

    // Grid 2 : 3x3 subdivision
    subdiv_size = static_cast<int>( MathTrait<Real>::ceil( static_cast<Real>( 2 * pattern_size ) / static_cast<Real>( 3 ) ) ) ;
    ComputeMeanValuesInSubdivisions( samples_Li , samples_Lx , samples_Ly , 3 , subdiv_size , pattern_size , c , s , sumLi , sumLx , sumLy ) ;
    ComputeBinaryValues( sumLi , sumLx , sumLy , 3 , outIndex , desc ) ;

    // Grid 3 : 4x4 subdivision
    subdiv_size = pattern_size / 2 ;
    ComputeMeanValuesInSubdivisions( samples_Li , samples_Lx , samples_Ly , 4 , subdiv_size , pattern_size , c , s , sumLi , sumLx , sumLy ) ;
    ComputeBinaryValues( sumLi , sumLx , sumLy , 4 , outIndex , desc ) ;

    assert( outIndex == 486 ) ; // Just to be sure (and we are sure ! completly sure !)
  }

} // namespace features
} // namespace openMVG

#endif
