/*@@@**************************************************************************
 * \file  rangePredictor.cpp
 * \author Hernan Badino
 * \date  Mon Jun 22 14:54:58 EDT 2009
 * \notes 
 *******************************************************************************
 *****     (C) COPYRIGHT Hernan Badino - All Rights Reserved               *****
 ******************************************************************************/

/* INCLUDES */
#include "velodyneStream.h"
#include "rangePredictor.h"
#include "sphericalRangeImage.h"
#include "drawingList.h"
#include "colorEncoding.h"
#include "stereoCamera.h"
#include "uShortImage.h"

#include "minMaxFilter.h"

#include "ippDefs.h"

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

#include <sys/time.h>

using namespace VIC;

/// Constructors.
CRangePredictorOp::CRangePredictorOp ( COperator * const f_parent_p )
        : COperator (   f_parent_p, "RangePredictor" ),
          m_minRangeImage (                            ),
          m_maxRangeImage (                            ),
          m_minMaxSriKernelSize (                 7, 5 ),
          m_maskMinMaxFilter_b (                 false ),
          m_minMaxDispKernelSize (                5, 7 ),
          m_sriDeltas (                        -.5, .5 ),
          m_applyStrategy1_b (                    true ),
          m_lutU (                                     ),
          m_lutV (                                     ),
          m_copiedSri (                                ),
          m_remappedFRImage (                          ),
          m_remappedRImage (                           ),
          m_remapRightImg_b (                    false ),
          m_interpRightImg_b (                    true ),
          m_dispOffset_i (                          -5 ),
          m_predMinRightDispImg (                      ),
          m_predMaxRightDispImg (                      ),
          m_predRightDispImg (                         ),
          m_predict4RightImg_b (                 false )
{
    registerDrawingLists();

    BEGIN_PARAMETER_GROUP("Disparity Image Prediction", false, CColor::red );
    
    ADD_BOOL_PARAMETER( "Apply Strategy 1?",
                        "Apply strategy 1? If not, Strategy 2 will be applied.",
                        m_applyStrategy1_b,
                        this,
                        ApplyStrategy1,
                        CRangePredictorOp );


    ADD_INT2D_PARAMETER( "Min/Max SRI Kernel Size",
                         "Kernel size for the application of the min/max "
                         "filter on the sri image (corresponding to Strategy 1)",
                         m_minMaxSriKernelSize,
                         "W", "H",
                         this,
                         MinMaxSriKernelSize,
                         CRangePredictorOp );
 
    ADD_BOOL_PARAMETER( "Apply invalid mask to output?",
                        "Invalidate image position which corresponds to "
                        "filters using invalid values.",
                        m_maskMinMaxFilter_b,
                        this,
                        MaskMinMaxFilter,
                        CRangePredictorOp );



    ADD_INT2D_PARAMETER( "Min/Max Disp Img Kernel Size",
                         "Kernel size for the application of the min/max "
                         "filter on the disparity image (corresponding to Strategy 1&2)",
                         m_minMaxDispKernelSize,
                         "W", "H",
                         this,
                         MinMaxDispKernelSize,
                         CRangePredictorOp );

    ADD_DBL2D_PARAMETER( "SRI Delta Distance for SRI",
                         "Delta distance to apply to the SRI before predicting "
                         "the corresponding disparity image (corresponding to Strategy 1&2)",
                         m_sriDeltas,
                         "Sub", "Add",
                         this,
                         SriDeltas,
                         CRangePredictorOp );

    ADD_BOOL_PARAMETER( "Predict 4 Right Image",
                        "Create a max/min and disp prediction for the right iamge as well.",
                        m_predict4RightImg_b,
                        this,
                        Predict4RightImg,
                        CRangePredictorOp );

    ADD_LINE_SEPARATOR;

    ADD_BOOL_PARAMETER( "Remap Right Image",
                        "Created a remapped right image with the predicted disp img.",
                        m_remapRightImg_b,
                        this,
                        RemapRightImg,
                        CRangePredictorOp );

    ADD_BOOL_PARAMETER( "Interpolate Holes?",
                        "Interpolate holes in remapped right image.",
                        m_interpRightImg_b,
                        this,
                        InterpRightImg,
                        CRangePredictorOp );

    ADD_INT_PARAMETER( "Disparity offset",
                       "Disparity offset for remapped right image.",
                       m_dispOffset_i,
                       this,
                       DispOffset,
                       CRangePredictorOp );

    END_PARAMETER_GROUP;
    BEGIN_PARAMETER_GROUP("Display Options", false, CColor::red );

      addDrawingListParameter ( "Min Velodyne Image" );
      addDrawingListParameter ( "Max Velodyne Image" );
      addDrawingListParameter ( "Predicted Max Disp Image" );
      addDrawingListParameter ( "Predicted Min Disp Image" );
      addDrawingListParameter ( "Predicted Disp Image" );
      addDrawingListParameter ( "LUT Image" );
      addDrawingListParameter ( "Remapped Rectified Right Image" );

      addDrawingListParameter ( "Predicted Max Right Disp Image" );
      addDrawingListParameter ( "Predicted Min Right Disp Image" );
      addDrawingListParameter ( "Predicted Right Disp Image" );

    END_PARAMETER_GROUP;
}

/// Virtual destructor.
CRangePredictorOp::~CRangePredictorOp ()
{
}

