/*@@@**************************************************************************
* \file  velodyneBaseop
* \author Hernan Badino
* \date  Wed Apr  8 14:27:15 GMT 2009
* \notes 
*******************************************************************************
******************************************************************************/

/* INCLUDES */
#include "display.h"
#include "velodyneBase.h"
#include "logger.h"
#include "drawingList.h"
#include "stereoCamera.h"
#include "colorEncoding.h"
#include "velodyneStream.h"
#include "velodyne.h"
#include "uShortImage.h"
#include "medianFilter.h"

#include "3DMatrix.h"

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

//#include "ippDefs.h"
#include <sys/time.h>

using namespace VIC;

/// Constructors.
CVelodyneBaseOp::CVelodyneBaseOp ( COperator * const f_parent_p )
        : COperator (                   f_parent_p, "VelodyneBase" ),
          m_sri (                                                  ),
          m_repSri (                                               ),
          m_fillerOp (                                             ),
          m_fillHolesHor_b (                                  true ),
          m_fillHolesVert_b (                                 true ),
          m_colorEnc ( CColorEncoding::CET_HUE, S2D<float>(4.,30.) ),
          m_localVectorImg (                                       ),
          m_worldVectorImg (                                       ),
          m_leftCamVectorImg (                                     ),
          m_birdViewScale  (                              5.0, 5.0 ),
          m_birdViewOffset  (                             0.0, 0.0 ),
          m_birdViewDispSize (                                1, 1 )
{
    registerDrawingList ("Spherical Range Image",
                         S2D<int> (0, 2),
                         true );

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

    registerDrawingList ("SRI - 3D Points Bird-view",
                         S2D<int> (1, 1),
                         false );

    registerDrawingList ("Intensified SRI",
                         S2D<int> (1, 1),
                         true );

    /// Default value for sri.
    m_sri.setRoiTopLeft(S2D<double> (-0.35f, -0.08f));
    m_sri.setRoiSize(S2D<double> (0.7f,  .53f ));
    m_sri.setScale(S2D<double> ( 0.0019, 0.005 ));

    CParameterSet * sriSubset_p = m_sri.getParameterSet("Spherical Range Image");
    sriSubset_p -> setName ( "Spherical Range Image");
    m_paramSet_p -> addSubset ( sriSubset_p );

    /// Default value for repSri.
    m_repSri.setRoiTopLeft(S2D<double> (-0.35f, -0.28f));
    m_repSri.setRoiSize(S2D<double> (0.7f,  .53f ));
    m_repSri.setScale(S2D<double> ( 0.0019, 0.005 ));


    sriSubset_p = m_repSri.getParameterSet("Reprojected SRI");
    m_paramSet_p -> addSubset ( sriSubset_p );
    

      ADD_BOOL_PARAMETER( "Fill Horizontal Holes",
                          "Fill horizontal holes in the resulting SRI?",
                          m_fillHolesHor_b,
                          this,
                          FillHolesHor,
                          CVelodyneBaseOp );

      ADD_BOOL_PARAMETER( "Fill Vertical Lines",
                          "Fill empty vertical lines in the resulting SRI?",
                          m_fillHolesVert_b,
                          this,
                          FillHolesVert,
                          CVelodyneBaseOp );

      m_fillerOp.addParameters ( *m_paramSet_p );

    m_colorEnc.addParameters ( m_paramSet_p,
                               "SRI Color Encoding", 
                               "Color encoding for the SRI." );
    
    //m_paramSet_p -> addParameter ( 
    //        m_colorEnc.createParameter ("SRI Color Encoding", 
    //                                    "Color encoding for the SRI.") );


    BEGIN_PARAMETER_GROUP("Displays", false, CColor::red );

      CDrawingList *list_p;
      list_p = getDrawingList("Spherical Range Image");
      ADD_DISPSTATE_PARAMETER( list_p -> getName(),
                               "Spherical Range Image display.",
                               list_p -> getDisplayState(),
                               list_p,
                               DisplayState,
                               CDrawingList );
      
      list_p = getDrawingList("SRI - 3D Points Bird-view");
      ADD_DISPSTATE_PARAMETER( list_p -> getName(),
                               "Bird view of the 3D points obtained from the SRI.",
                               list_p -> getDisplayState(),
                               list_p,
                               DisplayState,
                               CDrawingList );

      list_p = getDrawingList("Intensified SRI");
      ADD_DISPSTATE_PARAMETER( list_p -> getName(),
                               "Encoded SRI where the grey value is taken from the "
                               "projection of the corresponding 3D point from the left image.",
                               list_p -> getDisplayState(),
                               list_p,
                               DisplayState,
                               CDrawingList );


      addDrawingListParameter ( "Reprojected Velodyne Image" );

    END_PARAMETER_GROUP;
}

