/*@@@**************************************************************************
 * \file   pyrDynProg4Freespace.cpp
 * \author Hernan Badino
 * \date   Tue November 20 14:46 CET 2007
 * \notes 
 *******************************************************************************
 ******************************************************************************/

/* INCLUDES */
#include "pyrDynProg.h"
#include <math.h>
#include "dynProgOp.h"

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

using namespace VIC;

/* *************************** METHOD ************************************** */
/**
 * Standardconstructor.
 *
 *
 * \brief          Standardconstructor.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           Set the initial variables and allocate the object.
 * \remarks        -
 * \sa             -
 *
 * \param           (IN)   - .
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
CPyramidalDynProg::CPyramidalDynProg( const int f_width_i,
                                      const int f_height_i,
                                      const int f_levels_i )
        : m_currPath_v   (                                            0 ),
          m_levelVecRes_v (                                           0 ),
          m_levelFollowPath_v (                                       0 ),
          m_levelPathTolVec_v (                                       0 ),
          m_maxLevel_i (                                     f_levels_i ),
          m_applyMFAllLevels_b (                                   true )
{
    create ( f_width_i,
             f_height_i,
             f_levels_i );

    /// Per default is true.
    setApplyMedianFilter ( true );

}

CPyramidalDynProg::CPyramidalDynProg(  )
        : m_currPath_v (                                              0 ),
          m_levelVecRes_v (                                           0 ),
          m_levelFollowPath_v (                                       0 ),
          m_levelPathTolVec_v (                                       0 ),
          m_maxLevel_i (                                              1 ),
          m_applyMFAllLevels_b (                                   true )
{
    create ( 8,
             8,
             m_maxLevel_i );

    /// Per default is true.
    setApplyMedianFilter ( true );

}
/* *************************** METHOD ************************************** */
/**
 * Reinitialize object.
 *
 *
 * \brief          Reinitialize object.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           Set the initial variables and allocate the object.
 * \remarks        -
 * \sa             -
 *
 * \param           (IN)   - .
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::create ( const int f_width_i,
                                 const int f_height_i,
                                 const int f_levels_i )
{
    m_maxLevel_i = f_levels_i;
    // Check for level value first.
    if (m_maxLevel_i >= PDP_MAX_LEVELS)
        m_maxLevel_i =  PDP_MAX_LEVELS - 1;

    if (m_maxLevel_i < 0)
        m_maxLevel_i = 0;

    int i = 0;
    int f_w_i = f_width_i;

    // Create first dyn prog object.
    m_dynProgObj_p[0] = new CDynamicProgrammingOp ( f_width_i,
                                                    f_height_i );

    // The first float img will be given in the parameter call.
    // For now we set it to null.
    m_pyrImg_p[0] = NULL;

    // The first expected gradient vector will be given in the set method call.
    // For now we set it to null.
    m_expectedGradient_p[0] = NULL;

    for (i = 1; i <= m_maxLevel_i; ++i)
    {
        f_w_i /= 2;

        if (f_w_i < 2)
        {
            printf("Pyramid is too high. Setting max level from %i to %i", 
                   m_maxLevel_i, i-1 );
            m_maxLevel_i = i-1;
            break;
        }
        
        m_dynProgObj_p[i] = new CDynamicProgrammingOp ( f_w_i,
                                                        f_height_i );
        
        m_pyrImg_p[i]     = new CFloatImage           ( );
        m_pyrImg_p[i] -> setSize ( f_w_i,
                                   f_height_i );
        m_pyrImg_p[i] -> ensureAllocation();

        m_expectedGradient_p[i] = new float[ f_height_i ];
    }

    for (; i < PDP_MAX_LEVELS; ++i)
    {
        m_dynProgObj_p[i] = NULL;
        m_pyrImg_p[i]     = NULL;
        m_expectedGradient_p[i] = NULL;
    }

    m_currPath_v.resize   ( f_height_i );
    m_levelVecRes_v.resize( f_height_i );
}


/* *************************** METHOD ************************************** */
/**
 * Destructor.
 *
 *
 * \brief          Destructor.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           -
 * \remarks        -
 * \sa             -
 *
 *************************************************************************** */
CPyramidalDynProg::~CPyramidalDynProg( )
{
    destroy();
}