void CRangePredictorOp::registerDrawingLists()
{
    /// Register drawing list.
    registerDrawingList ("Min Velodyne Image",
                         S2D<int> (0, 2),
                         false );

    /// Register drawing list.
    registerDrawingList ("Max Velodyne Image",
                         S2D<int> (1, 2),
                         false );

    registerDrawingList ("Predicted Max Disp Image",
                         S2D<int> (2, 2),
                         true );

    registerDrawingList ("Predicted Min Disp Image",
                         S2D<int> (1, 2),
                         true );

    registerDrawingList ("Predicted Disp Image",
                         S2D<int> (1, 2),
                         false );

    registerDrawingList ("LUT Image",
                         S2D<int> (1, 2),
                         false );

    registerDrawingList ("Remapped Rectified Right Image",
                         S2D<int> (1, 1),
                         false );

    registerDrawingList ("Predicted Max Right Disp Image",
                         S2D<int> (2, 2),
                         true );

    registerDrawingList ("Predicted Min Right Disp Image",
                         S2D<int> (1, 2),
                         true );

    registerDrawingList ("Predicted Right Disp Image",
                         S2D<int> (1, 2),
                         false );
}

/// Init event.
bool CRangePredictorOp::initialize()
{
    allocateSRIs();

    m_minRangeImage.clear();
    m_maxRangeImage.clear();
    
    CUShortImage * imgL_p = dynamic_cast<CUShortImage *>(getInput ( "Rectified Left Image" ) );

    if ( imgL_p )
    {
        S2D<unsigned int> imgSize = imgL_p -> getSize();
        
        m_predMinDispImg.freeMemory();
        m_predMinDispImg.setSize ( imgSize );
        m_predMinDispImg.ensureAllocation();

        m_predMaxDispImg.freeMemory();
        m_predMaxDispImg.setSize ( imgSize );
        m_predMaxDispImg.ensureAllocation();

        m_auxDispImg1.freeMemory();
        m_auxDispImg1.setSize ( imgSize );
        m_auxDispImg1.ensureAllocation();

        m_auxDispImg2.freeMemory();
        m_auxDispImg2.setSize ( imgSize );
        m_auxDispImg2.ensureAllocation();

        m_predRightDispImg.freeMemory();
        m_predRightDispImg.setSize ( imgSize );
        m_predRightDispImg.ensureAllocation ( );

        m_predMinRightDispImg.freeMemory();
        m_predMinRightDispImg.setSize ( imgSize );
        m_predMinRightDispImg.ensureAllocation ( );

        m_predMaxRightDispImg.freeMemory();
        m_predMaxRightDispImg.setSize ( imgSize );
        m_predMaxRightDispImg.ensureAllocation ( );
    }
    
    return COperator::initialize();
}

/// Reset event.
bool CRangePredictorOp::reset()
{
    return COperator::reset();
}

bool CRangePredictorOp::exit()
{
    return COperator::exit();
}

/// Cycle event.
bool CRangePredictorOp::cycle()
{
    computeMinMaxDisparityImages();
    
    struct timeval tv1, tv2;
        
    gettimeofday(&tv1, NULL);

    recreateLuts();
    
    predictDisparityImage2 ( m_copiedSri, 
                             m_predDispImg );

    if ( m_remapRightImg_b )
        remapRightImage ( );
    
    if (m_predict4RightImg_b)
    {
        predict4RightImg();
    }
    

    gettimeofday(&tv2, NULL);
    printf("New Disparity Image Prediction: %lf milliseconds\n",
           (((double)tv2.tv_usec) - ((double)tv1.tv_usec))/1000.);

    registerOutput ( "Predicted Disparity Image", &m_predDispImg  );    
    registerOutput ( "Predicted Min Disp Image",  &m_predMinDispImg  );    
    registerOutput ( "Predicted Max Disp Image",  &m_predMaxDispImg  );
    registerOutput ( "DispImg Prediction Lut U",  &m_lutU  );
    registerOutput ( "DispImg Prediction Lut V",  &m_lutV  );
    registerOutput ( "Remapped Rectified Right Image",  &m_remappedRImage  );

    static CIO_int dispOffset = m_dispOffset_i;
    registerOutput ( "Disp Offset for Remapped RRI",  &dispOffset  );

    //printf("registered output for max disp img is %p\n", &m_predMaxDispImg );
    
    registerOutput ( "Predicted Right Disparity Image", &m_predDispImg  );    
    registerOutput ( "Predicted Min Right Disp Image",  &m_predMinDispImg  );    
    registerOutput ( "Predicted Max Right Disp Image",  &m_predMaxDispImg  );
    
    return COperator::cycle();
}

void
CRangePredictorOp::predict4RightImg()
{
    S2D<unsigned int> imgSize = m_predDispImg.getSize();

    if ( imgSize !=
         m_predRightDispImg.getSize() )
    {
        m_predRightDispImg.freeMemory();
        m_predRightDispImg.setSize ( imgSize );
        m_predRightDispImg.ensureAllocation ( );

        m_predMinRightDispImg.freeMemory();
        m_predMinRightDispImg.setSize ( imgSize );
        m_predMinRightDispImg.ensureAllocation ( );

        m_predMaxRightDispImg.freeMemory();
        m_predMaxRightDispImg.setSize ( imgSize );
        m_predMaxRightDispImg.ensureAllocation ( );
    }

    m_predMinRightDispImg.clear();
    m_predMaxRightDispImg.clear();
    m_predRightDispImg.clear();
    
    int h_i = m_predDispImg.getHeight();
    int w_i = m_predDispImg.getWidth();

#if defined ( _OPENMP )
    const int numThreads_i = (int) std::min(omp_get_max_threads(),32);
#pragma omp parallel for num_threads(numThreads_i) schedule(dynamic)
#endif
    for (int i = 0; i < h_i; ++i)
    {
        float *  ld_p = m_predDispImg.getScanline(i);
        float *  rd_p = m_predRightDispImg.getScanline(i);

        for (int j = 0; j < w_i; ++j, ++ld_p )
        {
            if (*ld_p >= 0)
            {
                int newj_i = (int)(j - *ld_p + .5);

                if (newj_i > 0 && *ld_p > rd_p[newj_i])
                    rd_p[newj_i]   = *ld_p;
            }
        }
    }

#if defined ( _OPENMP )
#pragma omp parallel for num_threads(numThreads_i) schedule(dynamic)
#endif
    for (int i = 0; i < h_i; ++i)
    {
        float *  minl_p = m_predMinDispImg.getScanline(i);
        float *  minr_p = m_predMinRightDispImg.getScanline(i);

        for (int j = 0; j < w_i; ++j, ++minl_p)
        {
            if (*minl_p >= 0)
            {
                int newj_i = (int)(j - *minl_p + .5);

                if (newj_i > 0 && *minl_p > minr_p[newj_i])
                    minr_p[newj_i] = *minl_p;
            }
        }
    }

#if defined ( _OPENMP )
#pragma omp parallel for num_threads(numThreads_i) schedule(dynamic)
#endif
    for (int i = 0; i < h_i; ++i)
    {
        float *  maxl_p = m_predMaxDispImg.getScanline(i);
        float *  maxr_p = m_predMaxRightDispImg.getScanline(i);

        for (int j = 0; j < w_i; ++j, ++maxl_p)
        {
            if (*maxl_p >= 0)
            {
                int newj_i = (int)(j - *maxl_p + .5);

                if (newj_i > 0 && *maxl_p > maxr_p[newj_i])
                    maxr_p[newj_i] = *maxl_p;
            }
        }
    }    
}