/// Virtual destructor.
CVelodyneBaseOp::~CVelodyneBaseOp ()
{
    printf("~CVelodyneBaseOp called\n");
}

/// Init event.
bool CVelodyneBaseOp::initialize()
{
    printf("Registering SRI\n");
    
    registerOutput ( "SRI",                            &m_sri );
    registerOutput ( "Reprojected SRI",                &m_repSri );

    registerOutput ( "SRI - Sensor 3D Points",         &m_localVectorImg );
    registerOutput ( "SRI - World 3D Points",          &m_worldVectorImg );
    registerOutput ( "SRI - Left Camera Projections",  &m_leftCamVectorImg );
    registerOutput ( "SRI - Color encoding",           &m_colorEnc );

    return COperator::initialize();
}

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

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

/// Cycle event.
bool CVelodyneBaseOp::cycle()
{
    const CVelodyneDataStream * stream_p = 
        dynamic_cast<CVelodyneDataStream*> ( getInput( "Velodyne Data Stream" ) );

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

    /// Resize vector image if required.
    if ( m_localVectorImg.getWidth()  != m_sri.getImage().getWidth() || 
         m_localVectorImg.getHeight() != m_sri.getImage().getHeight() )
    {
        m_localVectorImg.freeMemory();
        m_localVectorImg.setWidth  ( m_sri.getImage().getWidth() );
        m_localVectorImg.setHeight ( m_sri.getImage().getHeight() );
        m_localVectorImg.ensureAllocation();

        m_worldVectorImg.freeMemory();
        m_worldVectorImg = m_localVectorImg;
        m_worldVectorImg.ensureAllocation();

        m_leftCamVectorImg.freeMemory();
        m_leftCamVectorImg = m_localVectorImg;
        m_leftCamVectorImg.ensureAllocation();
    }

    if ( velodyne_p )
        m_sri.setVelodyne ( *velodyne_p );

    /// Compute SRI
    m_sri.clear();
    
    if ( stream_p )
        m_sri.addMeasurements ( *stream_p );

    if (0)
    {
        /// TESTING 
        static int mode_si = 0;
        
        ++mode_si;
                
        for (int i = 0; i < m_sri.getImage().getHeight();++i)
        {
            for (int j = 0; j < m_sri.getImage().getWidth();++j)
            {
                C3DVector pt1;
                SSphericalPointData pt2;
            
                //mode_si=1;

                if (mode_si%3 == 0) // Spherical.
                {
                    m_sri.image2Local ( j, i, 1, pt1 );
                    pt1 *= (30/pt1.z());
                    m_sri.getVelodyne().local2Spherical ( pt1, pt2 );
                
                    m_sri.getImage().getScanline(i)[j] = pt2.range_d; // 30; // 
                }
                else if (mode_si%3 == 1) // Planar.
                {
                    m_sri.image2Local ( j, i, 1, pt1 );
                    pt1 *= (30/pt1.z());
                    m_sri.getVelodyne().local2Spherical ( pt1, pt2 );
                
                    m_sri.getImage().getScanline(i)[j] = 30; // 
                }
                else if (mode_si%3 == 2) // Planar.
                {
                    m_sri.image2Local ( j, i, 1, pt1 );
                    //m_sri.getVelodyne().local2Spherical ( pt1, pt2 );
                    m_sri.getImage().getScanline(i)[j] = pt1.z() * 0.2;

                }
            }
        }
    }
    
    
    if (m_fillHolesHor_b)
        m_fillerOp.fillHoles ( m_sri, m_fillHolesVert_b );

    /// Compute 3D points from SRI
    m_localVectorImg.clear();
    m_sri.image2Local ( m_localVectorImg );

    /// Compute point in local coord system to world and left cam img.
    computePtsInWorldAndImageCoordSystem ( );

    if ( velodyne_p )
        m_repSri.setVelodyne ( *velodyne_p );
    
    struct timeval tv1, tv2;
    gettimeofday(&tv1, NULL);
    reprojectRangeImage();
    gettimeofday(&tv2, NULL);

    printf("Reprojection of Spherical Range Image: %lf milliseconds\n",
           (((double)tv2.tv_usec) - ((double)tv1.tv_usec))/1000.);


    if (m_fillHolesHor_b)
        m_fillerOp.fillHoles ( m_repSri, m_fillHolesVert_b );

    bool m_applyMF_b = true;
    if (m_applyMF_b)
    {
        if (m_auxImg.getSize() != m_repSri.getImage().getSize())
        {
            m_auxImg.freeMemory();
            m_auxImg.setSize ( m_repSri.getImage().getSize() );
            m_auxImg.ensureAllocation();
        }

        CMedianFilter<double,double>::compute5x5(  m_repSri.getImage(),   
                                                   m_auxImg );
        m_auxImg.copyImageContentTo ( m_repSri.getImage());
    }
    

    registerOutput ( "SRI",                            &m_sri );
    registerOutput ( "Reprojected SRI",                &m_repSri );

    registerOutput ( "SRI - Sensor 3D Points",         &m_localVectorImg );
    registerOutput ( "SRI - World 3D Points",          &m_worldVectorImg );
    registerOutput ( "SRI - Left Camera Projections",  &m_leftCamVectorImg );
    registerOutput ( "SRI - Color encoding",           &m_colorEnc );

    return COperator::cycle();
}