/* *************************** METHOD ************************************** */
/**
 * Deletes object.
 *
 *
 * \brief          Deletes object.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \remarks        -
 * \sa             -
 *
 * \param           (IN)   - .
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::destroy()
{
    int i;
    
    if (m_dynProgObj_p[0])
    {
        delete m_dynProgObj_p[0];
        m_dynProgObj_p[0] = NULL;
    }
    
    for (i = 1; i <= m_maxLevel_i; ++i)
    {
        if (m_dynProgObj_p[i])
        {
            delete m_dynProgObj_p[i];
            m_dynProgObj_p[i] = NULL;
        }
        
        if (m_pyrImg_p[i] )
        {
            delete m_pyrImg_p[i] ;
            m_pyrImg_p[i]  = NULL;
        }

        if (m_expectedGradient_p[i])
        {
            delete m_expectedGradient_p[i];
            m_expectedGradient_p[i] = NULL;
        }
    }
}

/******************************/
/*    SET GET METHODS         */
/*****************************/
/* *************************** METHOD ************************************** */
/**
 * Set the image size and the maximum level of the pyramid.
 *
 *
 * \brief          Set the image size and the maximum level of the pyramid.
 * \author         Hernan Badino
 * \date           22.11.2007
 *
 * \note           If there is any change since initialization the object is
 *                 reinitialized.
 * \remarks        -
 * \sa             -
 *
 * \param          - f_width_i (IN)  - Width of the original cost image.
 * \param          - f_height_i (IN) - Height of the original cost image.
 * \param          - f_levels_i (IN) - Number of levels of the pyramid.
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setPyramidParams ( const int f_width_i,
                                           const int f_height_i,
                                           const int f_levels_i )
{
    if (f_width_i  != getWidth()  || 
        f_height_i != getHeight() ||
        f_levels_i != getMaxLevel() )
    {
        bool applyMedianFilter_b = m_dynProgObj_p[0]->getApplyMedianFilter();

        destroy();
        create ( f_width_i,
                 f_height_i,
                 f_levels_i );

        setApplyMedianFilter ( applyMedianFilter_b );
    }
}

/* *************************** METHOD ************************************** */
/**
 * Set the distance cost for all levels.
 *
 *
 * \brief          Set the distance cost for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param          - f_distCost_f (IN) - Cost for level 0 of the pyramid.
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setDistanceCost ( const float f_distCost_f )
{
    m_dynProgObj_p[0] -> setDistanceCost ( f_distCost_f );

    int i;
    float f_cost_f = f_distCost_f;

    for (i = 1; i <= m_maxLevel_i; ++i)
    {
        f_cost_f *= 2;
        m_dynProgObj_p[i] -> setDistanceCost ( f_cost_f );
    }
}

/* *************************** METHOD ************************************** */
/**
 * Set the distance threshold.
 *
 *
 * \brief          Set the distance threshold for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param          - f_distTh_f (IN) - Threshold for level 0 of the pyramid.
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setDistanceTh ( const float f_distTh_f )
{
    m_dynProgObj_p[0] -> setDistanceTh ( f_distTh_f );

    int i;
    float f_th_f = f_distTh_f;

    for (i = 1; i <= m_maxLevel_i; ++i)
    {
        f_th_f /= 2;
        m_dynProgObj_p[i] -> setDistanceTh ( f_th_f );
    }
}

/* *************************** METHOD ************************************** */
/**
 * Set the prediction cost for all levels.
 *
 *
 * \brief          Set the prediction cost for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param          - f_predCost_f (IN) - Cost for level 0 of the pyramid.
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setPredictionCost ( const float f_predCost_f )
{
    m_dynProgObj_p[0] -> setPredictionCost ( f_predCost_f );

    int i;
    float f_cost_f = f_predCost_f;

    for (i = 1; i <= m_maxLevel_i; ++i)
    {
        //f_cost_f *= 2;
        m_dynProgObj_p[i] -> setPredictionCost ( f_cost_f );
    }
}


/* *************************** METHOD ************************************** */
/**
 * Set the prediction distance threshold for all levels.
 *
 *
 * \brief          Set the prediction distance threshold for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param          - f_distTh_f (IN) - Threshold for level 0 of the pyramid.
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setPredictionTh ( const float f_predTh_f )
{    
    m_dynProgObj_p[0] -> setPredictionTh ( f_predTh_f );

    int i;
    float f_th_f = f_predTh_f;

    for (i = 1; i <= m_maxLevel_i; ++i)
    {
        f_th_f /= 2;
        m_dynProgObj_p[i] -> setPredictionTh ( f_th_f );
    }
}

/* *************************** METHOD ************************************** */
/**
 * Set the initial cost for all levels.
 *
 *
 * \brief          Set the initial cost for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param          - f_initialCost_f (IN) - Cost for level 0 of the pyramid.
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setInitialCost ( const float f_initialCost_f )
{
    m_dynProgObj_p[0] -> setInitialCost ( f_initialCost_f );

    int i;
    float f_cost_f = f_initialCost_f;

    for (i = 1; i <= m_maxLevel_i; ++i)
    {
        //f_cost_f *= 2;
        m_dynProgObj_p[i] -> setInitialCost ( f_cost_f );
    }
}

/* *************************** METHOD ************************************** */
/**
 * Set the min cost value to consider for all levels.
 *
 *
 * \brief          Set the min cost value to consider for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param          - f_maxCost_f (IN) - Cost for level 0 of the pyramid.
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setMaxCostValue ( const float f_maxCost_f )
{
    m_dynProgObj_p[0] -> setMaxCostValue ( f_maxCost_f );

    int i;
    float f_cost_f = f_maxCost_f;

    for (i = 1; i <= m_maxLevel_i; ++i)
    {
        //f_cost_f *= 2;
        m_dynProgObj_p[i] -> setMaxCostValue ( f_cost_f );
    }
}

/* *************************** METHOD ************************************** */
/**
 * Set the min cost value to consider for all levels.
 *
 *
 * \brief          Set the min cost value to consider for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param          - f_maxCost_f (IN) - Cost for level 0 of the pyramid.
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setMinCostValue ( const float f_minCost_f )
{
    m_dynProgObj_p[0] -> setMinCostValue ( f_minCost_f );

    int i;
    float f_cost_f = f_minCost_f;

    for (i = 1; i <= m_maxLevel_i; ++i)
    {
        //f_cost_f *= 2;
        m_dynProgObj_p[i] -> setMinCostValue ( f_cost_f );
    }
}

/* *************************** METHOD ************************************** */
/**
 * Set the appliance of the median filter for all levels.
 *
 *
 * \brief          Set the appliance of the median filter for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param          - f_apply_b (IN) - Apply MF?
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setApplyMedianFilter ( const bool f_apply_b )
{
    int i;

    for (i = 1; i <= m_maxLevel_i; ++i)
    {
        m_dynProgObj_p[i] -> setApplyMedianFilter ( f_apply_b && m_applyMFAllLevels_b );
    }

    m_dynProgObj_p[0]  -> setApplyMedianFilter ( f_apply_b );
}

/* *************************** METHOD ************************************** */
/**
 * Set if the the application of the median filter must be for all levels.
 *
 *
 * \brief          Set the application of the median filter must be for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param          - f_applyMFAllLevels_b (IN) - Apply MF to all levels?
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setApplyMedianFilterAllLevels ( const bool f_applyMFAllLevels_b )
{
    m_applyMFAllLevels_b = f_applyMFAllLevels_b;    
    setApplyMedianFilter ( m_dynProgObj_p[0]->getApplyMedianFilter() );
}


/* *************************** METHOD ************************************** */
/**
 * Set the size of the median filter kernel for all levels.
 *
 *
 * \brief          Set the size of the median filter kernel for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param          - f_size_i (IN) - Size of the kernel.
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setMedFiltKernelSize ( const int f_size_i )
{
    // The kernel of the median filter is related to the height of the image.
    // Since the height of the image doest not change with the pyramid level,
    // we can set it equal for all levels.

    int i;
    int f_newSize_i = f_size_i;
    
    if (f_newSize_i < 3) f_newSize_i = 3;
    f_newSize_i += 1 - f_newSize_i%2;
    
    for (i = 0; i <= m_maxLevel_i; ++i)
    {
        m_dynProgObj_p[i] -> setMedFiltKernelSize ( f_newSize_i );
    }    
}

/* *************************** METHOD ************************************** */
/**
 * Set the +- correction factor for all levels.
 *
 *
 * \brief          Set the +- correction factor for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param          - f_tol_i (IN) - Correction factor.
 * \param          - (OUT)
 * \param          - (INOUT)
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setFollowPathTolerance ( const int f_tol_i )
{
    int i;
    for (i = 0; i <= m_maxLevel_i; ++i)
    {
        m_dynProgObj_p[i] -> setFollowPathTolerance ( f_tol_i );
    }    
}

/* *************************** METHOD ************************************** */
/**
 * \brief          Set the distance vector for all levels.
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param[in]      - f_vec_p (IN) - Distance vector for level 0 of the pyr.
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */

void CPyramidalDynProg::setExpectedGradient ( const float * const f_vec_p )
{
    // Do something only if vector pointer has changed.
    //if (m_expectedGradient_p[0] == f_vec_p)
    //    return;

    m_expectedGradient_p[0] = (float * )f_vec_p;

    m_dynProgObj_p[0] -> setExpectedGradient ( m_expectedGradient_p[0] );

    if ( f_vec_p == NULL)
    {
        int i;
        // Compute distance vector for every level.
        for (i = 1; i <= m_maxLevel_i; ++i)
            m_dynProgObj_p[i] -> setExpectedGradient ( NULL );
    }
    else
    {
        int f_factor_i = 1;
        int f_height_i = m_dynProgObj_p[0] -> getHeight();
        
        // Compute gradient vector for every level.
        for (int l = 1; l <= m_maxLevel_i; ++l)
        {
            f_factor_i *= 2;

            for (int i = 0; i < f_height_i; ++i)
            {
                (m_expectedGradient_p[l])[i] = (m_expectedGradient_p[0])[i]/f_factor_i;
            }
            
            m_dynProgObj_p[l] -> setExpectedGradient ( m_expectedGradient_p[l] );
        }    
    }
}

/* *************************** METHOD ************************************** */
/**
 * \brief          Set the distance cost vector for all levels.
 * \author         Hernan Badino
 * \date           19.09.2008
 *
 * \note           
 * \remarks        -
 * \sa             -
 *
 * \param[in]      - f_vec_p (IN) - Distance vector for level 0 of the pyr.
 * \return         -
 * \exception      none none
 *
 *************************************************************************** */
