#ifndef __SPHERICALRANGEIMAGE_H
#define __SPHERICALRANGEIMAGE_H

/**
 *******************************************************************************
 *
 * @file sphericalRangeImage.h
 *
 * \class CSphericalRangeImage
 * \date  Tue Oct 13, 2009
 * \author Hernan Badino (hernan.badino@gmail.com)
 *
 * \brief Implements a model for handling a Spherical Range Image (SRI).
 *
 * A spherical range image is an image composed of ranges. The image coordinates 
 * represent the azimuth (columns) and elevation (rows) of a spherical coordinate 
 * system. Image coordinate of a given image position plus the range stored at that 
 * position permits the computation of a 3D Euclidean point.
 * The computation of 3D points is performed with LUT tables. Two different LUT 
 * methods are implemented and the user must specified which to used when 
 * instantiating the object or by calling to the setOptimizationMode() method.
 * The class provides all convenience functions to transform image data into 3D 
 * and vice-versa.
 *
 *******************************************************************************/

/* INCLUDES */
#include "velodyne.h"
#include "standardTypes.h"
#include "3DMatrix.h"
#include "3DPointVector.h"
#include "3DVectorImage.h"
#include "velodyneStream.h"
#include "doubleImage.h"

/* CONSTANTS */

namespace VIC
{
    /* PROTOTYPES */
    class CParameterSet;

    /* CLASS DEFINITION */
    class CSphericalRangeImage: public CIOObj
    {
    /// Public data types.
    public:
        typedef enum
        {
            IM_NN,
            IM_LINEAR
        } EInterpolationMode;
        
    /// Public data types.
    public:
        typedef enum
        {
            OM_MEMORY,
            OM_SPEED
        } EOptimizationMode;

    /// Constructors/Destructor
    public:
        CSphericalRangeImage ( );
        virtual ~CSphericalRangeImage ( );        

    /// Operators.
    public:
        CSphericalRangeImage & operator = ( const CSphericalRangeImage &f_other );

        virtual bool          copyFrom ( const CSphericalRangeImage &f_other,
                                         bool f_allocateImage_b = false,
                                         bool f_reuseLuts_b = true );

    /// Sets/Gets
    public:
        virtual void     clear();
        
        /// Add measurements.
        virtual bool     addMeasurement  ( double f_range_d,
                                           double f_azimuth_d,
                                           double f_elevation_d );

        virtual bool     addMeasurement  ( SSphericalPointData f_meas );

        virtual bool     addMeasurements ( const CSphericalDataVector &f_vector );

        virtual bool     addMeasurements ( const CVelodyneDataStream &f_vector );

    /// Sets/Gets
    public:
    /// Convert image to spherical coordinates and viceversa.
    public:
         /// Convert image to spherical coordinates.
        virtual bool     image2Spherical ( double  f_u_d,
                                           double  f_v_d,
                                           double &fr_azimuth_d,
                                           double &fr_elevation_d ) const;

        /// Convert image to spherical coordinates.
        virtual bool     image2Spherical ( double  f_u_d,
                                           double  f_v_d,
                                           double &fr_range_d,
                                           double &fr_azimuth_d,
                                           double &fr_elevation_d ) const;

        /// Convert image to spherical coordinates.
        virtual bool     image2Spherical ( double               f_u_d,
                                           double               f_v_d,
                                           SSphericalPointData &fr_meas ) const;

        /// Convert spherical coordinates to image.
        virtual bool     spherical2Image ( double f_azimuth_d,
                                           double f_elevation_d,
                                           double &fr_u_d,
                                           double &fr_v_d ) const;        

        /// Convert image to spherical coordinates.
        virtual double   column2Azimuth ( double  f_u_d  ) const;

        /// Convert image to spherical coordinates.
        virtual double   row2Elevation ( double  f_v_d ) const;

        /// Convert image to spherical coordinates.
        virtual double   azimuth2Column ( double  f_az_d  ) const;

        /// Convert image to spherical coordinates.
        virtual double   elevation2Row ( double  f_el_d ) const;

    /// Convert image to 3D points taking ranges from the stored image.
    public:
        /// Convert image to 3D point in local coordinate system.
        virtual bool     image2Local ( int        f_u_i,
                                       int        f_v_i,
                                       C3DVector &fr_point ) const;

        /// Convert image to 3D point in local coordinate system.
        virtual bool     image2Local ( int     f_u_i,
                                       int     f_v_i,
                                       double &fr_x_d,
                                       double &fr_y_d,
                                       double &fr_z_d ) const;

        /// Convert image to 3D point in local coordinate system.
        virtual bool     image2World ( int        f_u_i,
                                       int        f_v_i,
                                       C3DVector &fr_point ) const;

        /// Convert image to 3D point in local coordinate system.
        virtual bool     image2World ( int     f_u_i,
                                       int     f_v_i,
                                       double &fr_x_d,
                                       double &fr_y_d,
                                       double &fr_z_d ) const;

    /// Convert image to 3D points, ranges given as parameter.
    public:
        /// Convert image to 3D point in local coordinate system.
        virtual bool     image2Local ( int        f_u_i,
                                       int        f_v_i,
                                       double     f_range_d,
                                       C3DVector &fr_point ) const;

        /// Convert image to 3D point in local coordinate system.
        virtual bool     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;

        /// Convert image to 3D point in local coordinate system.
        virtual bool     image2World ( int        f_u_i,
                                       int        f_v_i,
                                       double     f_range_d,
                                       C3DVector &fr_point ) const;