/// 
bool
CVelodyneBaseOp::reprojectRangeImage()
{
    const CVelodyne * velodyne_p = 
        dynamic_cast<CVelodyne*> ( getInput( "Velodyne Sensor" ) );

    int h_i = (int) m_worldVectorImg.getHeight();
    int w_i = (int) m_worldVectorImg.getWidth();
    
    int dstW_i = m_repSri.getImage().getWidth();
    int dstH_i = m_repSri.getImage().getHeight();

#if defined ( _OPENMP )
    const unsigned int numThreads_ui = std::min(omp_get_max_threads(), 32);
    CSphericalDataVector vecDst_p[32];

    for (unsigned int i = 0 ; i < numThreads_ui; ++i)
        vecDst_p[i].reserve ( dstH_i * dstW_i );
#pragma omp parallel for num_threads(numThreads_ui) schedule(dynamic)
#else
    CSphericalDataVector vecDst;
    vecDst.reserve ( dstH_i * dstW_i );
#endif
    for (int i = 0 ; i < h_i; ++i)
    {
#if defined ( _OPENMP )
        const unsigned int threadNum_ui = omp_get_thread_num();
        CSphericalDataVector &vecDst = vecDst_p[threadNum_ui];
#endif

        C3DVector *vec_p = m_worldVectorImg.getScanline(i);
        SSphericalPointData sphMeas;
        
        for (int j = 0; j < w_i; ++j, ++vec_p)
        {
            velodyne_p -> local2Spherical ( *vec_p,
                                            sphMeas );
            vecDst.push_back ( sphMeas );
        }
        
    }

#if defined ( _OPENMP )
    CSphericalDataVector &vecDst = vecDst_p[0];
    for (unsigned int i = 1 ; i < numThreads_ui; ++i)
        vecDst.insert ( vecDst.end(),
                        vecDst_p[i].begin(),
                        vecDst_p[i].end() );
#endif

    m_repSri.getImage().clear();
    
    m_repSri.addMeasurements ( vecDst );

    return true;
}