void CPyramidalDynProg::setDistCostVector ( std::vector <float> * const f_vec_p )
{
    for (int i = 0; i <= m_maxLevel_i; ++i)
        m_dynProgObj_p[i] -> setDistCostVector ( f_vec_p );
}

/******************************/
/*    COMPUTE METHODS         */
/******************************/

/* *************************** METHOD ************************************** */
/**
 * Computes dynammic programming with the input image and return 
 * as output the optimal path from left to right.
 *
 *
 * \brief          Computes dynammic programming with the input cost image.
 *
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           -
 * \remarks        -
 * \sa             -
 *
 * \param          f_costImg_p (IN)    - Cost image.
 * \param          f_vecRes_p (IN/OUT) - Prediction and output path.
 * \return         bool                - Success of computation.
 *
 *************************************************************************** */

bool 
CPyramidalDynProg::compute ( const CFloatImage      & f_costImg,
                             std::vector<int>       & fr_vecRes_v,
                             const std::vector<int> & f_followPath_v,
                             const std::vector<int> & f_pathTolVector_v )
{
    bool f_res_b;

    if ( m_maxLevel_i == 0 )
    {
        f_res_b =  m_dynProgObj_p[0]  -> compute (  f_costImg,
                                                    fr_vecRes_v );
    }
    else
    {
        /// Compute pyramids first
        f_res_b = computePyramidImgs ( &f_costImg );
        
        if (!f_res_b) return false;
        
        int i, j;
        const int f_height_i = m_dynProgObj_p[0] -> getHeight();

        // Resize vectors.
        m_levelFollowPath_v.resize ( f_followPath_v.size() );
        m_levelPathTolVec_v.resize ( f_pathTolVector_v.size() );

        // We have to start in the last level of the pyramid. The first step
        // is to convert the prediction given in fr_vecRes_p, which is given in 
        // the original cost image coordinates, to a prediction of the last level
        // of the pyramid. Thus, we have just to scale the content of fr_vecRes_p
        // with scale factor 2^level. We do the same for followpath and pathtolvector
        // in case they are available.

        for (j = 0; j < f_height_i; ++j)
        {
            m_levelVecRes_v[j] = fr_vecRes_v[j] >> m_maxLevel_i;

            if (f_followPath_v.size())
            {
                m_levelFollowPath_v[j] = f_followPath_v[j] >> m_maxLevel_i;

                if (f_pathTolVector_v.size())
                    m_levelPathTolVec_v[j] = f_pathTolVector_v[j] >> m_maxLevel_i;
            }            
        }

        //printf("\nComputing level %i\n\n", m_maxLevel_i);
        
        m_dynProgObj_p[m_maxLevel_i]  -> compute ( *m_pyrImg_p[m_maxLevel_i],
                                                   m_levelVecRes_v,
                                                   m_levelFollowPath_v,
                                                   m_levelPathTolVec_v );

        // For every level but the first one.
        for (i = m_maxLevel_i-1; i >= 0 && f_res_b; --i)
        {
            for (j = 0; j < f_height_i; ++j)
            {
                // Transfer path from previous level to current level (*=2)
                //printf("Result for level %i in row %i is %i\n", i+1, j, m_levelVecRes_v[j]);
                
                m_currPath_v[j] = m_levelVecRes_v[j] << 1;
            }

            if (i != 0)
            {
                /// Compute prediction scaling the original input vector.
                for (j = 0; j < f_height_i; ++j)
                {
                    m_levelVecRes_v[j] = fr_vecRes_v[j] >> i;
                }

                //printf("\nComputing level %i\n\n", i);
                m_dynProgObj_p[i]  -> compute ( *m_pyrImg_p[i],
                                                m_levelVecRes_v,
                                                m_currPath_v );
            }
            else
            {                
                // If it is the original level use directly fr_vecRes_v and avoid
                // to scale the prediction.
                m_dynProgObj_p[0]  -> compute ( *m_pyrImg_p[0],
                                                fr_vecRes_v,
                                                m_currPath_v );
            }
        }

        for (j = 0; j < f_height_i; ++j)
        {
            //printf("Result for level 0 in row %i is %i\n", j, fr_vecRes_v[j]);
            
            m_currPath_v[j] = m_levelVecRes_v[j] << 1;
        }
    }
    
    return f_res_b;
}