        /// Convert image to 3D point in local coordinate system.
        virtual bool     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;

    /// Convert all image points to 3D points.
    public:
        /// Convert the whole image to 3D points in a vector.
        virtual bool     image2Local ( C3DPointDataVector &fr_vector ) const;

        /// Convert the whole image to 3D points in a vector.
        virtual bool     image2World ( C3DPointDataVector &fr_vector ) const;

        /// Convert the whole image to a C3DVectorImage.
        virtual bool     image2Local ( C3DVectorImage &f_img ) const;

        /// Convert the whole image to a C3DVectorImage.
        virtual bool     image2World ( C3DVectorImage &f_img ) const;

    /// Parameters.
    public:
        virtual CParameterSet * getParameterSet ( std::string f_name_str = "Spherical Range Image" );

    /// Sets/Gets
    public:
        /// Set and Get Velodyne
        bool                 setVelodyne ( CVelodyne f_velodyne );
        CVelodyne            getVelodyne (  ) const;
        
        /// Set the roi.
        bool                 setRoi ( S2D<double> f_topLeft,
                                      S2D<double> f_size );
        
        /// Set the ROI top left corner
        bool                 setRoiTopLeft ( S2D<double> f_topLeft );
        S2D<double>          getRoiTopLeft ( ) const;

        /// Set the roi size.
        bool                 setRoiSize ( S2D<double> f_roiSize );
        S2D<double>          getRoiSize ( ) const;

        /// Set the scale.
        bool                 setScale ( S2D<double> f_scale );
        S2D<double>          getScale ( ) const;

        /// Set the interp mode.
        bool                 setInterpolationMode ( EInterpolationMode f_mode_e );
        EInterpolationMode   getInterpolationMode ( ) const;

        /// Set the interp mode.
        bool                 setOptimizationMode ( EOptimizationMode f_mode_e );
        EOptimizationMode    getOptimizationMode ( ) const;

        /// Get the sri image (const version).
        const CDoubleImage & getImage ( ) const;

        /// Get the sri image (version).
        CDoubleImage & getImage ( );
        
        /// Get rotation of between local and world coord systems.
        C3DMatrix            getRotation() const;

        /// Get translation between local and world coord systems.
        C3DVector            getTranslation() const;

        /// Verify if other sri has the same format as this one.
        bool                 hasSameFormat ( const CSphericalRangeImage & f_other ) const;

    /// Protected help functions.
    protected:
        /// Recaltulate look up table and vectors.
        void             recalculateLuts();

        /// Free Look-up Vectors.
        void             freeLutsAndImage();

        /// Allocate Look-up Vectors.
        void             allocateLutsAndImage();

    /// Protected members
    protected:        
        /// Image
        C3DVectorImage      m_lut2Local;

        /// Image
        C3DVectorImage      m_lut2World;

        /// Image
        CDoubleImage        m_image;

        /// Interpolation mode.
        EInterpolationMode  m_interpMode_e;

        /// Optimization mode.
        EOptimizationMode   m_optMode_e;

        /// Velodyne
        CVelodyne           m_velodyne;

        /// Rotation matrix
        C3DMatrix           m_rotation;

        /// Translation vector.
        C3DVector           m_translation;
        
        /// ROI top left [deg].
        S2D<double>         m_roiTopLeft;

        /// Size [deg].
        S2D<double>         m_roiSize;

        /// Size [deg].
        S2D<double>         m_scale;

        /// Self allocation
        bool                m_luvsOwner_b;
        
        /// Size [deg].
        double *            m_luvCosAz_p;

        /// Size [deg].
        double *            m_luvSinAz_p;

        /// Size [deg].
        double *            m_luvCosEl_p;

        /// Size [deg].
        double *            m_luvSinEl_p;

        /// Parameter set
        CParameterSet *     m_paramSet_p;

    };
    
    
    /// Get rotation of between local and world coord systems.
    inline C3DMatrix
    CSphericalRangeImage::getRotation() const
    {
        return m_rotation;
    }

    /// Get translation between local and world coord systems.
    inline C3DVector
    CSphericalRangeImage::getTranslation() const
    {
        return m_translation;
    }
    
    /// Verify if other sri has the same format as this one.
    inline bool
    CSphericalRangeImage::hasSameFormat ( const CSphericalRangeImage & f_other ) const
    {
        return ( m_image.hasSameFormat ( f_other.m_image ) && 
                 m_interpMode_e == f_other.m_interpMode_e && 
                 m_optMode_e    == f_other.m_optMode_e && 
                 m_roiSize      == f_other.m_roiSize && 
                 m_scale        == f_other.m_scale &&
                 m_roiTopLeft   == f_other.m_roiTopLeft );
    }
    
    /// Get the sri image.
    inline
    const CDoubleImage & 
    CSphericalRangeImage::getImage ( ) const
    {
        return m_image;   
    }

    /// Get the sri image.
    inline
    CDoubleImage & 
    CSphericalRangeImage::getImage ( )
    {
        return m_image;   
    }
}


#endif // __SPHERICALRANGEIMAGE_H

/* ////////////  Version History ///////////////
 *  $Log: sphericalRangeImage.h,v $
 *  Revision 1.3  2009/11/18 15:51:01  badino
 *  badino: documentation added. Some other global changes.
 *
 *  Revision 1.2  2009/10/13 19:52:26  badino
 *  badino: added documentation.
 *
 *//////////////////////////////////////////////