/// Show event.
bool
CVelodyneBaseOp::show()
{
    CDrawingList *  list1_p;
    
    list1_p = getDrawingList ("Spherical Range Image");
    list1_p -> clear();

    if ( list1_p -> isVisible())
        list1_p -> addColorEncImage ( &m_sri.getImage(),
                                      0, 0, 
                                      800, 600, 
                                      m_colorEnc, 
                                      1.f,
                                      false );

    list1_p = getDrawingList ("Reprojected Velodyne Image");
    list1_p -> clear();

    if ( list1_p -> isVisible())
        list1_p -> addColorEncImage ( &m_repSri.getImage(), 
                                      0, 0, 
                                      800, 600, 
                                      m_colorEnc,
                                      1.f, 
                                      false );

    showIntensifiedSRI();

    showBirdView();

    return COperator::show();
}

void
CVelodyneBaseOp::showIntensifiedSRI()
{
    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 )
        return;    

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

    CDrawingList *  list_p = getDrawingList ("Intensified SRI");
    list_p -> clear();

    if ( list_p -> isVisible() )

    {
        unsigned short int * limg_p = (unsigned short int *) leftImg_p -> getData();
   
        const double sw_f = 800;
        const double sh_f = 600;
    
        const unsigned int w_ui = m_sri.getImage().getWidth();
        const int          h_i  = m_sri.getImage().getHeight();

        const int greyValueScale_i = 256;

#if defined ( _OPENMP )
        CDrawingList *  lists_p[32];

        lists_p[0] = list_p;

        const int numThreads_i = std::min(omp_get_max_threads(), 32);

        for (int i = 1; i < numThreads_i; ++i)
            lists_p[i] = new CDrawingList ("");

#pragma omp parallel for num_threads(numThreads_i) schedule(dynamic)
#endif
        for ( int i = 0; i < h_i; ++i)
        {
#ifdef _OPENMP
            CDrawingList * addList_p = lists_p[omp_get_thread_num()];
            CColorEncoding colorEnc  = m_colorEnc;
#else
            CDrawingList * addList_p = list_p;
            CColorEncoding &colorEnc  = m_colorEnc;
#endif
            const C3DVector *vimg_p = m_worldVectorImg.getScanline(i);
            SRgb  color;

            for (unsigned int j = 0; j < w_ui; ++j, ++vimg_p)
            {
                if (vimg_p->z() > 0)
                {
                    double u_f, v_f;
                    
                    camera_p -> world2Image ( *vimg_p,
                                              u_f, v_f );
                    
                    int u_i = u_f + .5;
                    int v_i = v_f + .5;
                    
                    if ( u_i >= 0 && v_i >= 0 && 
                         u_i < (int)leftImg_p->getWidth() && 
                         v_i < (int)leftImg_p->getHeight() )
                    {
                        uint8_t intensity_ui = limg_p[leftImg_p->getWidth() * v_i + u_i]/greyValueScale_i;
                        color.set ( intensity_ui, intensity_ui, intensity_ui);
                    }
                    else
                        /// Encode range.
                        colorEnc.colorFromValue ( vimg_p->z(),
                                                  color );

                    addList_p -> setLineColor ( SRgba( color, 255) );
                    addList_p -> setFillColor ( SRgba( color, 255) );
                    addList_p -> addFilledRectangle ( j*sw_f/w_ui,       i*sh_f/h_i,
                                                      (j+1.f)*sw_f/w_ui, (i+1.f)*sh_f/h_i);       
                }            
            }       
        }
        /// Once iteration done add all list to the one which is going to
        /// be displayed.
#if defined ( _OPENMP )
        for (int i = 1; i < numThreads_i; ++i)
        {
            lists_p[0] -> addDrawingList ( *lists_p[i] );
            delete lists_p[i];
        }
#endif
    }
}