bool 
CRangePredictorOp::remapRightImage ( )
{
    CFloatImage *imgL_p = dynamic_cast<CFloatImage *>(getInput ( "Rectified Float Left Image" ) );
    CFloatImage *imgR_p = dynamic_cast<CFloatImage *>(getInput ( "Rectified Float Right Image" ) );

    if (!imgL_p || !imgR_p) return false;

    S2D<unsigned int> imgSize = m_predDispImg.getSize();

    if ( imgSize !=
         m_remappedRImage.getSize() )
    {
        m_remappedRImage.freeMemory();
        m_remappedRImage.setSize ( imgSize );
        m_remappedRImage.ensureAllocation ( );

        m_remappedFRImage.freeMemory();
        m_remappedFRImage.setSize ( imgSize );
        m_remappedFRImage.ensureAllocation ( );
    }

    int h_i = m_predDispImg.getHeight();
    int w_i = m_predDispImg.getWidth();


    //m_remappedFRImage.copyImageContent(*imgL_p);
    //m_remappedFRImage.fill(-1);
    
    
    //#undef _OPENMP
#if not defined ( _OPENMP )
    float *  factors_p = new float[w_i];
#else
    const int numThreads_i = (int) std::min(omp_get_max_threads(),32);
    float *factorsVec_p[32];

    for (int i = 0; i < numThreads_i; ++i)
        factorsVec_p[i] = new float[w_i];
    
#pragma omp parallel for num_threads(numThreads_i) schedule(dynamic)
#endif
    for (int i = 0; i < h_i; ++i)
    {
#if defined ( _OPENMP )
        const unsigned int threadNum_ui = omp_get_thread_num();
        float *factors_p = factorsVec_p[threadNum_ui];
#endif        
        memset(factors_p, 0, sizeof(float) * w_i);
        
        float *  disp_p = m_predDispImg.getScanline(i);
        float *  srcR_p = imgR_p->getScanline(i);
        float *  dstR_p = m_remappedFRImage.getScanline(i) + m_dispOffset_i;
        float *  left_p = imgL_p->getScanline(i);

        for (int j = 0; j < w_i; ++j, ++disp_p, ++dstR_p, ++left_p)
        {
            if (j + m_dispOffset_i < 0 || j + m_dispOffset_i >= w_i) continue;

            if (*disp_p>0)
            {
                float newJ_f = j - *disp_p;
                int newJ_i = (int)(newJ_f + 0.5);
                
                /// NN or Linear interpolation?
                bool interpolate_b = false;

                if ( !interpolate_b ) 
                {
                    *dstR_p  = srcR_p[newJ_i];
                }
                else
                {
                    float f1,f2;
                    int idx1_i;
                    int idx2_i;
                    if (newJ_i > newJ_f)
                    {
                        idx1_i = newJ_i;
                        idx2_i = newJ_i-1;
                        f2 = newJ_i - newJ_f;
                        f1 = 1-f2;
                    }
                    else
                    {
                        idx1_i = newJ_i+1;
                        idx2_i = newJ_i;
                        f1 = newJ_f - newJ_i;
                        f2 = 1-f1;
                    }

                    //printf("for (%i %i) disp %f idx1_i = %i idx2_i = %i (f1 = %f f2 = %f)\n",
                    //       i, j, *disp_p, idx1_i, idx2_i, f1, f2 );

                    if (idx1_i >= 0 && idx1_i < w_i && 
                        idx2_i >= 0 && idx2_i < w_i ) 
                    {
                        *dstR_p = srcR_p[idx1_i] * f1 + srcR_p[idx2_i] * f2;
                    }
                    /*
                    else
                        if ( idx1_i >= 0 && idx1_i < w_i )
                        {
                            *dstR_p = srcR_p[idx1_i];
                        }
                        else
                            if ( idx2_i >= 0 && idx2_i < w_i ) 
                            {
                                *dstR_p = srcR_p[idx2_i];
                            }
                    */

                    /*
                    if (idx1_i >= 0 && idx1_i < w_i)
                    {
                        if (factors_p[idx1_i] > 0)
                        {   
                            float sumFactors_f = factors_p[idx1_i] + f1;
                            dstR_p[idx1_i] = ( dstR_p[idx1_i] * factors_p[idx1_i] + 
                                               *srcR_p * f1) / sumFactors_f;
                            factors_p[idx1_i] = sumFactors_f;
                        }
                        else
                        {
                            dstR_p[idx1_i]    = *srcR_p;
                            factors_p[idx1_i] = f1;
                        }
                    }
                
                    if (idx2_i >= 0 && idx2_i < w_i)
                    {
                        if (factors_p[idx2_i] > 0)
                        {   
                            float sumFactors_f = factors_p[idx2_i] + f2;
                            dstR_p[idx2_i] = ( dstR_p[idx2_i] * factors_p[idx2_i] + 
                                               *srcR_p * f2) / sumFactors_f;
                            factors_p[idx2_i] = sumFactors_f;
                        }
                        else
                        {
                            dstR_p[idx2_i]    = *srcR_p;
                            factors_p[idx2_i] = f2;
                        }
                    }
                    */
                }
            }
            else
                *dstR_p = *left_p;
        }
    }    
#if not defined (_OPENMP)
    delete [] factors_p;
#else
    for (int i = 0; i < numThreads_i; ++i)
        delete factorsVec_p[i];
#endif

    if ( m_interpRightImg_b )
    {
        /// Lets now interpolate wholes.
#if defined (_OPENMP)
#pragma omp parallel for num_threads(numThreads_i) schedule(dynamic)
#endif
        for (int i = 0; i < h_i; ++i)
        {
            float *  dstR_p = m_remappedFRImage.getScanline(i);
            int j = 0;
            
            while ( dstR_p[j] == -1 && j < w_i ) ++j;
            
            while ( dstR_p[j] != -1 && j < w_i )
            {
                while ( dstR_p[j] != -1 && j < w_i ) ++j;
                int firstIdx=j-1;
                
                while ( dstR_p[j] == -1 && j < w_i ) ++j;
                int secondIdx=j;
                
                /// This can be optimized.
                float slope_f = (dstR_p[secondIdx] - dstR_p[firstIdx])/(secondIdx-firstIdx);
                for (int k = firstIdx+1; k < secondIdx; ++k)
                {
                    dstR_p[k] = dstR_p[secondIdx] + slope_f * (k-firstIdx);
                }
            }
        }    
    }
    
    IppiSize  srcSize_o;
    srcSize_o.width  = m_remappedFRImage.getWidth();
    srcSize_o.height = m_remappedFRImage.getHeight();

    IppiSize dstSize_o;
    dstSize_o.width  = m_remappedRImage.getWidth();
    dstSize_o.height = m_remappedRImage.getHeight();

    IppStatus ippStatus_i;
    /// Convert float image to unsigned short image. LEFT
    ippStatus_i =
        ippiConvert_32f16u_C1R ( (Ipp32f*) m_remappedFRImage.getData(),
                                 dstSize_o.width * sizeof(float),
                                 (Ipp16u*) m_remappedRImage.getData(),
                                 dstSize_o.width * sizeof(unsigned short int),
                                 srcSize_o, ippRndNear );

    if (ippStatus_i != ippStsNoErr)
    {
        logger::warn("IPP failed:");
        printIPPError( ippStatus_i );
        return false;
    }

    return true;
}


