/*@@@**************************************************************************
 * \file  sphericalRangeImage
 * \author Hernan Badino
 * \date  Wed Jul 29 11:13:39 EDT 2009
 * \notes 
 *******************************************************************************
 *****          (C) COPYRIGHT Hernan Badino - All Rights Reserved          *****
 ******************************************************************************/

/* INCLUDES */
#include "dbl2DParam.h"
#include "paramBaseConnector.h"
#include "sphericalRangeImage.h"
#include "parameterSet.h"

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

using namespace VIC;

/// Constructors/Destructor
CSphericalRangeImage::CSphericalRangeImage ( )
        : m_lut2Local (                 ),
          m_lut2World (                 ),
          m_image (                     ),
          m_interpMode_e (        IM_NN ),
          m_optMode_e (        OM_SPEED ),
          m_velodyne (                  ),
          m_roiTopLeft (         0., 0. ),
          m_roiSize (            0., 0. ),
          m_scale (              0., 0. ),
          m_luvsOwner_b (          true ),
          m_luvCosAz_p (           NULL ),
          m_luvSinAz_p (           NULL ),
          m_luvCosEl_p (           NULL ),
          m_luvSinEl_p (           NULL ),
          m_paramSet_p (           NULL )
{
    m_rotation.loadIdentity();
    m_translation.clear();
}

CSphericalRangeImage::~CSphericalRangeImage ( )
{
    freeLutsAndImage();
}


CSphericalRangeImage & 
CSphericalRangeImage::operator = ( const CSphericalRangeImage &f_other )
{
    copyFrom ( f_other, false, true );
    return *this;
}


inline bool
CSphericalRangeImage::copyFrom ( const CSphericalRangeImage &f_other,
                                 bool f_allocateImage_b /* = false */,
                                 bool f_reuseLuts_b /* = true */)
{
    bool success_b;
    
    freeLutsAndImage();
    
    success_b = m_image.copyFrom ( f_other.m_image, f_allocateImage_b );

    m_interpMode_e = f_other.m_interpMode_e;
    m_optMode_e    = f_other.m_optMode_e;
    m_velodyne     = f_other.m_velodyne;
    m_rotation     = f_other.m_rotation;
    m_translation  = f_other.m_translation;
    m_roiTopLeft   = f_other.m_roiTopLeft;
    m_roiSize      = f_other.m_roiSize;
    m_scale        = f_other.m_scale;

    if ( not m_lut2Local.copyFrom ( f_other.m_lut2Local, not f_reuseLuts_b ) )
        success_b = false;

    if ( not m_lut2World.copyFrom ( f_other.m_lut2World, not f_reuseLuts_b ) )
        success_b = false;
    
    
    if  ( m_optMode_e == OM_MEMORY )
    {
        if ( f_reuseLuts_b )
        {
            m_luvCosAz_p = f_other.m_luvCosAz_p;
            m_luvSinAz_p = f_other.m_luvSinAz_p;
            m_luvCosEl_p = f_other.m_luvCosEl_p;
            m_luvSinEl_p = f_other.m_luvSinEl_p;
            m_luvsOwner_b = false;
        }
        else
        {
            m_luvsOwner_b = true;
            m_luvCosAz_p = new double[m_image.getWidth()];
            m_luvSinAz_p = new double[m_image.getWidth()];
            
            m_luvCosEl_p = new double[m_image.getHeight()];
            m_luvSinEl_p = new double[m_image.getHeight()];
        }
    }

    return success_b;
}


inline void
CSphericalRangeImage::allocateLutsAndImage()
{
    /// freeLutsAndImage must be called before calling this method.
    freeLutsAndImage();
    
    m_image.setSize (  m_roiSize.width  / m_scale.x + 0.5,
                       m_roiSize.height / m_scale.y + 0.5 );
    m_image.ensureAllocation ( );
    m_image.clear();

    if ( m_optMode_e == OM_SPEED )
    {
        m_lut2Local.setSize ( m_image.getWidth(), m_image.getHeight() );
        m_lut2Local.ensureAllocation ( );
        m_lut2Local.clear();

        m_lut2World.setSize ( m_image.getWidth(), m_image.getHeight() );
        m_lut2World.ensureAllocation ( );
        m_lut2World.clear();
    }
    else
    {
        m_luvsOwner_b = true;
        m_luvCosAz_p = new double[m_image.getWidth()];
        m_luvSinAz_p = new double[m_image.getWidth()];

        m_luvCosEl_p = new double[m_image.getHeight()];
        m_luvSinEl_p = new double[m_image.getHeight()];
    }
    
}