void
CVelodyneBaseOp::showBirdView ()
{
    CDrawingList *  list1_p;

    list1_p = getDrawingList ("SRI - 3D Points Bird-view");
    list1_p -> clear();

    if ( !list1_p -> isVisible() )
        return;

    const int m_encodeMode_e = 2;

    const float dispWidth_f  = 800 * m_birdViewDispSize.x;
    const float dispHeight_f = 600 * m_birdViewDispSize.y;
    
    const unsigned int size_ui = ( m_localVectorImg.getWidth() * 
                                   m_localVectorImg.getHeight() );

    const C3DVector * img_p = m_localVectorImg.getData();

    for (unsigned int i = 0; i < size_ui; ++i, ++img_p )
    {
        C3DVector localPoint = *img_p;
        SRgb      colorLine;
       
        m_colorEnc.colorFromValue ( localPoint.at((int)m_encodeMode_e), 
                                    colorLine );
        
        localPoint.at(0) += m_birdViewOffset.x;
        localPoint.at(2) += m_birdViewOffset.y;

        float u_f = localPoint.x() * m_birdViewScale.x + (dispWidth_f)/2;
        float v_f = (dispHeight_f)/2 - localPoint.z() * m_birdViewScale.y;
        
        if ( u_f > 0 && u_f < dispWidth_f &&
             v_f > 0 && v_f < dispHeight_f )
        {
            list1_p -> setLineColor ( SRgba( colorLine, 255 ) );
            list1_p -> setFillColor ( SRgba( colorLine, 20) );
            list1_p -> addFilledRectangle ( u_f-.5, v_f-.5, u_f + .5, v_f + .5);
        }
    }
}


void 
CVelodyneBaseOp::keyPressed ( CKeyEvent * f_event_p )
{
    if ( f_event_p -> qtKeyEvent_p -> key() == Qt::Key_R ) 
    {
        m_birdViewScale   = S2D<double>(5.f,5.f);
        m_birdViewOffset  = S2D<double>(0.f,0.f);

        showBirdView();
        updateDisplay();
    }

}

