
/*@@@**************************************************************************
 ** \file  medFilter_inline
 * \date   Tue Nov 10 15:47:12 EST 2009
 * \author Hernan Badino
 * \notes  
*******************************************************************************
*****          (C) COPYRIGHT Hernan Badino - All Rights Reserved          *****
******************************************************************************/

#if defined ( _OPENMP )
 #include <omp.h>
#endif

/* MACROS */
#define CMEDIANFILTER_PIX_SWAP(a,b) { float temp=(a);(a)=(b);(b)=temp; }
#define CMEDIANFILTER_PIX_SORT(a,b) { if ((a)>(b)) CMEDIANFILTER_PIX_SWAP((a),(b)); }

template <class _SrcType, class _DstType>
inline bool
CMedianFilter<_SrcType, _DstType>::compute3x3( const CTypedImage<_SrcType> &f_srcImg,
                                               CTypedImage<_DstType>       &fr_dstImg )
{
    if ( fr_dstImg.getWidth()  < f_srcImg.getWidth() ||
         fr_dstImg.getHeight() < f_srcImg.getHeight() )
        return false;
        
    const int w_i = f_srcImg.getWidth()-1, h_i = f_srcImg.getHeight()-1;

#if defined ( _OPENMP )
    int numThreads_ci = std::min(omp_get_max_threads(), 32);
#pragma omp parallel for num_threads(numThreads_ci) schedule(dynamic)
#endif
    for (int i = 1; i < h_i; ++i)
    {
        _SrcType v0, v1, v2, v3, v4, v5, v6, v7, v8;
  
        _SrcType  *srcm1_p, *srcp1_p, *srcp0_p;
        srcm1_p = f_srcImg.getScanline(i-1);
        srcp0_p = f_srcImg.getScanline(i  );
        srcp1_p = f_srcImg.getScanline(i+1);

        _DstType *tgt_p = fr_dstImg.getScanline(i);

        for (int j = 1; j < w_i; ++j)
        {
            v0 = srcm1_p[j-1]; //0
            v1 = srcm1_p[j  ]; //1
            v2 = srcm1_p[j+1]; //2
      
            v3 = srcp0_p[j-1]; //3
            v4 = srcp0_p[j  ]; //5
            v5 = srcp0_p[j+1]; //5
          
            v6 = srcp1_p[j-1]; //6
            v7 = srcp1_p[j  ]; //7
            v8 = srcp1_p[j+1]; //8
          
            CMEDIANFILTER_PIX_SORT(v1, v2) ; CMEDIANFILTER_PIX_SORT(v4, v5) ; CMEDIANFILTER_PIX_SORT(v7, v8) ;
            CMEDIANFILTER_PIX_SORT(v0, v1) ; CMEDIANFILTER_PIX_SORT(v3, v4) ; CMEDIANFILTER_PIX_SORT(v6, v7) ;
            CMEDIANFILTER_PIX_SORT(v1, v2) ; CMEDIANFILTER_PIX_SORT(v4, v5) ; CMEDIANFILTER_PIX_SORT(v7, v8) ;
            CMEDIANFILTER_PIX_SORT(v0, v3) ; CMEDIANFILTER_PIX_SORT(v5, v8) ; CMEDIANFILTER_PIX_SORT(v4, v7) ;
            CMEDIANFILTER_PIX_SORT(v3, v6) ; CMEDIANFILTER_PIX_SORT(v1, v4) ; CMEDIANFILTER_PIX_SORT(v2, v5) ;
            CMEDIANFILTER_PIX_SORT(v4, v7) ; CMEDIANFILTER_PIX_SORT(v4, v2) ; CMEDIANFILTER_PIX_SORT(v6, v4) ;
            CMEDIANFILTER_PIX_SORT(v4, v2) ;
            // the result is in v4.
          
            tgt_p[j] = (_DstType) v4;
        }
    }  

    return true;
}