inline void
CSphericalRangeImage::freeLutsAndImage()
{
    m_image.freeMemory();
    m_lut2Local.freeMemory();
    m_lut2World.freeMemory();

    if ( m_luvsOwner_b )
    {
        if (m_luvCosAz_p)
        {
            delete [] m_luvCosAz_p;
        }
        
        if (m_luvSinAz_p)
        {
            delete [] m_luvSinAz_p;
        }
        
        if (m_luvCosEl_p)
        {
            delete [] m_luvCosEl_p;
        }
        
        if (m_luvSinEl_p)
        {
            delete [] m_luvSinEl_p;
        }
    }

    m_luvCosAz_p = NULL;
    m_luvSinAz_p = NULL;
    m_luvCosEl_p = NULL;
    m_luvSinEl_p = NULL;

}




/// Add a measurement.
inline bool
CSphericalRangeImage::addMeasurement ( double f_range_d,
                                       double f_azimuth_d,
                                       double f_elevation_d )
{
    const int w_i = m_image.getWidth();
    const int h_i = m_image.getHeight();
    
    double u_d = azimuth2Column ( f_azimuth_d );
    double v_d = elevation2Row  ( f_elevation_d );

    int    u_i = (u_d + .5);
    int    v_i = (v_d + .5);

    if ( !(u_i >= 0 && u_i < w_i && 
           v_i >= 0 && v_i < h_i ) )
        return false;

    double *data_p = m_image.getData();
    
    data_p[v_i * w_i + u_i] = f_range_d;

    return true;
}

inline bool
CSphericalRangeImage::addMeasurement ( SSphericalPointData f_meas )
{
    return addMeasurement ( f_meas.range_d,
                            f_meas.azimuth_d,
                            f_meas.elevation_d );
}

bool
CSphericalRangeImage::addMeasurements ( const CSphericalDataVector &f_vector )
{
    const int w_i = m_image.getWidth();
    const int h_i = m_image.getHeight();
    const int size_i = (int)f_vector.size();
    
    double * const data_p = m_image.getData();
    
#if 0 && 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 < size_i; ++i)
    {
        double u_d = azimuth2Column ( f_vector[i].azimuth_d );
        double v_d = elevation2Row  ( f_vector[i].elevation_d );
    
        int    u_i = (u_d + .5);
        int    v_i = (v_d + .5);
        
        if ( u_i >= 0 && u_i < w_i && 
             v_i >= 0 && v_i < h_i )
        {
            double &res_d = data_p[v_i * w_i + u_i];

            if ( !res_d ||
                 res_d > f_vector[i].range_d)
                res_d = f_vector[i].range_d;
        }
    }

    return true;
}

bool
CSphericalRangeImage::addMeasurements ( const CVelodyneDataStream &f_vector )
{
    const int w_i = m_image.getWidth();
    const int h_i = m_image.getHeight();

    const int size_i = (int)f_vector.size();
    
    double * const data_p = m_image.getData();
    
#if 0 && 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 < size_i; ++i)
    {
        double u_d = azimuth2Column ( f_vector[i].azimuth_d );
        double v_d = elevation2Row  ( f_vector[i].elevation_d );
        
        int    u_i = (u_d + .5);
        int    v_i = (v_d + .5);
        
        if ( u_i >= 0 && u_i < w_i && 
             v_i >= 0 && v_i < h_i )
        {
            double &res_d = data_p[v_i * w_i + u_i];

            if ( !res_d ||
                 res_d > f_vector[i].range_d)
                res_d = f_vector[i].range_d;
        }
    }

    return true;
}