void 
CVelodyneBaseOp::mouseMoved ( CMouseEvent * f_event_p )
{
    static S2D<int> prevPos;

    CDrawingList *  list1_p =  getDrawingList ("SRI - 3D Points Bird-view");

    if ( f_event_p -> displayScreen == list1_p->getPosition() &&
         list1_p -> isVisible() )
    {
        if ((f_event_p -> qtMouseEvent_p -> buttons() & Qt::LeftButton) != 0)
        {
            if (f_event_p -> qtMouseEvent_p -> modifiers() & Qt::ShiftModifier )
            {
                float deltaX_f = (f_event_p -> qtMouseEvent_p -> x() - prevPos.x);
                float deltaY_f = (f_event_p -> qtMouseEvent_p -> y() - prevPos.y);
                
                if (f_event_p -> qtMouseEvent_p -> modifiers() & Qt::AltModifier )
                {
                    m_birdViewOffset.x += deltaX_f/m_birdViewScale.x;
                    m_birdViewOffset.y -= deltaY_f/m_birdViewScale.y;
                }
                else
                    if (prevPos.isValid())
                    {
                        float motion_f = (deltaY_f + deltaX_f)/10.;
                        m_birdViewScale.x = std::max( 1., motion_f + m_birdViewScale.x);
                        m_birdViewScale.y = m_birdViewScale.x;
                    }
                showBirdView();
                updateDisplay();
            }
        }
    }
    
    list1_p =  getDrawingList ("Spherical Range Image");
    
    int dispWidth_i  = 800;
    int dispHeight_i = 600;
    
    if ( f_event_p -> displayScreen == list1_p->getPosition() &&
        list1_p -> isVisible() )
    {
        const int w_ui = m_sri.getImage().getWidth();
        const int h_ui = m_sri.getImage().getHeight();

        int x_i = (int)(f_event_p -> posInScreen.x / dispWidth_i  * w_ui + .5);
        int y_i = (int)(f_event_p -> posInScreen.y / dispHeight_i * h_ui + .5);
        
        if ( x_i >=0 && x_i < w_ui && 
             y_i >=0 && y_i < h_ui )
        {
            printf("(v,u) (%i, %i) El: %f Az: %f Range: %f\n",
                   y_i, x_i, 
                   m_sri.row2Elevation ( y_i ),
                   m_sri.column2Azimuth ( x_i ),
                   m_sri.getImage().getScanline(y_i)[x_i] );
        }
    }

    /*
        else
        {
            //const CUShortImage *        img_p  = dynamic_cast<CUShortImage  *>(getInput ( "Rectified Left Image" ) );
            const CUShortImage *         disp_p = dynamic_cast<CUShortImage  *>(getInput ( "Disparity Image" ) );
            const CStereoCamera *  cam_p  = dynamic_cast<CStereoCamera *>(getInput ( "Camera" ) );
            const CStereoCamera *  rcam_p = dynamic_cast<CStereoCamera *>(getInput ( "Rectified Camera" ) );
            const CVelodyne * velodyne_p  = dynamic_cast<CVelodyne*> ( getInput( "Velodyne Sensor" ) );

            int dispWidth_i  = 800;
            int dispHeight_i = 600;
        
            CDrawingList *  list_p;
            list_p = getDrawingList ("Velodyne Bird View");

            if ( list_p -> isVisible ( ) )
            {
                S2D<int> drawPos = list_p -> getPosition ( );
        
                if ( f_event_p->displayScreen.x >= drawPos.x  && 
                     f_event_p->displayScreen.x < drawPos.x + m_birdViewDispSize.x &&
                     f_event_p->displayScreen.y >= drawPos.y  && 
                     f_event_p->displayScreen.y < drawPos.y + m_birdViewDispSize.y )
                {
                    double x_f = ( (f_event_p->posInDisplay.x - (drawPos.x * dispWidth_i)) -
                                   (dispWidth_i * m_birdViewDispSize.x)/2 ) / m_birdViewScale.x - m_birdViewOffset.x;
            
                    double z_f = -( (f_event_p->posInDisplay.y - (drawPos.y * dispHeight_i)) -
                                    (dispHeight_i*m_birdViewDispSize.y)/2 ) / m_birdViewScale.y  - m_birdViewOffset.y;
            
                    double y_f = -1.55;

                    //double u_f, v_f;
            
                    double u_p[8];
                    double v_p[8];
                    int idx_i = 0;
            
                    //cam_p -> world2Image ( x_f, y_f, z_f, u_f, v_f );

                    if ( z_f > 0 ) //&& u_f > 0 && v_f > 0 && u_f <= img_p->getWidth() && v_f <= img_p->getHeight())
                    {
                        double deltaHeight_f = -.75;
                        double deltaX_f = 0.2;
                        double deltaZ_f = 0.2;
                    
                        C3DVector point, worldPoint;
                        for (; deltaHeight_f <= .75; deltaHeight_f+=1.5)
                        {
                            point.set ( x_f-deltaX_f,
                                        y_f+deltaHeight_f,
                                        z_f-deltaZ_f );
                        
                            velodyne_p -> local2World ( point, worldPoint );
                            cam_p -> world2Image ( worldPoint.x(), worldPoint.y(), worldPoint.z(), u_p[idx_i], v_p[idx_i]);
                            ++idx_i;
                      
                            point.set ( x_f-deltaX_f,
                                        y_f+deltaHeight_f,
                                        z_f+deltaZ_f );

                            velodyne_p -> local2World ( point, worldPoint );
                            cam_p -> world2Image ( worldPoint.x(), worldPoint.y(), worldPoint.z(),u_p[idx_i], v_p[idx_i]);
                            ++idx_i;
                        
                            point.set ( x_f+deltaX_f,
                                        y_f+deltaHeight_f,
                                        z_f+deltaZ_f );

                            velodyne_p -> local2World ( point, worldPoint );
                            cam_p -> world2Image ( worldPoint.x(), worldPoint.y(), worldPoint.z(),u_p[idx_i], v_p[idx_i]);
                            ++idx_i;

                            point.set ( x_f+deltaX_f,
                                        y_f+deltaHeight_f,
                                        z_f-deltaZ_f );

                            velodyne_p -> local2World ( point, worldPoint );
                            cam_p -> world2Image ( worldPoint.x(), worldPoint.y(), worldPoint.z(),u_p[idx_i], v_p[idx_i]);
                            ++idx_i;                
                        }

                        list_p =  getDrawingList ("Image Position Overlay" );
                        list_p -> clear();

                        deltaHeight_f = 0.75;

                        float *d_p = (float *) disp_p ->getData();

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

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

                        for (unsigned int i = 0; i < disp_p -> getHeight(); ++i)
                        {
                            for (unsigned int j = 0; j < disp_p -> getWidth(); ++j, ++d_p)
                            {
                                if (*d_p > 0)
                                {
                                    rcam_p -> image2World ( j, i, *d_p, 
                                                            worldPoint.at(0), 
                                                            worldPoint.at(1), 
                                                            worldPoint.at(2) );

                                    velodyne_p -> world2Local ( worldPoint, point );

                                    //printf("%i %i %f %f (%f %f) %f (%f %f) %f (%f %f)\n", j, i, *d_p, tx, x_f-deltaX_f, x_f+deltaX_f, ty, y_f-deltaHeight_f, y_f+deltaHeight_f, 
                                    //       tz, z_f-deltaZ_f, z_f+deltaZ_f );

                                    if ( point.x() > x_f-deltaX_f && 
                                         point.x() < x_f+deltaX_f && 
                                         point.y() > y_f-deltaHeight_f && 
                                         point.y() < y_f+deltaHeight_f && 
                                         point.z() > z_f-deltaZ_f && 
                                         point.z() < z_f+deltaZ_f )
                                    {
                                        //printf("j = %i i = %i y: %f ref: %f\n", j, i, ty, y_f);
                                        list_p -> addRectangle ( (j-.5)/scaleFactor_f, (i-.5)/scaleFactor_f,
                                                                 (j+.5)/scaleFactor_f, (i+.5)/scaleFactor_f );
                                    }
                                }
                            }
                        }
                
                        for (int i = 0; i < 4; ++i)
                        {
                            list_p -> setLineColor ( CColor::red  );
                            list_p -> addLine ( u_p[i], v_p[i], u_p[(i+1)%4], v_p[(i+1)%4] );
                            list_p -> setLineColor (  CColor::blue );
                            list_p -> addLine ( u_p[i+4], v_p[i+4], u_p[(i+1)%4+4], v_p[(i+1)%4+4] );
                            list_p -> setLineColor ( CColor::green );
                            list_p -> addLine ( u_p[i], v_p[i], u_p[(i+4)], v_p[(i+4)] );
                        }
                
                    }
                
                    updateDisplay();
                }
                else
                {
                }
            }
        }
    }
    */

    prevPos.x = f_event_p -> qtMouseEvent_p -> x();
    prevPos.y = f_event_p -> qtMouseEvent_p -> y();

    return COperator::mouseMoved ( f_event_p );
}