/* *************************** METHOD ************************************** */
/**
 * Computes cost image pyramid.
 *
 *
 * \brief          Computes cost image pyramid.
 *
 * \author         Hernan Badino
 * \date           20.11.2007
 *
 * \note           -
 * \remarks        -
 * \sa             -
 *
 * \param          f_costImg_p (IN)    - Cost image.
 * \return         bool                - Success of computation.
 *
 *************************************************************************** */
bool 
CPyramidalDynProg::computePyramidImgs ( const CFloatImage * const f_costImg_p )
{
    // First level of the pyramid is not instantiated. We just use the original
    // given parameter.
    m_pyrImg_p[ 0 ] = (CFloatImage * )f_costImg_p;

    const int f_height_i = m_pyrImg_p[0] -> getHeight();

    for (int level = 1; level <= m_maxLevel_i; ++level)
    {
        //printf("Computing from level %i to level %i\n", level-1, level);
        
        const int f_widthDst_i = m_pyrImg_p[ level  ] -> getWidth();
        //const int f_widthSrc_i = m_pyrImg_p[level-1] -> GetWidth();

#if 0 && defined ( _OPENMP ) //// Probably cause of failure.
        const unsigned int numThreads_ui = omp_get_max_threads();
#pragma omp parallel for num_threads(numThreads_ui) schedule(static)
#endif
        for (int i = 0; i < f_height_i; ++i)
        {
            float *src_p = m_pyrImg_p[level-1] -> getScanline(i);
            float *dst_p = m_pyrImg_p[level  ] -> getScanline(i);

            int f_colDst_i = 0, f_colSrc_i = 0;

            for ( ;
                  f_colDst_i < f_widthDst_i;
                  ++f_colDst_i, ++f_colSrc_i, ++f_colSrc_i )
            {
                const float &src1_f = *src_p; ++src_p;
                const float &src2_f = *src_p; ++src_p;

                float &dst_f = *dst_p; ++dst_p;

                if ( 0 )
                {
                    if ( src1_f < 0 || src2_f < 0)
                    {
                        printf("level %i pos i:%i j:%i val1 %f val2 %f \n",
                               level, i, f_colSrc_i, src1_f, src2_f);
                    }
                }
                
                if ( src1_f > 0 && src2_f > 0 )
                    dst_f = std::min(src1_f, src2_f);
                else if (src1_f <= 0)      
                    dst_f = src2_f;
                else //if (src2_f <= 0) 
                    dst_f = src1_f;
                
                //dst_f = std::max(src1_f, src2_f);

                // (0 && random() > RAND_MAX/2)
                //  dst_f = std::max(src1_f, src2_f);
                //se
                //  dst_f = std::min(src1_f, src2_f);

                //dst_f = (src1_f + src2_f);
                //dst_f = (src1_f + src2_f)/2.;
            }
        }
    }
    
    return true;
}