inline void
CSphericalRangeImage::clear()
{
    m_image.clear();
}


/// Convert image to spherical coordinates.
inline bool
CSphericalRangeImage::image2Spherical ( double  f_u_d,
                                        double  f_v_d,
                                        double &fr_azimuth_d,
                                       double &fr_elevation_d ) const
{
    fr_azimuth_d   = column2Azimuth ( f_u_d );
    fr_elevation_d = row2Elevation  ( f_v_d );    
    
    return true;    
}

/// Convert image to spherical coordinates.
inline bool
CSphericalRangeImage::image2Spherical ( double  f_u_d,
                                        double  f_v_d,
                                        double &fr_range_d,
                                        double &fr_azimuth_d,
                                        double &fr_elevation_d ) const
{
    const int w_i = m_image.getWidth();
    const int h_i = m_image.getHeight();
    
    double *data_p = m_image.getData();

    image2Spherical ( f_u_d,
                      f_v_d,
                      fr_azimuth_d,
                      fr_elevation_d );

    int u_i = (int)(f_u_d + .5);
    int v_i = (int)(f_v_d + .5);

    if ( !(u_i >= 0 && u_i < w_i && 
           v_i >= 0 && v_i < h_i ) )
        return false;

    fr_range_d = data_p[v_i * w_i + u_i];

    return true;
}


/// Convert image to spherical coordinates.
inline bool
CSphericalRangeImage::image2Spherical ( double               f_u_d,
                                        double               f_v_d,
                                        SSphericalPointData &fr_meas ) const
{
    return image2Spherical (  f_u_d, 
                              f_v_d, 
                              fr_meas.range_d, 
                              fr_meas.azimuth_d, 
                              fr_meas.elevation_d );
}

/// Convert spherical coordinates to image.
inline bool
CSphericalRangeImage::spherical2Image ( double  f_azimuth_d,
                                        double  f_elevation_d,
                                        double &fr_u_d,
                                        double &fr_v_d ) const
{
    fr_u_d = azimuth2Column ( f_azimuth_d   );
    fr_v_d = elevation2Row  ( f_elevation_d );

    return true;    
}

/// Convert image to 3D point in local coordinate system.
inline bool
CSphericalRangeImage::image2Local ( int         f_u_i,
                                    int         f_v_i,
                                    C3DVector  &fr_point ) const
{
    const int &w_i = m_image.getWidth();
    const int &h_i = m_image.getHeight();
    
    if ( !(f_u_i >= 0 && f_u_i < w_i && 
           f_v_i >= 0 && f_v_i < h_i ) )
        return false;

    const double &range_d = m_image.getScanline(f_v_i)[f_u_i];

    if ( m_optMode_e == OM_MEMORY )
    {
        const double cosElTimesRange_d = m_luvCosEl_p[f_v_i] * range_d;
        
        fr_point.at(0) = cosElTimesRange_d * m_luvSinAz_p [ f_u_i ];
        fr_point.at(1) = -range_d          * m_luvSinEl_p [ f_v_i ];
        fr_point.at(2) = cosElTimesRange_d * m_luvCosAz_p [ f_u_i ];
    }
    else
    {
        fr_point = m_lut2Local.getScanline ( f_v_i )[f_u_i] * range_d;
    }
    
    return true;
}

/// Convert image to 3D point in local coordinate system.
inline bool
CSphericalRangeImage::image2Local ( int     f_u_i,
                                    int     f_v_i,
                                    double &fr_x_d,
                                    double &fr_y_d,
                                    double &fr_z_d ) const
{
    C3DVector vec;
    
    if ( ! image2Local ( f_u_i,
                         f_v_i,
                         vec ) )
        return false;
    
    fr_x_d = vec.x();
    fr_y_d = vec.y();
    fr_z_d = vec.z();

    return true;
}


