/*@@@**************************************************************************
 * \file  sriHolesFiller
 * \author Hernan Badino
 * \date  Mon Aug  3 14:46:22 EDT 2009
 * \notes 
 *******************************************************************************
 *****          (C) COPYRIGHT Hernan Badino - All Rights Reserved          *****
 ******************************************************************************/

/* INCLUDES */
#include "sriHolesFiller.h"

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

using namespace VIC;


CSriHolesFiller::CSriHolesFiller()
        : m_holesCount_p (           NULL ),
          m_vectorSize_ui (             0 ),
          m_maxImgDistance_ui (     20.0f ),
          m_minRangeProportion_d (    0.9 ),
          m_minHoles_ui (              20 )
{
    
}

CSriHolesFiller::~CSriHolesFiller()
{
    if ( m_holesCount_p )
        delete [] m_holesCount_p;
    
}

        
bool
CSriHolesFiller::fillHoles ( CSphericalRangeImage &f_sri,
                             const bool            f_fillVertEmptyLines_b )
{
    const int w_i = f_sri.getImage().getWidth();
    const int h_i = f_sri.getImage().getHeight();
    
    if ( (int)m_vectorSize_ui < h_i)
    {
        if (m_holesCount_p)
            delete [] m_holesCount_p;
        
        m_holesCount_p = new uint16_t[h_i];
        m_vectorSize_ui = h_i;
    }

    double * const dist_p = (double * )f_sri.getImage().getData();

    /// Max distance between two measurements to apply interpolation.
    //unsigned int m_maxImgDistance_ui = 20;
    //double       m_minRangeProportion_d = 0.9;
    //unsigned int m_minHoles_ui = 10;    
        
    /// Fill first horizontal missing holes.
#if defined ( _OPENMP )
    const unsigned int numThreads_ui = omp_get_max_threads();
#pragma omp parallel for num_threads(numThreads_ui) schedule(dynamic)
#endif
    for (int i = 0; i < h_i; ++i)
    {
        m_holesCount_p[i] = 0;
        
        double *scanLine = dist_p + i * w_i;

        double dist1_f = 0.f;
        double dist2_f = 0.f;
        int   pos1_i, pos2_i;

        int j = 0;
        while (j < w_i && scanLine[j] == 0.f)
        {
            ++j;
        }
        ++j;

        while (j < w_i)
        {
            int lastJ_i = j;
            while (j < w_i && scanLine[j] != 0.f)
            {
                ++j;
            }

            m_holesCount_p[i] += j - lastJ_i + 1;
            //printf("emptyLine[%i] = %u\n", i, m_holesCount_p[i]);
            
            pos1_i  = j - 1;
            dist1_f = scanLine[pos1_i];

            ++j;
            
            while (j < w_i && scanLine[j] == 0.f)
            {
                ++j;
            }

            pos2_i  = j;
            
            if ( pos2_i < (int) w_i )
            {
                dist2_f = scanLine[pos2_i];
                
                double f1_d = fabs(dist1_f);
                double f2_d = fabs(dist2_f);
                double prop_d = f1_d > f2_d?f2_d/f1_d:f1_d/f2_d;
                
                //SHOWVAL(prop_d);
                
                if ( pos2_i - pos1_i < (int)m_maxImgDistance_ui &&
                     prop_d > m_minRangeProportion_d )
                {
                    //dist2_f = scanLine[pos2_i];
                    
                    int k = pos1_i + 1;
                    double step_f = (dist2_f-dist1_f)/(pos2_i - pos1_i);
                    
                    while ( k < pos2_i )
                    {
                        scanLine[k] = scanLine[k-1] + step_f;
                        ++k;
                    }
                }
            }
            
            ++j;
        }
        //printf("Final emptyLine[%i] = %u, %i (w_i = %i)\n", i, m_holesCount_p[i], (m_holesCount_p[i]>w_i/10)?1:0, w_i);
        //m_holesCount_p[i] = (m_holesCount_p[i]>20)?0:1;
    }
    
    if (f_fillVertEmptyLines_b)
    {
        int i = 0;
        while ( i <  h_i &&
                m_holesCount_p[i] <= m_minHoles_ui )
            ++i;

        int filledLine1_i;
        int filledLine2_i;
    
        /// Identify the first two lines to interpolate.
        ++i;
        while ( i <  h_i )
        {
            while ( i <  h_i &&
                    m_holesCount_p[i] > m_minHoles_ui )
                ++i;
            filledLine1_i = i - 1;

            ++i;

            while ( i <  h_i &&
                    m_holesCount_p[i] <= m_minHoles_ui )
                ++i;
            filledLine2_i = i;

            if ( filledLine2_i < h_i )
            {
                /// Here I use a special strategy to avoid cache misses.
                /// The first line to interpolate is normally interpolated.
                /// Every following empty lines is updated extrapolating the previous
                /// two lines. In this way, most cache misses.
                /// will only occur when interpolating the first line, since the 
                /// amount of data that needs to be moved to the cache might be 
                /// large. But the interpolation for the 2nd and following lines
                /// needs only to load in the cache the last 3 scanlines of the 
                /// image, leading to less cache misses (unless the width of the
                /// image is huge, in which case cache misses will occur anyway).
                int k = filledLine1_i + 1;

                {
                    double *scanLine1_p = dist_p + filledLine1_i * w_i;
                    double *scanLine_p  = dist_p + (k) * w_i;
                    double *scanLine2_p = dist_p + filledLine2_i * w_i;
                    /// Lets interpolate now.
                    /// Interpolate only first line.
                    int linesDistance_i = (filledLine2_i-filledLine1_i);
                    for (int j = 0; j < w_i; ++j)
                    {
                        double f1_d = fabs(scanLine1_p[j]);
                        double f2_d = fabs(scanLine2_p[j]);
                        double prop_d = f1_d > f2_d?f2_d/f1_d:f1_d/f2_d;

                        if ( prop_d > m_minRangeProportion_d )
                        {
                            if ( scanLine1_p[j] == 0 )
                                scanLine_p[j] = 0; // scanLine1_p[j];
                            else
                                if ( scanLine2_p[j] == 0 )
                                    scanLine_p[j] = 0; //scanLine2_p[j];
                                else
                                {
                                    scanLine_p[j] = scanLine1_p[j] + (scanLine2_p[j] - scanLine1_p[j])/(linesDistance_i);
                                    /*
                                    printf("%i Interpolation of %f and %f for position %i (to %i) es %f\n",
                                           j,
                                           scanLine1_p[j],
                                           scanLine2_p[j],
                                           k,
                                           filledLine2_i,
                                           scanLine_p[j] );
                                    */
                                }

                        }
                    }
                }
            
                ++k;

                while (k < filledLine2_i)
                {
                    double *scanLine_2_p = dist_p + (k-2) * w_i;
                    double *scanLine_1_p = dist_p + (k-1) * w_i;
                    double *scanLine_p   = dist_p + (k  ) * w_i;

                    /// Interpolate 2nd and and following lines.
                    for (int j = 0; j < w_i; ++j)
                    {
                        if (scanLine_1_p[j] != 0)
                            scanLine_p[j] = 2*scanLine_1_p[j] - scanLine_2_p[j];
                        /*
                        printf("%i Extrapolation of %f and %f for position %i (to %i) es %f\n",
                               j,
                               scanLine_2_p[j],
                               scanLine_1_p[j],
                               k,
                               filledLine2_i,
                               scanLine_p[j] );
                        */
                    }
                    ++k;
                }
            }

            ++i;
        }
    }   
    return true;
}

