#ifndef __VELODYNE_H
#define __VELODYNE_H

/**
 *******************************************************************************
 *
 * @file velodyne.h
 *
 * \class CVelodyne
 * \date  Tue Oct 13, 2009
 * \author Hernan Badino (hernan.badino@gmail.com)
 *
 * \brief Implements a model for handling Velodyne measurements.
 *
 * The class implements the required methods to transform Velodyne
 * measurements into Euclidean points. A world and local coordinate system
 * can be defined by specifing a rotation and translation of the velodyne
 * with respect to the origin of the world coordinate system.
 *
 *
 *******************************************************************************/

/* INCLUDES */
#include "paramIOHandling.h"
#include "3DRowVector.h"
#include "ioObj.h"
#include "3DMatrix.h"
#include "3DRowVector.h"
#include "sphericalDataVector.h"

/* CONSTANTS */

namespace VIC
{
    class CVelodyne: public CIOObj
    {
        /// Constructors/Destructor
    public:
        
        CVelodyne ();
        CVelodyne ( const CParamIOHandling & fr_paraReader );
        virtual ~CVelodyne();        

        /// Parameter handling.
    public:
        virtual bool      load ( const CParamIOHandling & fr_paraReader );
        virtual bool      save ( const CParamIOHandling & fr_paraWriter ) const;
        
        /// Get/Set methods.
    public:

        /// Rotation.
        virtual C3DMatrix getRotation ( ) const;            
        virtual bool      setRotation ( const C3DMatrix & f_matrix );

        /// Translation.
        virtual C3DVector getTranslation ( ) const;
        virtual bool      setTranslation ( const C3DVector & f_translation );


    public:
        /// Velodyne/World transformations.
    public:

        /// World to spherical (sensor) coordinate system.
        virtual bool      world2Spherical ( double  f_x_d, 
                                            double  f_y_d,
                                            double  f_z_d,
                                            double &fr_range_d,
                                            double &fr_azimuth_d,
                                            double &fr_elevation_d ) const;

        virtual bool      world2Spherical ( C3DVector  f_point,
                                            double    &fr_range_d,
                                            double    &fr_azimuth_d,
                                            double    &fr_elevation_d ) const;

        virtual bool      world2Spherical ( C3DVector            f_point,
                                            SSphericalPointData &fr_meas ) const;

        /// Local to spherical (sensor) coordinate system.
        virtual bool      local2Spherical ( double  f_x_d, 
                                            double  f_y_d,
                                            double  f_z_d,
                                            double &fr_range_d,
                                            double &fr_azimuth_d,
                                            double &fr_elevation_d ) const;
        
        virtual bool      local2Spherical ( C3DVector  f_point,
                                            double    &fr_range_d,
                                            double    &fr_azimuth_d,
                                            double    &fr_elevation_d ) const;

        virtual bool      local2Spherical ( C3DVector            f_point,
                                            SSphericalPointData &fr_meas ) const;

        /// Spherical (sensor) to world coordinate system.
        virtual bool      spherical2World  ( double  f_range_d,
                                             double  f_azimuth_d,
                                             double  f_elevation_d, 
                                             double &fr_x_d,
                                             double &fr_y_d,
                                             double &fr_z_d ) const;


        virtual bool      spherical2World  ( double       f_range_d,
                                             double       f_azimuth_d,
                                             double       f_elevation_d, 
                                             C3DVector  &fr_point ) const;

        virtual bool      spherical2World  ( SSphericalPointData f_meas,
                                             C3DVector          &fr_point ) const;
        
        /// Spherical (sensor) to local coordinate system.
        virtual bool      spherical2Local ( double  f_range_d,
                                            double  f_azimuth_d,
                                            double  f_elevation_d, 
                                            double &fr_x_d,
                                            double &fr_y_d,
                                            double &fr_z_d ) const;

        virtual bool      spherical2Local ( double       f_range_d,
                                            double       f_azimuth_d,
                                            double       f_elevation_d, 
                                            C3DVector  &fr_point ) const;
        
        virtual bool      spherical2Local ( SSphericalPointData f_meas,
                                            C3DVector          &fr_point ) const;

       /// Set a fixed elevation.
        virtual bool      setElevation  ( double  f_elevation_d );
        
        /// Set a fixed elevation.
        virtual double    getElevation  ( ) const { return m_fixedElevation_d; }
        

        /// Spherical (sensor) to world coordinate system.
        virtual bool      spherical2World  ( double  f_range_d,
                                             double  f_azimuth_d, 
                                             double &fr_x_d,
                                             double &fr_y_d,
                                             double &fr_z_d ) const;
        
        /// Spherical (sensor) to world coordinate system.
        virtual bool      spherical2World  ( double       f_range_d,
                                             double       f_azimuth_d, 
                                             C3DVector  &fr_point ) const;
        
        /// Spherical (sensor) to local coordinate system.
        virtual bool      spherical2Local  ( double  f_range_d,
                                             double  f_azimuth_d, 
                                             double &fr_x_d,
                                             double &fr_y_d,
                                             double &fr_z_d ) const;
         
        /// Spherical (sensor) to local coordinate system.
        virtual bool      spherical2Local  ( double       f_range_d,
                                             double       f_azimuth_d, 
                                             C3DVector  &fr_point ) const;

        /// World 2 local transformation.
        virtual bool      world2Local ( const C3DVector  &f_worldPoint,
                                        C3DVector        &fr_localPoint ) const;                                

        /// World 2 local transformation.
        virtual bool      local2World ( const C3DVector  &f_localPoint,
                                        C3DVector        &fr_worldPoint ) const;
        

        /// Protected members:
    protected:
        
        /// Rotation matrix to world coordinate system.
        C3DMatrix          m_rotation;

        /// Rotation matrix to world coordinate system.
        C3DMatrix          m_invRotation;

        /// Translation vector to world coordinate system.
        C3DVector          m_translation;
        
        /// Fixed elevation (for faster computation).
        double             m_fixedElevation_d;

        /// Sin of fixed elevation (for faster computation).
        double             m_sinFixedElevation_d;

        /// Cos of fixed elevation (for faster computation).
        double             m_cosFixedElevation_d;
    };
}


#endif // __VELODYNE_H

/* ////////////  Version History ///////////////
 *  $Log: velodyne.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.
 *
 *//////////////////////////////////////////////