bool
CRangePredictorOp::predictDisparityImage2 ( const CSphericalRangeImage &f_sri,
                                            CFloatImage                &fr_predDispImg )
{
    const CStereoCamera * camera_p = 
        dynamic_cast<CStereoCamera*> ( getInput( "Rectified Camera" ) );

    if ( !camera_p )
        return false;
    

    const CDoubleImage &sriDImage = f_sri.getImage();

    S2D<unsigned int> imgSize = sriDImage.getSize();

    if ( m_sriFImage.getSize() != imgSize )
    {
        m_sriFImage.freeMemory();
        m_sriFImage.setSize ( imgSize );
        m_sriFImage.ensureAllocation ( );
    }
    
    unsigned int bytes_ui = imgSize.width * imgSize.height;
    float  * fData_p = m_sriFImage.getData();
    double * dData_p = sriDImage.getData();

    for (unsigned int i = 0; i < bytes_ui; ++i, ++fData_p, ++dData_p)
        *fData_p = *dData_p<=0?0:camera_p -> getDisparityFromDistance ( (float)(*dData_p) ); 

    if ( fr_predDispImg.getSize() !=
         m_lutU.getSize() )
    {
        fr_predDispImg.freeMemory();
        fr_predDispImg.setSize ( m_lutU.getSize() );
        fr_predDispImg.ensureAllocation ( );
    }

    const int interpolation_i = IPPI_INTER_NN;//LINEAR; //IPPI_INTER_CUBIC; 
    IppiSize  srcSize_o;

    srcSize_o.width  = imgSize.width;
    srcSize_o.height = imgSize.height;

    IppiRect srcROI_o;
    srcROI_o.x      = 0;
    srcROI_o.y      = 0;
    srcROI_o.width  = srcSize_o.width;
    srcROI_o.height = srcSize_o.height;

    IppiSize dstSize_o;
    dstSize_o.width  = fr_predDispImg.getWidth();
    dstSize_o.height = fr_predDispImg.getHeight();

    IppStatus ippStatus_i;

    /// Remap Image.
    ippStatus_i =
        ippiRemap_32f_C1R( (Ipp32f*) m_sriFImage.getData ( ),        // src
                           srcSize_o,                                // size
                           srcROI_o.width * sizeof(float),           // step src
                           srcROI_o,                                 // roi src
                           (Ipp32f*) m_lutU.getData ( ),             // pxMap
                           dstSize_o.width * sizeof(float),          // step pxMap
                           (Ipp32f*) m_lutV.getData ( ),             // pyMap
                           dstSize_o.width * sizeof(float),          // step pyMap
                           (Ipp32f*) fr_predDispImg.getData ( ),     // dest
                           dstSize_o.width * sizeof(float),          // step dest
                           dstSize_o,                                // roi dest
                           interpolation_i );                        // interpolation

    if (ippStatus_i != ippStsNoErr)
    {
        logger::warn("IPP failed:");
        printIPPError( ippStatus_i );
        return false;
    }
    
    return true;
}