bool
CSriHolesFiller::addParameters ( CParameterSet & fr_paramSet )
{
    /// Braking naming rules to be able to use the macros.
    CParameterSet *m_paramSet_p = &fr_paramSet;
    
    BEGIN_PARAMETER_GROUP("SRI Holes Filler", false, CColor::white );

      ADD_UINT_PARAMETER( "Max image distance",
                          "Max image distance of two neighbors to fill the "
                          "hole between them.",
                          m_maxImgDistance_ui,
                          this,
                          MaxImgDistance,
                          CSriHolesFiller );

      ADD_DOUBLE_PARAMETER( "Min Range Proportion",
                            "Min range proportion between the neighbors "
                            "to fill the  hole between them.",
                            m_minRangeProportion_d,
                            this,
                            MinRangeProportion,
                            CSriHolesFiller );

      ADD_UINT_PARAMETER( "Min holes",
                          "Minimum number of holes in a line required to apply "
                          "vertical interpolation to those lines",
                          m_minHoles_ui,
                          this,
                          MinHoles,
                          CSriHolesFiller );

    END_PARAMETER_GROUP;

    return true;
}

/* ////////////  Version History ///////////////
 *  $Log: sriHolesFiller.cpp,v $
 *  Revision 1.2  2009/11/18 15:50:14  badino
 *  badino: global changes.
 *
 *//////////////////////////////////////////////