/// Convert image to 3D point in world coordinate system.
inline bool
CSphericalRangeImage::image2World ( int        f_u_i,
                                    int        f_v_i,
                                    C3DVector &fr_point ) const
{
    const int &w_i = m_image.getWidth();
    const int &h_i = m_image.getHeight();
    
    if ( !(f_u_i >= 0 && f_u_i < w_i && 
           f_v_i >= 0 && f_v_i < h_i ) )
        return false;

    const double &range_d = m_image.getScanline(f_v_i)[f_u_i];

    if ( m_optMode_e == OM_MEMORY )
    {
        const double cosElTimesRange_d = m_luvCosEl_p[f_v_i] * range_d;
        
        fr_point.at(0) = cosElTimesRange_d * m_luvSinAz_p [ f_u_i ];
        fr_point.at(1) = -range_d          * m_luvSinEl_p [ f_v_i ];
        fr_point.at(2) = cosElTimesRange_d * m_luvCosAz_p [ f_u_i ];

        fr_point = m_rotation * fr_point + m_translation;
    }
    else
    {
        fr_point = (m_lut2Local.getScanline ( f_v_i )[f_u_i] * range_d) + m_translation;
    }
    
    return true;
}

/// Convert image to 3D point in local coordinate system.
inline bool
CSphericalRangeImage::image2World ( int     f_u_i,
                                    int     f_v_i,
                                    double &fr_x_d,
                                    double &fr_y_d,
                                    double &fr_z_d ) const
{
    C3DVector point;
    
    if ( !image2World ( f_u_i,
                        f_v_i,
                        point ) )
         return false;
    
    fr_x_d = point.x();
    fr_y_d = point.y();
    fr_z_d = point.z();

    return true;
}


/// Convert the whole image to 3D points in a vector.
inline bool
CSphericalRangeImage::image2Local ( C3DPointDataVector &fr_vector ) const
{
    const int h_i = m_image.getHeight();
    const int w_i = m_image.getWidth();
        
    if ( m_optMode_e == OM_MEMORY )
    {
#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 )
        {
            const double &cosEl_d = m_luvCosEl_p[i];
            const double &sinEl_d = m_luvSinEl_p[i];

            const double * imgScanline_p = m_image.getScanline(i);
            const double * cosAz_p       = m_luvCosAz_p;
            const double * sinAz_p       = m_luvSinAz_p;

            for ( int j = 0 ; j < w_i; ++j, ++imgScanline_p, ++cosAz_p, ++sinAz_p )
            {
                const double cosElTimesRange_d = cosEl_d * (*imgScanline_p);

                C3DVector point ( cosElTimesRange_d * (*sinAz_p),
                                  -(*imgScanline_p) * sinEl_d,
                                  cosElTimesRange_d * (*cosAz_p) );
            
                fr_vector.push_back( point );
            }
        }
    }
    else
    {
#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 )
        {
            const double *    imgScanline_p = m_image.getScanline(i);
            const C3DVector * lutScanline_p = m_lut2Local.getScanline(i);

            for ( int j = 0 ; j < w_i; ++j, ++imgScanline_p, ++lutScanline_p )
            {            
                fr_vector.push_back( *lutScanline_p * *imgScanline_p );
            }
        }
    }

    return true;
}

/// Convert the whole image to 3D points in a vector.
bool
CSphericalRangeImage::image2World ( C3DPointDataVector &fr_vector ) const
{
    const int h_i = m_image.getHeight();
    const int w_i = m_image.getWidth();
        
    if ( m_optMode_e == OM_MEMORY )
    {
#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 )
        {
            const double &cosEl_d = m_luvCosEl_p[i];
            const double &sinEl_d = m_luvSinEl_p[i];
            
            const double * imgScanline_p = m_image.getScanline(i);
            const double * cosAz_p       = m_luvCosAz_p;
            const double * sinAz_p       = m_luvSinAz_p;
            
            for ( int j = 0 ; j < w_i; ++j, ++imgScanline_p, ++cosAz_p, ++sinAz_p )
            {
                const double cosElTimesRange_d = cosEl_d * (*imgScanline_p);
                
                C3DVector point ( cosElTimesRange_d * (*sinAz_p),
                                  -(*imgScanline_p) * sinEl_d,
                                  cosElTimesRange_d * (*cosAz_p) );
                
                point = m_rotation * point + m_translation;
                
                fr_vector.push_back( point );
            }
        }
    }
    else
    {
#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 )
        {
            const double *    imgScanline_p = m_image.getScanline(i);
            const double *    endScanline_p = imgScanline_p + w_i;
            const C3DVector * lutScanline_p = m_lut2World.getScanline(i);

           for ( ; imgScanline_p < endScanline_p;
                  ++imgScanline_p, ++lutScanline_p )
            {
                fr_vector.push_back( *lutScanline_p * *imgScanline_p + m_translation );
            }
        }
    }

    return true;
}