void
CRangePredictorOp::recreateLuts()
{
    const CStereoCamera * camera_p = 
        dynamic_cast<CStereoCamera*> ( getInput( "Rectified Camera" ) );

    const CSphericalRangeImage * repSri_p = 
        dynamic_cast<CSphericalRangeImage*> ( getInput( "Reprojected SRI" ) );

    const CVelodyne * velodyne_p = 
        dynamic_cast<CVelodyne *> ( getInput( "Velodyne Sensor" ) );

    CUShortImage  * leftImg_p = dynamic_cast<CUShortImage  *>(getInput ( "Rectified Left Image" ) );

    if ( !leftImg_p || 
         !velodyne_p ||
         !camera_p || 
         !repSri_p )
        return;
    
    S2D<unsigned int> imgSize = leftImg_p -> getSize();

    /// Check if reallocation is required.
    if ( m_lutU.getSize () == imgSize &&
         repSri_p -> hasSameFormat ( m_copiedSri) )
        return;

    m_copiedSri.copyFrom ( *repSri_p,  
                           false,  /// allocate?
                           true ); /// reuseLuts?
    
    m_lutU.freeMemory();
    m_lutV.freeMemory();
    m_lutU.setSize ( imgSize );
    m_lutV.setSize ( imgSize );
    m_lutU.ensureAllocation();
    m_lutV.ensureAllocation();

    memset(m_lutU.getData(), 0xff, imgSize.width * imgSize.height );
    memset(m_lutV.getData(), 0xff, imgSize.width * imgSize.height );

    int h_i =  imgSize.height;

    double u_d, v_d;

#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)
    {
        C3DVector worldPnt;
        SSphericalPointData velPnt;
        
        float * lU_p = m_lutU.getScanline ( i );
        float * lV_p = m_lutV.getScanline ( i );
        
        for (unsigned int j = 0; j < imgSize.width; ++j, ++lU_p, ++lV_p)
        {
            if ( camera_p -> image2World ( j, i, 10, 
                                           worldPnt ) )
            {
                if ( velodyne_p -> local2Spherical ( worldPnt, velPnt ) )
                {
                    if ( repSri_p -> spherical2Image ( velPnt.azimuth_d,
                                                       velPnt.elevation_d,
                                                       u_d, v_d ) )
                    {
                        *lU_p = u_d;
                        *lV_p = v_d;
                        
                        if (0) 
                            printf ("img %i %i 10, velPnt = %f %f %f - Img %f %f (%ix%i)\n",
                                    j, i, velPnt.azimuth_d, velPnt.elevation_d, velPnt.range_d,
                                    u_d, v_d, 
                                    repSri_p->getImage().getWidth(),
                                    repSri_p->getImage().getHeight() );            
                    }
                }
            }
        }
    }
}


void
CRangePredictorOp::computeMinMaxDisparityImages()
{
    const CSphericalRangeImage * sri_p = 
        dynamic_cast<CSphericalRangeImage*> ( getInput( "SRI" ) );

    if (sri_p)
    {
        if ( not sri_p -> hasSameFormat ( m_minRangeImage ) )
        {
            printf("SRIs DON'T HAVE THE SAME FORMAT.");
            allocateSRIs();
        }
          
        struct timeval tv1, tv2;
        
        m_minRangeImage.setVelodyne (  sri_p -> getVelodyne() );
        m_maxRangeImage.setVelodyne (  sri_p -> getVelodyne() );

        m_maxRangeImage.getImage().clear();
        m_maxRangeImage.getImage().clear();

        
        if (m_applyStrategy1_b)
        {
            gettimeofday(&tv1, NULL);
            
            CMinMaxFilterBase::EFilterMode mode_e;
            
            if ( m_maskMinMaxFilter_b )
                mode_e = CMinMaxFilterBase::FM_MASK_INVALID;
            else
                mode_e = CMinMaxFilterBase::FM_IGNORE_INVALID;

            CMinMaxFilter<double>::filterMaximum (  sri_p -> getImage(),
                                                    m_maxRangeImage.getImage(),
                                                    m_minMaxSriKernelSize.width, 
                                                    m_minMaxSriKernelSize.height, 
                                                    0., 
                                                    mode_e);

            CMinMaxFilter<double>::filterMinimum (  sri_p -> getImage(),
                                                    m_minRangeImage.getImage(),
                                                    m_minMaxSriKernelSize.width, 
                                                    m_minMaxSriKernelSize.height, 
                                                    0., 
                                                    mode_e );
            //gettimeofday(&tv1, NULL);
            
            predictDisparityImage(m_minRangeImage, m_auxDispImg1, m_sriDeltas.min );
            //predictDisparityImage2(m_minRangeImage, m_auxDispImg1, m_sriDeltas.min );            

            CMinMaxFilter<float,float>::filterMaximum (  m_auxDispImg1,
                                                         m_predMaxDispImg,
                                                         m_minMaxDispKernelSize.width,
                                                         m_minMaxDispKernelSize.height,
                                                         0.,
                                                         CMinMaxFilterBase::FM_IGNORE_INVALID );

            predictDisparityImage(m_maxRangeImage, m_auxDispImg1,  m_sriDeltas.max );

            CMinMaxFilter<float,float>::filterMinimum (  m_auxDispImg1,
                                                         m_predMinDispImg,
                                                         m_minMaxDispKernelSize.width,
                                                         m_minMaxDispKernelSize.height,
                                                         0.,
                                                         CMinMaxFilterBase::FM_IGNORE_INVALID );
            gettimeofday(&tv2, NULL);

            printf("Disparity Image Prediction Strategy 1: %lf milliseconds\n",
                   (((double)tv2.tv_usec) - ((double)tv1.tv_usec))/1000.);
            if ( 0 )
                computeDisparityReduction ( );
        }
        else
        {
            gettimeofday(&tv1, NULL);
            //predictDisparityImage(*sri_p, m_auxDispImg, 0.f );
            predictDisparityImages( *sri_p, 
                                    m_auxDispImg1, 
                                    m_auxDispImg2, 
                                    m_sriDeltas.min, 
                                    m_sriDeltas.max );
            
            CMinMaxFilter<float,float>::filterMinimum (  m_auxDispImg1,
                                                         m_predMinDispImg,
                                                         m_minMaxDispKernelSize.width,
                                                         m_minMaxDispKernelSize.height,
                                                         0.,
                                                         CMinMaxFilterBase::FM_IGNORE_INVALID );

            CMinMaxFilter<float,float>::filterMaximum (  m_auxDispImg2,
                                                         m_predMaxDispImg,
                                                         m_minMaxDispKernelSize.width,
                                                         m_minMaxDispKernelSize.height,
                                                         0.,
                                                         CMinMaxFilterBase::FM_IGNORE_INVALID );

            gettimeofday(&tv2, NULL);

            printf("Disparity Image Prediction Strategy 2: %lf milliseconds\n",
                   (((double)tv2.tv_usec) - ((double)tv1.tv_usec))/1000.);

            if ( 0 )
                computeDisparityReduction ( );

        }
    }
}

