/*@@@**************************************************************************
 * \file  camera
 * \author Hernan Badino
 * \date  Wed Jun 10 18:56:01 EDT 2009
 * \notes 
 *******************************************************************************
 ******************************************************************************/

/* INCLUDES */
#include "doubleParam.h"
#include "camera.h"

using namespace VIC;

CCamera::CCamera ( ) 
        : m_focalLength_d (         0 ),
          m_fu_d (                  0 ),
          m_fv_d (                  0 ),
          m_u0_d (                  0 ),
          m_v0_d (                  0 )
{

}

CCamera::CCamera ( const CParamIOHandling & fr_paraReader )
        : m_focalLength_d (         0 ),
          m_fu_d (                  0 ),
          m_fv_d (                  0 ),
          m_u0_d (                  0 ),
          m_v0_d (                  0 )
{
    load ( fr_paraReader );
}

CCamera::~CCamera()
{

}
        
bool
CCamera::load ( const CParamIOHandling & fr_paraReader )
{
    std::string value_str;
    CDoubleParameter paraDouble;
    double sx=1.f, sy=1.f;

    if ( fr_paraReader.get ( "Focal Length", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        m_focalLength_d = paraDouble.getValue();
    
    if ( fr_paraReader.get ( "Pixel Width", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        sx = paraDouble.getValue();

    if ( fr_paraReader.get ( "Pixel Height", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        sy = paraDouble.getValue();
    
    if ( fr_paraReader.get ( "Center X", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        m_u0_d = paraDouble.getValue();

    if ( fr_paraReader.get ( "Center Y", value_str ) && 
         paraDouble.setValueFromString ( value_str ))
        m_v0_d = paraDouble.getValue();
    
    m_fu_d = m_focalLength_d/sx;
    m_fv_d = m_focalLength_d/sy;
    
    printf("Camera params: %f %f %f %f %f\n", m_fu_d, m_fv_d, m_focalLength_d, m_u0_d, m_v0_d);
    
    return true;
}

/// Focal length U.
void
CCamera::setFu( double f_fu_d )
{
    m_fu_d = f_fu_d;
}

double
CCamera::getFu() const
{
    return m_fu_d;
}

/// Focal length V.
void
CCamera::setFv(double f_fv_d)
{
    m_fv_d = f_fv_d;
}

double
CCamera::getFv() const
{
    return m_fv_d;
}

/// Principal Point U
void
CCamera::setU0( double f_u0_d )
{
    m_u0_d = f_u0_d;
}

double
CCamera::getU0() const
{
    return m_u0_d;
}

/// Principal Point V
void
CCamera::setV0( double f_v0_d )
{
    m_v0_d = f_v0_d;
}

double
CCamera::getV0() const
{
    return m_v0_d;
}

/// Principal Point
void
CCamera::setPrincipalPoint( double f_v0_d, 
                            double f_u0_d )
{
    m_u0_d = f_u0_d;
    m_v0_d = f_v0_d;
}
        
/// World 3D point to image point.
bool
CCamera::world2Image ( C3DVector  f_point,
                       double    &fr_u_d,
                       double    &fr_v_d ) const
{
    return world2Image ( f_point.x(), 
                         f_point.y(), 
                         f_point.z(), 
                         fr_u_d,
                         fr_v_d );
}


/// World 3D point to image point.
bool
CCamera::world2Image ( double  f_x_d, 
                       double  f_y_d,
                       double  f_z_d,
                       double &fr_u_d,
                       double &fr_v_d ) const
{
    fr_u_d = f_x_d / f_z_d * m_fu_d + m_u0_d;
    fr_v_d = m_v0_d - f_y_d / f_z_d * m_fv_d;

    return true;
}

/// Image position to world position.
bool
CCamera::imageAndDistance2World ( double  f_u_d, 
                                  double  f_v_d,
                                  double  f_z_d,
                                  double &fr_x_d,
                                  double &fr_y_d ) const
{
    fr_x_d = (f_u_d - m_u0_d) / m_fu_d * f_z_d;
    fr_y_d = (m_v0_d - f_v_d) / m_fv_d * f_z_d;

    return true;
}

/// Image position to world position.
void
CCamera::scale ( double f_scale_d )
{
    //m_focalLength_d *= f_scale_d;
    m_fu_d          *= f_scale_d;
    m_fv_d          *= f_scale_d;
    m_u0_d          *= f_scale_d;
    m_v0_d          *= f_scale_d;
}

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