void
CVelodyneBaseOp::computePtsInWorldAndImageCoordSystem ( )
{
    const CVelodyne     velodyne = getInputReference<CVelodyne> ("Velodyne Sensor");
    const CStereoCamera camera   = getInputReference<CStereoCamera> ("Rectified Camera");
    
    C3DMatrix rotation    = velodyne.getRotation();
    C3DVector translation = velodyne.getTranslation();

    int h_i           = m_localVectorImg.getHeight();
    unsigned int w_ui = m_localVectorImg.getWidth();    

    m_worldVectorImg.clear();
    m_leftCamVectorImg.clear();

#if defined ( _OPENMP )
    const int numThreads_i = omp_get_max_threads();
#pragma omp parallel for num_threads(numThreads_i) schedule(static)
#endif
    for (int i = 0; i < h_i; ++i )
    {
        const C3DVector * localImg_p = m_localVectorImg.getScanline(i);
        C3DVector * worldImg_p       = m_worldVectorImg.getScanline(i);
        C3DVector * leftCamImg_p     = m_leftCamVectorImg.getScanline(i);

        for ( unsigned j = 0; j < w_ui; ++j, ++localImg_p, ++worldImg_p, ++leftCamImg_p )
        {            
            C3DVector projPoint;
            *worldImg_p = rotation * *localImg_p + translation;
            
            if ( camera.world2Image ( *worldImg_p,
                                      projPoint ) )
                
                *leftCamImg_p = projPoint;
        }
    }
}