void
CRangePredictorOp::computeDisparityReduction ( )
{
    int h_i = m_predMinDispImg.getHeight();
    int w_i = m_predMinDispImg.getWidth();
    
    float *  min_p = (float *) m_predMinDispImg.getData();
    float *  max_p = (float *) m_predMaxDispImg.getData();

    long int dispRange1Sum_i = 0;
    long int dispRange2Sum_i = 0;
    long int pointCount_i    = 0;
    int      defaultDRange_i = 110;
    
    for (int i = 0; i < h_i; ++i)
    {
        for (int j = 0; j < w_i; ++j, ++max_p, ++min_p)
        {
            int dispRange_i = (int)(*max_p - *min_p + .5);
            
            if ( *min_p > 0 && *max_p > *min_p )
            {
                dispRange1Sum_i += std::min(dispRange_i, j);
            }
            else
                dispRange1Sum_i += std::min(defaultDRange_i, j);

            ++pointCount_i;
            dispRange2Sum_i += std::min(defaultDRange_i, j);
        }
    }

    printf( "pnts = %li, range1: %li range2: %li prop %lf\n",
            pointCount_i, dispRange1Sum_i, dispRange2Sum_i,
            dispRange1Sum_i/(double)dispRange2Sum_i);
}


void
CRangePredictorOp::predictDisparityImages ( const CSphericalRangeImage &f_sri, 
                                            CFloatImage                &fr_predMinDispImg,
                                            CFloatImage                &fr_predMaxDispImg,
                                            double                      f_deltaMin_d,
                                            double                      f_deltaMax_d )
{
    const CStereoCamera * camera_p  = 
        dynamic_cast<CStereoCamera *>(getInput ( "Rectified Camera" ) );

    int dImgW_i = fr_predMinDispImg.getWidth();
    int dImgH_i = fr_predMinDispImg.getHeight();

    if ( not camera_p ||
         dImgW_i != (int)fr_predMinDispImg.getWidth() || 
         dImgH_i != (int)fr_predMinDispImg.getHeight() )
        return;
    
    unsigned int w_ui = f_sri.getImage().getWidth();
    int h_i  = f_sri.getImage().getHeight();

    float *minDispImg_p = (float *) fr_predMinDispImg.getData();
    float *maxDispImg_p = (float *) fr_predMaxDispImg.getData();

    fr_predMinDispImg.clear();
    fr_predMaxDispImg.clear();
    
#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)
    {    
        C3DVector worldPoint;

        double u_d, v_d, d_d;
        int    u_i, v_i;
        double * ranges_p = f_sri.getImage().getScanline(i);

        for (unsigned int j = 0; j < w_ui; ++j, ++ranges_p )
        {
            double range_d = *ranges_p - f_deltaMin_d;

            if ( f_sri.image2World ( j, i, range_d, worldPoint ) )
            {
                camera_p -> world2Image ( worldPoint,
                                          u_d, v_d, d_d );
                
                u_i = (u_d+.5);
                v_i = (v_d+.5);
                
                float &dst_d = maxDispImg_p[v_i * dImgW_i + u_i];
                
                if ( u_i >=0        && 
                     u_i < dImgW_i  && 
                     v_i >=0        && 
                     v_i < dImgH_i  &&
                     d_d > 0        &&
                     u_i - d_d >= 0 &&
                     d_d > dst_d )
                    dst_d = d_d;
            }

            range_d = *ranges_p + f_deltaMax_d;
            
            if ( f_sri.image2World ( j, i, range_d, worldPoint ) )
            {
                camera_p -> world2Image ( worldPoint,
                                          u_d, v_d, d_d );
                
                u_i = (u_d+.5);
                v_i = (v_d+.5);
                
                float &dst_d = minDispImg_p[v_i * dImgW_i + u_i];
                
                if ( u_i >=0        && 
                     u_i < dImgW_i  && 
                     v_i >=0        && 
                     v_i < dImgH_i  &&
                     d_d > 0        &&
                     u_i - d_d >= 0 &&
                     d_d > dst_d )
                    dst_d = d_d;
            }
        }
    }
}