/// Convert the whole image to a C3DVectorImage.
bool
CSphericalRangeImage::image2Local ( C3DVectorImage &fr_img ) const
{
    const int h_i = m_image.getHeight();
    const int w_i = m_image.getWidth();
        
    if ( w_i > (int)fr_img.getWidth() ||
         h_i > (int)fr_img.getHeight() )
        return false;

    if ( m_optMode_e == OM_MEMORY )
    {
#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 *scanline_p = fr_img.getScanline ( i );
            
            const double &cosEl_d = m_luvCosEl_p[i];
            const double &sinEl_d = m_luvSinEl_p[i];
            
            const double * imgScanline_p = m_image.getScanline(i);
            const double * cosAz_p       = m_luvCosAz_p;
            const double * sinAz_p       = m_luvSinAz_p;
            
            for ( int j = 0 ; j < w_i; ++j, ++imgScanline_p, 
                      ++cosAz_p, ++sinAz_p, ++scanline_p )
            {
                const double cosElTimesRange_d = cosEl_d * (*imgScanline_p);
                
                scanline_p -> set ( cosElTimesRange_d * (*sinAz_p),
                                    -(*imgScanline_p) * sinEl_d,
                                    cosElTimesRange_d * (*cosAz_p) );
            }
        }
    }
    else
    {
#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 )
        {
            const double *    imgScanline_p = m_image.getScanline( i );
            const double *    endScanline_p = imgScanline_p + w_i;
            const C3DVector * lutScanline_p = m_lut2Local.getScanline( i );
            C3DVector *       dstScanline_p = fr_img.getScanline ( i );

            for ( ; imgScanline_p < endScanline_p;
                  ++imgScanline_p, ++lutScanline_p, ++dstScanline_p )
            {
                *dstScanline_p = *lutScanline_p * *imgScanline_p;
            }
        }
    }

    return true;
}

/// Convert the whole image to a C3DVectorImage.
bool
CSphericalRangeImage::image2World ( C3DVectorImage &fr_img ) const
{
    const int h_i = m_image.getHeight();
    const int w_i = m_image.getWidth();
        
    if ( w_i > (int)fr_img.getWidth() ||
         h_i > (int)fr_img.getHeight() )
        return false;

    if ( m_optMode_e == OM_MEMORY )
    {
#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 *scanline_p = fr_img.getScanline ( i );
            
            const double &cosEl_d = m_luvCosEl_p[i];
            const double &sinEl_d = m_luvSinEl_p[i];
            
            const double * imgScanline_p = m_image.getScanline(i);
            const double * cosAz_p       = m_luvCosAz_p;
            const double * sinAz_p       = m_luvSinAz_p;
            
            for ( int j = 0 ; j < w_i; ++j, ++imgScanline_p, 
                      ++cosAz_p, ++sinAz_p, ++scanline_p )
            {
                const double cosElTimesRange_d = cosEl_d * (*imgScanline_p);
                
                C3DVector point ( cosElTimesRange_d * (*sinAz_p),
                                  -(*imgScanline_p) * sinEl_d,
                                  cosElTimesRange_d * (*cosAz_p) );
                
                *scanline_p = m_rotation * point + m_translation;
            }
        }
    }
    else
    {
 #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 )
        {
            const double *    imgScanline_p = m_image.getScanline( i );
            const double *    endScanline_p = imgScanline_p + w_i;
            const C3DVector * lutScanline_p = m_lut2World.getScanline( i );
            C3DVector *       dstScanline_p = fr_img.getScanline ( i );
            
            for ( ; imgScanline_p < endScanline_p;
                  ++imgScanline_p, ++lutScanline_p, ++dstScanline_p )
            {
                *dstScanline_p =  ( *lutScanline_p * *imgScanline_p + m_translation );
            }
        }
    }
    
    return true;
}