template <class _SrcType, class _DstType>
inline bool
CMedianFilter<_SrcType, _DstType>::compute5x5( const CTypedImage<_SrcType> &f_srcImg,
                                               CTypedImage<_DstType>       &fr_dstImg )
{

    if ( fr_dstImg.getWidth()  < f_srcImg.getWidth() ||
         fr_dstImg.getHeight() < f_srcImg.getHeight() )
        return false;

    const int w_i = f_srcImg.getWidth()-2, h_i = f_srcImg.getHeight()-2;
  
    //#undef _OPENMP
#if defined ( _OPENMP )
    int numThreads_ci = std::min(omp_get_max_threads(), 32);
#pragma omp parallel for num_threads(numThreads_ci) schedule(dynamic)
#endif
    for (int i = 2; i < h_i; ++i)
    {
        _SrcType elems_p[25];
        _SrcType *el_p;
        _SrcType *src_p;

        _DstType * tgt_p = fr_dstImg.getScanline(i);

        for (int j = 2; j < w_i; j++)
        {
            int left_i = j-2;
            
            el_p = elems_p;
            src_p = f_srcImg.getScanline(i-2) + left_i;
            *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p; 
      
            src_p = f_srcImg.getScanline(i-1) + left_i;
            *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p; 
      
            src_p = f_srcImg.getScanline(i) + left_i;
            *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p; 
      
            src_p = f_srcImg.getScanline(i+1) + left_i;
            *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p; 

            src_p = f_srcImg.getScanline(i+2) + left_i;
            *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p++ = *src_p++; *el_p = *src_p;

            CMEDIANFILTER_PIX_SORT(elems_p[0], elems_p[1]) ;   CMEDIANFILTER_PIX_SORT(elems_p[3], elems_p[4]) ;   CMEDIANFILTER_PIX_SORT(elems_p[2], elems_p[4]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[2], elems_p[3]) ;   CMEDIANFILTER_PIX_SORT(elems_p[6], elems_p[7]) ;   CMEDIANFILTER_PIX_SORT(elems_p[5], elems_p[7]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[5], elems_p[6]) ;   CMEDIANFILTER_PIX_SORT(elems_p[9], elems_p[10]) ;  CMEDIANFILTER_PIX_SORT(elems_p[8], elems_p[10]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[8], elems_p[9]) ;   CMEDIANFILTER_PIX_SORT(elems_p[12], elems_p[13]) ; CMEDIANFILTER_PIX_SORT(elems_p[11], elems_p[13]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[11], elems_p[12]) ; CMEDIANFILTER_PIX_SORT(elems_p[15], elems_p[16]) ; CMEDIANFILTER_PIX_SORT(elems_p[14], elems_p[16]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[14], elems_p[15]) ; CMEDIANFILTER_PIX_SORT(elems_p[18], elems_p[19]) ; CMEDIANFILTER_PIX_SORT(elems_p[17], elems_p[19]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[17], elems_p[18]) ; CMEDIANFILTER_PIX_SORT(elems_p[21], elems_p[22]) ; CMEDIANFILTER_PIX_SORT(elems_p[20], elems_p[22]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[20], elems_p[21]) ; CMEDIANFILTER_PIX_SORT(elems_p[23], elems_p[24]) ; CMEDIANFILTER_PIX_SORT(elems_p[2], elems_p[5]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[3], elems_p[6]) ;   CMEDIANFILTER_PIX_SORT(elems_p[0], elems_p[6]) ;   CMEDIANFILTER_PIX_SORT(elems_p[0], elems_p[3]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[4], elems_p[7]) ;   CMEDIANFILTER_PIX_SORT(elems_p[1], elems_p[7]) ;   CMEDIANFILTER_PIX_SORT(elems_p[1], elems_p[4]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[11], elems_p[14]) ; CMEDIANFILTER_PIX_SORT(elems_p[8], elems_p[14]) ;  CMEDIANFILTER_PIX_SORT(elems_p[8], elems_p[11]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[12], elems_p[15]) ; CMEDIANFILTER_PIX_SORT(elems_p[9], elems_p[15]) ;  CMEDIANFILTER_PIX_SORT(elems_p[9], elems_p[12]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[13], elems_p[16]) ; CMEDIANFILTER_PIX_SORT(elems_p[10], elems_p[16]) ; CMEDIANFILTER_PIX_SORT(elems_p[10], elems_p[13]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[20], elems_p[23]) ; CMEDIANFILTER_PIX_SORT(elems_p[17], elems_p[23]) ; CMEDIANFILTER_PIX_SORT(elems_p[17], elems_p[20]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[21], elems_p[24]) ; CMEDIANFILTER_PIX_SORT(elems_p[18], elems_p[24]) ; CMEDIANFILTER_PIX_SORT(elems_p[18], elems_p[21]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[19], elems_p[22]) ; CMEDIANFILTER_PIX_SORT(elems_p[8], elems_p[17]) ;  CMEDIANFILTER_PIX_SORT(elems_p[9], elems_p[18]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[0], elems_p[18]) ;  CMEDIANFILTER_PIX_SORT(elems_p[0], elems_p[9]) ;   CMEDIANFILTER_PIX_SORT(elems_p[10], elems_p[19]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[1], elems_p[19]) ;  CMEDIANFILTER_PIX_SORT(elems_p[1], elems_p[10]) ;  CMEDIANFILTER_PIX_SORT(elems_p[11], elems_p[20]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[2], elems_p[20]) ;  CMEDIANFILTER_PIX_SORT(elems_p[2], elems_p[11]) ;  CMEDIANFILTER_PIX_SORT(elems_p[12], elems_p[21]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[3], elems_p[21]) ;  CMEDIANFILTER_PIX_SORT(elems_p[3], elems_p[12]) ;  CMEDIANFILTER_PIX_SORT(elems_p[13], elems_p[22]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[4], elems_p[22]) ;  CMEDIANFILTER_PIX_SORT(elems_p[4], elems_p[13]) ;  CMEDIANFILTER_PIX_SORT(elems_p[14], elems_p[23]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[5], elems_p[23]) ;  CMEDIANFILTER_PIX_SORT(elems_p[5], elems_p[14]) ;  CMEDIANFILTER_PIX_SORT(elems_p[15], elems_p[24]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[6], elems_p[24]) ;  CMEDIANFILTER_PIX_SORT(elems_p[6], elems_p[15]) ;  CMEDIANFILTER_PIX_SORT(elems_p[7], elems_p[16]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[7], elems_p[19]) ;  CMEDIANFILTER_PIX_SORT(elems_p[13], elems_p[21]) ; CMEDIANFILTER_PIX_SORT(elems_p[15], elems_p[23]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[7], elems_p[13]) ;  CMEDIANFILTER_PIX_SORT(elems_p[7], elems_p[15]) ;  CMEDIANFILTER_PIX_SORT(elems_p[1], elems_p[9]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[3], elems_p[11]) ;  CMEDIANFILTER_PIX_SORT(elems_p[5], elems_p[17]) ;  CMEDIANFILTER_PIX_SORT(elems_p[11], elems_p[17]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[9], elems_p[17]) ;  CMEDIANFILTER_PIX_SORT(elems_p[4], elems_p[10]) ;  CMEDIANFILTER_PIX_SORT(elems_p[6], elems_p[12]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[7], elems_p[14]) ;  CMEDIANFILTER_PIX_SORT(elems_p[4], elems_p[6]) ;   CMEDIANFILTER_PIX_SORT(elems_p[4], elems_p[7]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[12], elems_p[14]) ; CMEDIANFILTER_PIX_SORT(elems_p[10], elems_p[14]) ; CMEDIANFILTER_PIX_SORT(elems_p[6], elems_p[7]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[10], elems_p[12]) ; CMEDIANFILTER_PIX_SORT(elems_p[6], elems_p[10]) ;  CMEDIANFILTER_PIX_SORT(elems_p[6], elems_p[17]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[12], elems_p[17]) ; CMEDIANFILTER_PIX_SORT(elems_p[7], elems_p[17]) ;  CMEDIANFILTER_PIX_SORT(elems_p[7], elems_p[10]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[12], elems_p[18]) ; CMEDIANFILTER_PIX_SORT(elems_p[7], elems_p[12]) ;  CMEDIANFILTER_PIX_SORT(elems_p[10], elems_p[18]) ;
            CMEDIANFILTER_PIX_SORT(elems_p[12], elems_p[20]) ; CMEDIANFILTER_PIX_SORT(elems_p[10], elems_p[20]) ; CMEDIANFILTER_PIX_SORT(elems_p[10], elems_p[12]) ;

            // the result is in elems_p[12].
            tgt_p[j] = (_DstType) elems_p[12];
        }
    }  
    return true;
}

#undef CMEDIANFILTER_PIX_SORT
#undef CMEDIANFILTER_PIX_SWAP