void
CRangePredictorOp::predictDisparityImage ( const CSphericalRangeImage &f_sri, 
                                           CFloatImage                &fr_predDispImg,
                                           double                      f_rangeOffset_d  )
{
    const CStereoCamera * camera_p  = 
        dynamic_cast<CStereoCamera *>(getInput ( "Rectified Camera" ) );

    if ( not camera_p )
        return;
    
    unsigned int w_ui = f_sri.getImage().getWidth();
    int h_i           = f_sri.getImage().getHeight();
    
    int dImgW_i = fr_predDispImg.getWidth();
    int dImgH_i = fr_predDispImg.getHeight();

    float *dispImg_p = (float *) fr_predDispImg.getData();
    fr_predDispImg.clear();
    
    const C3DMatrix rotation    = f_sri.getRotation();
    const C3DVector translation = f_sri.getTranslation();
    
    rotation.print();
    
#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)
    {    
        C3DVector worldPoint, localPoint;

        double u_d, v_d, d_d;
        int    u_i, v_i;
        
        for (unsigned int j = 0; j < w_ui; ++j)
        {
            if ( f_sri.image2Local ( j, i, localPoint ) )
            {
                localPoint.at(2) = std::max((localPoint.z() + f_rangeOffset_d), 0.);
                
                worldPoint = rotation * localPoint + translation;
                
                camera_p -> world2Image ( worldPoint,
                                          u_d, v_d, d_d );
                
                u_i = (u_d+.5);
                v_i = (v_d+.5);
                
                float &dst_d = dispImg_p[v_i * dImgW_i + u_i];
                
                if ( u_i >=0        && 
                     u_i < dImgW_i  && 
                     v_i >=0        && 
                     v_i < dImgH_i  &&
                     d_d > 0        &&
                     u_i - d_d >= 0 &&
                     d_d > dst_d )
                    dst_d = d_d;
            }
        }
    }
}

/// Show event.
bool CRangePredictorOp::show()
{
    const CVelodyne * velodyne_p = 
        dynamic_cast<CVelodyne*> ( getInput( "Velodyne Sensor" ) );
    CCamera * camera_p  = dynamic_cast<CCamera *>(getInput ( "Rectified Camera" ) );
    CUShortImage  * leftImg_p = dynamic_cast<CUShortImage  *>(getInput ( "Rectified Left Image" ) );

    if ( not camera_p || not leftImg_p || not velodyne_p)
        return false;    

    CDrawingList *  list1_p; //, * list2_p;

    CColorEncoding &colorEnc = getInputReference<CColorEncoding> ("SRI - Color encoding");

    float imgScaleFactor_f = getCastedInputObject<CIO_float, float> ("Rectification Scale Factor", 1.f );

    list1_p = getDrawingList ("Predicted Disp Image");
    list1_p -> clear();
    list1_p ->  addImage ( m_predDispImg, 0, 0, 800, 600, 1/(100*imgScaleFactor_f) );

    list1_p = getDrawingList ("Predicted Min Disp Image");
    list1_p -> clear();
    list1_p -> addImage ( m_predMinDispImg, 0, 0, 800, 600, 1/(100*imgScaleFactor_f) );

    list1_p = getDrawingList ("Predicted Max Disp Image");
    list1_p -> clear();
    list1_p -> addImage ( m_predMaxDispImg, 0, 0, 800, 600, 1/(100*imgScaleFactor_f) );

    list1_p = getDrawingList ("Predicted Right Disp Image");
    list1_p -> clear();
    list1_p ->  addImage ( m_predRightDispImg, 0, 0, 800, 600, 1/(100*imgScaleFactor_f) );

    list1_p = getDrawingList ("Predicted Min Right Disp Image");
    list1_p -> clear();
    list1_p -> addImage ( m_predMinRightDispImg, 0, 0, 800, 600, 1/(100*imgScaleFactor_f) );

    list1_p = getDrawingList ("Predicted Max Right Disp Image");
    list1_p -> clear();
    list1_p -> addImage ( m_predMaxRightDispImg, 0, 0, 800, 600, 1/(100*imgScaleFactor_f) );

    list1_p = getDrawingList ("Min Velodyne Image");
    list1_p -> clear();
    list1_p -> addColorEncImage ( &m_minRangeImage.getImage(), 0, 0, 800, 600, colorEnc );

    list1_p = getDrawingList ("Max Velodyne Image");
    list1_p -> clear();
    list1_p -> addColorEncImage ( &m_maxRangeImage.getImage(), 0, 0, 800, 600, colorEnc );

    list1_p = getDrawingList ("Remapped Rectified Right Image");
    list1_p -> clear();
    list1_p -> addImage ( m_remappedRImage, 0, 0, 800, 600 );

    list1_p = getDrawingList ("LUT Image");
    list1_p -> clear();

    if ( list1_p -> isVisible() )
    {
        const int sampleV_i = 15;
        const int sampleU_i = 15;
        
        list1_p -> setLineColor ( SRgb(255,255,255) );
        
        int w_i = m_lutU.getWidth();
        int h_i = m_lutU.getHeight();
        
        for (int i = 0; i < h_i; i+=sampleV_i)
        {
            float *  lutU_p = m_lutU.getScanline(i);
            float *  lutV_p = m_lutV.getScanline(i);

            for (int j = 0; j < w_i; j+=sampleU_i, lutU_p+=sampleU_i, ++lutV_p+=sampleU_i)
            {
                if ( *lutU_p >=0 && *lutU_p < w_i &&
                     *lutV_p >=0 && *lutV_p < h_i )
                    list1_p -> addLine ( j, i, *lutU_p, *lutV_p );
            }
        }
    }
                 
    return COperator::show();
}

void 
CRangePredictorOp::keyPressed ( CKeyEvent * f_event_p )
{
    return COperator::keyPressed ( f_event_p );    
}