/// Set velodyne
bool
CSphericalRangeImage::setVelodyne ( CVelodyne f_velodyne )
{
    m_velodyne    = f_velodyne;
    m_translation = m_velodyne.getTranslation();

    if (m_velodyne.getRotation() != m_rotation)
    {
        m_rotation = m_velodyne.getRotation();
        
        allocateLutsAndImage();
        recalculateLuts();
    }
    else
    {        
    }

    return true;
}


/// Get velodyne
CVelodyne
CSphericalRangeImage::getVelodyne (  ) const
{
    return m_velodyne;
}

/// Set the roi.
bool
CSphericalRangeImage::setRoi ( S2D<double> f_topLeft,
                               S2D<double> f_size )
{
    if ( m_roiTopLeft == f_topLeft &&
         m_roiSize    == f_size )
        return true;
    
    m_roiTopLeft = f_topLeft;
    m_roiSize    = f_size;

    allocateLutsAndImage();
    recalculateLuts();    

    return true;    
}

/// Set the ROI top left corner
bool
CSphericalRangeImage::setRoiTopLeft ( S2D<double> f_topLeft )
{
    if ( f_topLeft == m_roiTopLeft )
        return true;

    m_roiTopLeft = f_topLeft;

    allocateLutsAndImage();
    recalculateLuts();    

    return true;
}

S2D<double>
CSphericalRangeImage::getRoiTopLeft ( ) const
{
    return m_roiTopLeft;
}

/// Set the roi size.
bool
CSphericalRangeImage::setRoiSize ( S2D<double> f_roiSize )
{
    if ( m_roiSize == f_roiSize )
        return true;

    //printf("\n\n\nSetting roi to %f %f (this is image %p data %p)\n", f_roiSize.width, f_roiSize.height, &m_image, m_image.getData());
    
    m_roiSize = f_roiSize;

    allocateLutsAndImage();
    recalculateLuts();    

    //printf("End setting roi to %f %f (this is image %p data %p) width %i height %i \n\n\n\n", m_roiSize.width, m_roiSize.height, &m_image, m_image.getData(), m_image.getWidth(), m_image.getHeight());
    return true;    
}


S2D<double>
CSphericalRangeImage::getRoiSize ( ) const
{
    return m_roiSize;
}


/// Set the scale.
bool
CSphericalRangeImage::setScale ( S2D<double> f_scale )
{
    if ( m_scale == f_scale )
        return true;
    
    m_scale = f_scale;

    allocateLutsAndImage();
    recalculateLuts();    

    return true;    
    
}


S2D<double>
CSphericalRangeImage::getScale ( ) const
{
    return m_scale;
}


/// Set the interp mode.
bool
CSphericalRangeImage::setInterpolationMode ( CSphericalRangeImage::EInterpolationMode f_mode_e )
{
    m_interpMode_e = f_mode_e;
    return true;
}


CSphericalRangeImage::EInterpolationMode
CSphericalRangeImage::getInterpolationMode ( ) const
{
    return m_interpMode_e;
}

/// Set the optimization mode.
bool
CSphericalRangeImage::setOptimizationMode ( CSphericalRangeImage::EOptimizationMode f_mode_e )
{
    if (m_optMode_e != f_mode_e)
    {
        m_optMode_e = f_mode_e;
        allocateLutsAndImage();

        recalculateLuts();
    }

    return true;
}


CSphericalRangeImage::EOptimizationMode
CSphericalRangeImage::getOptimizationMode ( ) const
{
    return m_optMode_e;
}

/// Convert image to spherical coordinates.
inline double
CSphericalRangeImage::column2Azimuth ( double  f_u_d ) const
{
    return m_roiTopLeft.x + f_u_d * m_scale.x;
}


