/*@@@**************************************************************************
* \file  velodyne
* \author Hernan Badino
* \date  Mon Jun 22 17:58:21 EDT 2009
* \notes 
*******************************************************************************
******************************************************************************/

/* INCLUDES */
#include "doubleParam.h"
#include "velodyne.h"
#include "logger.h"
#include <math.h>

using namespace VIC;

CVelodyne::CVelodyne ()
        : m_fixedElevation_d (     0. ),
          m_sinFixedElevation_d (  3. ),
          m_cosFixedElevation_d (  3. )
{
    m_rotation.loadIdentity( );
    m_invRotation.loadIdentity( );
    m_translation.clear( );
}

CVelodyne::CVelodyne ( const CParamIOHandling & fr_paraReader )
        : m_fixedElevation_d (     0. ),
          m_sinFixedElevation_d (  3. ),
          m_cosFixedElevation_d (  3. )
{
    m_rotation.loadIdentity( );
    m_invRotation.loadIdentity( );
    m_translation.clear( );

    load ( fr_paraReader );
}

CVelodyne::~CVelodyne()
{
}

bool 
CVelodyne::load ( const CParamIOHandling & fr_paraReader )
{
    std::string value_str;
    CDoubleParameter paraDouble;
    C3DVector rotation;

    if ( fr_paraReader.get ( "X Rotation", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        rotation.setX ( paraDouble.getValue() );

    if ( fr_paraReader.get ( "Y Rotation", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        rotation.setY ( paraDouble.getValue() );

    if ( fr_paraReader.get ( "Z Rotation", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        rotation.setZ ( paraDouble.getValue() );
    
    if ( fr_paraReader.get ( "X Translation", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        m_translation.setX (  paraDouble.getValue() );

    if ( fr_paraReader.get ( "Y Translation", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        m_translation.setY (  paraDouble.getValue() );

    if ( fr_paraReader.get ( "Z Translation", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        m_translation.setZ (  paraDouble.getValue() );
    
    m_rotation.loadIdentity( );

    m_rotation.rotateX ( rotation.x() );
    m_rotation.rotateY ( rotation.y() );
    m_rotation.rotateZ ( rotation.z() );
   
    m_invRotation = m_rotation.getInverse();
    
    /// by now.
    return true;    
}

bool 
CVelodyne::save ( const CParamIOHandling & /*fr_paraWriter*/ ) const
{
    return false;    
}

/// World to spherical (sensor) coordinate system.
inline bool
CVelodyne::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
{
    C3DVector point ( f_x_d, f_y_d, f_z_d );
    
    return world2Spherical ( point,
                             fr_range_d,
                             fr_azimuth_d,
                             fr_elevation_d );
}


/// World to spherical (sensor) coordinate system.
inline bool
CVelodyne::world2Spherical ( C3DVector  f_point,
                             double    &fr_range_d,
                             double    &fr_azimuth_d,
                             double    &fr_elevation_d ) const
{
    C3DVector localPoint = m_invRotation * (f_point - m_translation);    

    return local2Spherical ( localPoint,
                             fr_range_d,
                             fr_azimuth_d,
                             fr_elevation_d );    
}

/// World to spherical (sensor) coordinate system.
inline bool
CVelodyne::world2Spherical ( C3DVector            f_point,
                             SSphericalPointData &fr_meas ) const
{
    return world2Spherical ( f_point, 
                             fr_meas.range_d,
                             fr_meas.azimuth_d,
                             fr_meas.elevation_d );
}



/// Local to spherical (sensor) coordinate system.
inline bool
CVelodyne::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
{
    C3DVector point ( f_x_d, f_y_d, f_z_d );
    
    return local2Spherical ( point,
                             fr_range_d,
                             fr_azimuth_d,
                             fr_elevation_d );
}

/// Local to spherical (sensor) coordinate system.
inline bool
CVelodyne::local2Spherical ( C3DVector  f_point,
                             double    &fr_range_d,
                             double    &fr_azimuth_d,
                             double    &fr_elevation_d ) const
{
    fr_range_d     = f_point.magnitude();
    fr_elevation_d = asin( -f_point.y() / fr_range_d );
    fr_azimuth_d   = atan2( f_point.x(), f_point.z() );

    return true;
}

/// World to spherical (sensor) coordinate system.
inline bool
CVelodyne::local2Spherical ( C3DVector            f_point,
                             SSphericalPointData &fr_meas ) const
{
    return local2Spherical ( f_point, 
                             fr_meas.range_d,
                             fr_meas.azimuth_d,
                             fr_meas.elevation_d );
}


/// Spherical (sensor) to world coordinate system.
inline bool
CVelodyne::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
{
    C3DVector aux;
   
    if ( !spherical2World  ( f_range_d,
                             f_azimuth_d,
                             f_elevation_d, 
                             aux ) )
        return false;
    
    fr_x_d = aux.x();
    fr_y_d = aux.y();
    fr_z_d = aux.z();
    
    return true;
}


inline bool
CVelodyne::spherical2World  ( SSphericalPointData f_meas,
                              C3DVector          &fr_point ) const
{
    return spherical2World  ( f_meas.range_d,
                              f_meas.azimuth_d,
                              f_meas.elevation_d,
                              fr_point );
}


/// Set a fixed elevation.
inline bool
CVelodyne::setElevation  ( double  f_elevation_d )
{
    m_fixedElevation_d    = f_elevation_d;
    m_sinFixedElevation_d = sin(f_elevation_d);
    m_cosFixedElevation_d = cos(f_elevation_d);

    return true;
}

/// Spherical (sensor) to world coordinate system.
inline bool
CVelodyne::spherical2World  ( double  f_range_d,
                              double  f_azimuth_d, 
                              double &fr_x_d,
                              double &fr_y_d,
                              double &fr_z_d ) const
{
    C3DVector aux;
   
    if (! spherical2World  ( f_range_d,
                             f_azimuth_d, 
                             aux ) )
        return false;

    fr_x_d = aux.x();
    fr_y_d = aux.y();
    fr_z_d = aux.z();
    
    return true;
}

/// Spherical (sensor) to world coordinate system.
inline bool    
CVelodyne::spherical2World  ( double       f_range_d,
                              double       f_azimuth_d,
                              double       f_elevation_d, 
                              C3DVector  &fr_point ) const
{
    C3DVector localPoint;
    
    if ( !spherical2Local ( f_range_d,
                            f_azimuth_d,
                            f_elevation_d, 
                            localPoint ) )
        return false;
    
    fr_point = m_rotation * localPoint + m_translation;

    return true;
}

/// Spherical (sensor) to world coordinate system.
inline bool    
CVelodyne::spherical2World  ( double       f_range_d,
                              double       f_azimuth_d, 
                              C3DVector   &fr_point ) const
{
    C3DVector localPoint;
    
    if ( !spherical2Local ( f_range_d,
                            f_azimuth_d,
                            localPoint ) )
        return false;

    fr_point = m_rotation * localPoint + m_translation;
 
    return true;    
}

/// Spherical (sensor) to local coordinate system.
inline bool
CVelodyne::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
{
    C3DVector aux;
   
    if ( !spherical2Local  ( f_range_d,
                             f_azimuth_d,
                             f_elevation_d, 
                             aux ) )
        return false;

    fr_x_d = aux.x();
    fr_y_d = aux.y();
    fr_z_d = aux.z();
    
    return true;
}


/// Spherical (sensor) to local coordinate system.
inline bool
CVelodyne::spherical2Local  ( double  f_range_d,
                              double  f_azimuth_d, 
                              double &fr_x_d,
                              double &fr_y_d,
                              double &fr_z_d ) const
{
    C3DVector aux;
   
    if ( !spherical2Local  ( f_range_d,
                             f_azimuth_d, 
                             aux ) )
        return false;
    
    fr_x_d = aux.x();
    fr_y_d = aux.y();
    fr_z_d = aux.z();
    
    return true;
}

/// Spherical (sensor) to local coordinate system.
inline bool
CVelodyne::spherical2Local  ( double       f_range_d,
                              double       f_azimuth_d,
                              double       f_elevation_d, 
                              C3DVector   &fr_point ) const
{
    const double cosAz_d = cos(f_azimuth_d);
    const double sinAz_d = sin(f_azimuth_d);
    const double cosElTimesRange_d = cos(f_elevation_d) * f_range_d;
    const double sinEl_d = sin(f_elevation_d);
    
    fr_point.setX (  cosElTimesRange_d * sinAz_d );
    fr_point.setY ( -f_range_d * sinEl_d );
    fr_point.setZ (  cosElTimesRange_d * cosAz_d );

    return true;
}

/// Spherical (sensor) to local coordinate system.
inline bool    
CVelodyne::spherical2Local  ( double       f_range_d,
                              double       f_azimuth_d, 
                              C3DVector   &fr_point ) const
{
    if ( m_sinFixedElevation_d  > 2.0 )
    {
        logger::warn( "Please call first setElevation to be able to compute local coordinates for a fixed elevation angle.");
        return false;        
    }
    
    const double cosAz_d = cos(f_azimuth_d);
    const double sinAz_d = sin(f_azimuth_d);
    const double cosElTimesRange_d = m_cosFixedElevation_d * f_range_d;
    
    fr_point.setX(  cosElTimesRange_d * sinAz_d );
    fr_point.setY( -f_range_d * m_sinFixedElevation_d );
    fr_point.setZ(  cosElTimesRange_d * cosAz_d );
 
    return true;    
}

inline bool
CVelodyne::spherical2Local  ( SSphericalPointData f_meas,
                              C3DVector          &fr_point ) const
{
    return spherical2Local  ( f_meas.range_d,
                              f_meas.azimuth_d,
                              f_meas.elevation_d,
                              fr_point );
}

/// Rotation.
inline C3DMatrix
CVelodyne::getRotation ( ) const
{
    return m_rotation;
}

inline bool
CVelodyne::setRotation ( const C3DMatrix & f_matrix )
{
    m_rotation    = f_matrix;
    m_invRotation = f_matrix.getInverse();
    return true;
}

inline C3DVector
CVelodyne::getTranslation ( ) const
{
    return m_translation;
}

/// Translation.
inline bool
CVelodyne::setTranslation ( const C3DVector & f_translation )
{
    m_translation = f_translation;
    return true;
}

/// World 2 local transformation.
inline bool
CVelodyne::world2Local ( const C3DVector  &f_worldPoint,
                         C3DVector        &fr_localPoint ) const
{
    fr_localPoint = m_invRotation * (f_worldPoint - m_translation);
    return true;
}

                                        

/// World 2 local transformation.
inline bool
CVelodyne::local2World ( const C3DVector  &f_localPoint,
                         C3DVector        &fr_worldPoint ) const
{
    fr_worldPoint = m_rotation * f_localPoint + m_translation;
    return true;
}


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