void 
CRangePredictorOp::mouseMoved ( CMouseEvent * f_event_p )
{
    /*
      if ((f_event_p -> qtMouseEvent_p -> buttons() & Qt::LeftButton) != 0)
      {
      CDrawingList *  list_p;
      list_p = getDrawingList ("Colored Velodyne Image");
      S2D<int> drawPos1 = list_p -> getPosition ( );
      bool   visible1_b = list_p -> isVisible ( );

      list_p = getDrawingList ("Intensified Velodyne Image");
      S2D<int> drawPos2 = list_p -> getPosition ( );
      bool   visible2_b = list_p -> isVisible ( );

      const CVelodyne * velodyne_p = 
      dynamic_cast<CVelodyne*> ( getInput( "Velodyne Sensor" ) );
      CCamera * camera_p  = dynamic_cast<CCamera *>(getInput ( "Rectified Camera" ) );
      CUShortImage  * leftImg_p = dynamic_cast<CUShortImage  *>(getInput ( "Image Left" ) );
            
      if ( ( ( f_event_p->displayScreen == drawPos1 && visible1_b ) ||
      ( f_event_p->displayScreen == drawPos2 && visible2_b ) ) && 
      camera_p && leftImg_p && velodyne_p )
      {
      double *dist_p = (double *) m_rangeImage.getData();
            
      list_p = getDrawingList ("Velodyne Image - Left Image Overlay");            
      list_p -> clear();

      const double sw_f = 800.f;
      const double sh_f = 600.f;
      const int   w_i  = m_rangeImage.getWidth();
      const int   h_i  = m_rangeImage.getHeight();
            
            
      int u_i = (int)(f_event_p->posInScreen.x / sw_f * w_i);
      int v_i = (int)(f_event_p->posInScreen.y / sh_f * h_i);

      double range_d = dist_p[w_i * v_i + u_i];

      if (range_d > 0)
      {
      double azimuth_p[4], elevation_p[4];
                
      azimuth_p  [0] = (u_i-2) * m_scale.x + m_roiTL.x;
      elevation_p[0] = (v_i-1) * m_scale.y + m_roiTL.y;
      azimuth_p  [1] = (u_i+2) * m_scale.x + m_roiTL.x;
      elevation_p[1] = (v_i-1) * m_scale.y + m_roiTL.y;
      azimuth_p  [2] = (u_i+2) * m_scale.x + m_roiTL.x;
      elevation_p[2] = (v_i+1) * m_scale.y + m_roiTL.y;
      azimuth_p  [3] = (u_i-2) * m_scale.x + m_roiTL.x;
      elevation_p[3] = (v_i+1) * m_scale.y + m_roiTL.y;

      C3DVector  point[4];
      S2D<float> imgPos[4];

      float imgScaleFactor_f = getCastedInputObject<CIO_float, float> ("Rectification Scale Factor", 1.f );
      for (unsigned int i = 0; i < 4; ++i)
      {
      velodyne_p -> spherical2World ( range_d,
      azimuth_p[i],
      elevation_p[i],  
      point[i] );

      double aux1, aux2;

      camera_p -> world2Image ( point[i].x(),
      point[i].y(),
      point[i].z(),
      aux1, aux2);

      imgPos[i].x = aux1 / imgScaleFactor_f;
      imgPos[i].y = aux2 / imgScaleFactor_f;
      }

      list_p -> setLineColor ( SRgba(0,0,0,0) );
      list_p -> setFillColor ( SRgba(0,0,255,100) );

      list_p -> addFilledTriangle ( imgPos[0],
      imgPos[1],
      imgPos[2] );

      list_p -> addFilledTriangle ( imgPos[0],
      imgPos[2],
      imgPos[3] );
                    
      list_p -> setLineColor ( SRgba(255,255,255,255) );

      for (unsigned int i = 0; i < 4; ++i)
      list_p -> addLine ( imgPos[i], imgPos[(i+1)%4]);

      ////////////////////////// 
      list_p = getDrawingList ("Velodyne Image - Bird View Overlay");            
      list_p -> clear();
                
      S2D<float> *scale_p = dynamic_cast<S2D<float> *>
      (getInput ( "Velodyne Viewer - Scale" ) );
                
      S2D<int>   *dispSize_p = dynamic_cast<S2D<int> *>
      (getInput ( "Velodyne Viewer - Display Size" ) );
            
      if ( scale_p  && dispSize_p )
      {
      azimuth_p  [0] = (u_i) * m_scale.x + m_roiTL.x;
      elevation_p[0] = (v_i) * m_scale.y + m_roiTL.y;

      velodyne_p -> spherical2Local ( range_d,
      azimuth_p[0] + m_scale.x/2,
      elevation_p[0] + m_scale.y/2,
      point[0] );


      imgPos[0].x = ( (dispSize_p->width * leftImg_p->getWidth()/2) +
      point[0].x() * scale_p->x );

      imgPos[0].y = ( (dispSize_p->height * leftImg_p->getHeight()/2) -
      point[0].z() * scale_p->y );

                    
      list_p -> setFillColor ( SRgba(CColor::white, 80) );
      list_p -> setLineColor ( SRgba(CColor::white, 150) );
      list_p -> addFilledCircle ( imgPos[0], 3. * scale_p->x );

      list_p -> setFillColor ( SRgba(CColor::red, 80) );
      list_p -> setLineColor ( SRgba(CColor::white, 200) );
      list_p -> addFilledCircle ( imgPos[0], scale_p->x );
      }
                                            
                                            
      updateDisplay();
      }
      }
      }
    */
    return COperator::mouseMoved ( f_event_p );
}

void 
CRangePredictorOp::allocateSRIs()
{
    const CSphericalRangeImage * sri_p = 
        dynamic_cast<CSphericalRangeImage*> ( getInput( "SRI" ) );

    m_minRangeImage.copyFrom( *sri_p, true, true );
    m_maxRangeImage.copyFrom( *sri_p, true, true );
    
    //m_minRangeImage.ensureAllocation();

    //m_maxRangeImage =  *sri_p;
    //m_maxRangeImage.ensureAllocation();
}