/// Convert image to spherical coordinates.
inline double
CSphericalRangeImage::row2Elevation ( double  f_v_d ) const
{
    return m_roiTopLeft.y + f_v_d * m_scale.y;
}

/// Convert image to spherical coordinates.
inline double
CSphericalRangeImage::azimuth2Column ( double  f_az_d ) const
{
    double delta_d = f_az_d - m_roiTopLeft.x;

    if ( delta_d > (2*M_PI) )
        delta_d -= ((int)(delta_d / (2*M_PI))) * (2*M_PI);
    
    if ( delta_d < 0 )
        delta_d += ((int)(-delta_d / (2*M_PI))+1) * (2*M_PI);

    return delta_d/m_scale.x;
}

/// Convert image to spherical coordinates.
inline double
CSphericalRangeImage::elevation2Row ( double  f_el_d ) const

{
    double delta_d = f_el_d - m_roiTopLeft.y;

    if ( delta_d > (2*M_PI) )
        delta_d -= ((int)(delta_d / (2*M_PI))) * (2*M_PI);
    
    if ( delta_d < 0 )
        delta_d += ((int)(-delta_d / (2*M_PI))+1) * (2*M_PI);

    return delta_d/m_scale.y;
}


/// Recalculate Lut if velodyne or image format has changed.
inline void
CSphericalRangeImage::recalculateLuts()
{
    if (m_optMode_e == OM_SPEED )
    {
        const int width_i  = (int)m_image.getWidth();
        const int height_i = (int)m_image.getHeight();
        
#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 < height_i; ++i)
        {
            const double el_d = row2Elevation ( i );
            const double cosEl_d = cos( el_d );
            const double sinEl_d = sin( el_d );
            
            C3DVector *local_p = m_lut2Local.getScanline ( i );
            C3DVector *world_p = m_lut2World.getScanline ( i );
            
            for (int j = 0; j < width_i; ++j, ++local_p, ++world_p)
            {
                const double az_d = column2Azimuth ( j );
                
                local_p -> set( cosEl_d * sin( az_d ),
                              - sinEl_d,
                              cosEl_d * cos( az_d ) );

                *world_p = m_rotation * *local_p;
            }
            
        }
    }
    else
    {
        const int width_i  = m_image.getWidth();
        for (int j = 0; j < width_i; ++j)
        {
            double az_d = column2Azimuth ( j );
            m_luvCosAz_p[j] = cos( az_d );
            m_luvSinAz_p[j] = sin( az_d );
        }
        
        const int height_i = m_image.getHeight();
        for (int i = 0; i < height_i; ++i)
        {
            double el_d = row2Elevation ( i );
            m_luvCosEl_p[i] = cos( el_d );
            m_luvSinEl_p[i] = sin( el_d );
        }
    }
}

/// Convert image to 3D point in local coordinate system.
inline bool
CSphericalRangeImage::image2Local ( int        f_u_i,
                                    int        f_v_i,
                                    double     f_range_d,
                                    C3DVector &fr_point ) const
{
    const int &w_i = m_image.getWidth();
    const int &h_i = m_image.getHeight();
    
    if ( !(f_u_i >= 0 && f_u_i < w_i && 
           f_v_i >= 0 && f_v_i < h_i ) )
        return false;


    if ( m_optMode_e == OM_MEMORY )
    {
        const double cosElTimesRange_d = m_luvCosEl_p[f_v_i] * f_range_d;
        
        fr_point.at(0) = cosElTimesRange_d * m_luvSinAz_p [ f_u_i ];
        fr_point.at(1) = -f_range_d        * m_luvSinEl_p [ f_v_i ];
        fr_point.at(2) = cosElTimesRange_d * m_luvCosAz_p [ f_u_i ];
    }
    else
    {
        fr_point = m_lut2Local.getScanline ( f_v_i )[f_u_i] * f_range_d;
    }
    
    return true;    

}


/// Convert image to 3D point in local coordinate system.
inline bool
CSphericalRangeImage::image2Local ( int     f_u_i,
                                    int     f_v_i,
                                    double  f_range_d,
                                    double &fr_x_d,
                                    double &fr_y_d,
                                    double &fr_z_d ) const
{
    C3DVector vec;
    
    if ( ! image2Local ( f_u_i,
                         f_v_i,
                         f_range_d,
                         vec ) )
        return false;
    
    fr_x_d = vec.x();
    fr_y_d = vec.y();
    fr_z_d = vec.z();

    return true;

}


        /// Convert image to 3D point in local coordinate system.
inline bool
CSphericalRangeImage::image2World ( int        f_u_i,
                                    int        f_v_i,
                                    double     f_range_d,
                                    C3DVector &fr_point ) const
{
    const int &w_i = m_image.getWidth();
    const int &h_i = m_image.getHeight();
    
    if ( !(f_u_i >= 0 && f_u_i < w_i && 
           f_v_i >= 0 && f_v_i < h_i ) )
        return false;

    if ( m_optMode_e == OM_MEMORY )
    {
        const double cosElTimesRange_d = m_luvCosEl_p[f_v_i] * f_range_d;
        
        fr_point.at(0) = cosElTimesRange_d * m_luvSinAz_p [ f_u_i ];
        fr_point.at(1) = -f_range_d        * m_luvSinEl_p [ f_v_i ];
        fr_point.at(2) = cosElTimesRange_d * m_luvCosAz_p [ f_u_i ];

        fr_point = m_rotation * fr_point + m_translation;
    }
    else
    {
        fr_point = (m_lut2World.getScanline ( f_v_i )[f_u_i] * f_range_d) + m_translation;
    }
    
    return true;
}


        /// Convert image to 3D point in local coordinate system.
inline bool
CSphericalRangeImage::image2World ( int     f_u_i,
                                    int     f_v_i,
                                    double  f_range_d,
                                    double &fr_x_d,                                    
                                    double &fr_y_d,
                                    double &fr_z_d ) const
{
    C3DVector vec;
    
    if ( ! image2World ( f_u_i,
                         f_v_i,
                         f_range_d,
                         vec ) )
        return false;
    
    fr_x_d = vec.x();
    fr_y_d = vec.y();
    fr_z_d = vec.z();
    
    return true;
}

CParameterSet *   
CSphericalRangeImage::getParameterSet ( std::string f_name_str )
{
    if ( !m_paramSet_p )
    {
        m_paramSet_p = new CParameterSet ( NULL );
        m_paramSet_p -> setName ( f_name_str );

        m_paramSet_p -> addParameter (
                new CDbl2DParameter ( "ROI Top-Left",
                                      "Spherical coordinates corresponding to the "
                                      "top left of the SRI [deg].",
                                      getRoiTopLeft(),
                                      "Az.", "El.",
                                     new CParameterConnector< CSphericalRangeImage, S2D<double>, CDbl2DParameter>
                                     ( this,
                                       &CSphericalRangeImage::getRoiTopLeft,
                                       &CSphericalRangeImage::setRoiTopLeft ) ) );
        
        m_paramSet_p -> addParameter (
                new CDbl2DParameter ( "ROI Size",
                                      "ROI of the measurement space to consider for the "
                                      "SRI [deg].",
                                      getRoiSize(),
                                      "Az.", "El.",
                                      new CParameterConnector< CSphericalRangeImage, S2D<double>, CDbl2DParameter>
                                      ( this,
                                        &CSphericalRangeImage::getRoiSize,
                                        &CSphericalRangeImage::setRoiSize ) ) );
        
        m_paramSet_p -> addParameter (
                new CDbl2DParameter ( "Scale",
                                      "Scale for the SRI. Each pixel of the SRI will represent "
                                      "a unit scale [deg].",
                                      getScale(),
                                      "Az.", "El.",
                                      new CParameterConnector< CSphericalRangeImage, S2D<double>, CDbl2DParameter>
                                      ( this,
                                        &CSphericalRangeImage::getScale,
                                        &CSphericalRangeImage::setScale ) ) );
    }

    return m_paramSet_p;
}

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