/*@@@**************************************************************************
* @file  zkStereo.cpp
* \author Hernan Badino
* \date  Wed Apr  8 14:27:15 GMT 2009
* \notes 
*******************************************************************************
******************************************************************************/

/* INCLUDES */
#include "rigidMotionWidget.h"
#include "drawingList.h"

#include "logger.h"
#include "colorEncoding.h"

#include "zkStereo.h"
#include "zkStereo/h/s3dImage.h"
//#include "ippDefs.h"

using namespace VIC;

/// Constructors.
CZkStereoOp::CZkStereoOp ( COperator * const f_parent_p )
        : COperator ( f_parent_p, "ZkStereo" ),
          m_stereoOp (                       ),
          m_compute_b (                false ),
          m_minDisparity_i (            -100 ),
          m_maxDisparity_i (               1 ),
          m_winRadL0_ui (                 3u ),
          m_winRadRCLn_ui (               3u ),
          m_winRadD_ui (                  3u ),
          m_numIterations_ui (            5u ),
          m_maxScaler_f (               1.e4 ),
          m_useSAD_b (                  true ),
          m_minConfidence_f (            0.f )
{
    CDrawingList *  list_p;

    list_p = getDrawingList ("Disparity Image");
    list_p -> setPosition ( S2D<int> (2, 2) );
    list_p -> setVisibility ( false );

    ADD_BOOL_PARAMETER ( "Compute", 
                         "Compute ZKStereo?",
                         m_compute_b,
                         this,
                         Compute, 
                         CZkStereoOp );

    ADD_INT_PARAMETER ( "Min Disparity",
                        "Min disparity [px] (positive is to the right).",
                        m_minDisparity_i,
                        this,
                        MinDisparity,
                        CZkStereoOp );
    
    ADD_INT_PARAMETER ( "Max Disparity",
                        "Max disparity [px] (positive is to the right).",
                        m_maxDisparity_i,
                        this,
                        MaxDisparity,
                        CZkStereoOp );
    
    ADD_UINT_PARAMETER ( "Windows radius for L0",
                         "m_winRadL0_ui",
                         m_winRadL0_ui,
                         this,
                         WinRadL0,
                         CZkStereoOp );

    
    ADD_UINT_PARAMETER ( "Windows radius for row-col for Ln",
                         "Windows radius for computing Ln for the row and column [px]",
                         m_winRadRCLn_ui,
                         this,
                         WinRadRCLn,
                         CZkStereoOp );
    
    ADD_UINT_PARAMETER ( "Windows radius for disparity for Ln",
                         "Windows radius for computing Ln for the disparity [px]",
                         m_winRadD_ui,
                         this,
                         WinRadD,
                         CZkStereoOp );
    
    ADD_UINT_PARAMETER ( "Number of iterations",
                         "Number of iterations in which Ln is refined.",
                         m_numIterations_ui,
                         this,
                         NumIterations,
                         CZkStereoOp );
    
    ADD_FLOAT_PARAMETER ( "Max Scaler",
                          "Maximum value of the abs computation.",
                          m_maxScaler_f,
                          this,
                          MaxScaler,
                          CZkStereoOp );
    
    ADD_FLOAT_PARAMETER ( "Min Confidence",
                          "Minimum confidence of a found disparity to show it in the "
                          "final disparity image.",
                          m_minConfidence_f,
                          this,
                          MinConfidence,
                          CZkStereoOp );


    ADD_BOOL_PARAMETER ( "Use SAD",
                         "If not, (non-normalized) cross correlation is used [px]",
                         m_useSAD_b,
                         this,
                         UseSAD,
                         CZkStereoOp );
    

    addDrawingListParameter ("Disparity Image");
}

/// Virtual destructor.
CZkStereoOp::~CZkStereoOp ()
{
}

/// Init event.
bool CZkStereoOp::initialize()
{
    return COperator::initialize();
}

/// Reset event.
bool CZkStereoOp::reset()
{
    logger::msg("Reset called");
    return COperator::reset();
}

bool CZkStereoOp::exit()
{
    return COperator::exit();
}

/// Cycle event.
bool CZkStereoOp::cycle()
{
    if ( m_compute_b )
    {
        float scaleFactor_f = getCastedInputObject<CIO_float, float> ("Rectification Scale Factor", 1.f );
        /// Register float image outputs.
        CFloatImage *  lfImg_p = dynamic_cast<CFloatImage *>(getInput ( "Rectified Float Left Image" ) );
        CFloatImage *  rfImg_p = dynamic_cast<CFloatImage *>(getInput ( "Rectified Float Right Image" ) );
        
        CS3DImage lImg ( lfImg_p -> getWidth(), 
                         lfImg_p -> getHeight(), 
                         1, 
                         lfImg_p -> getData() );
        
        
        CS3DImage rImg ( rfImg_p -> getWidth(), 
                         rfImg_p -> getHeight(), 
                         1, 
                         rfImg_p -> getData() );
        
        printf("Calling FindDisparityMap ...\n");
        
        m_stereoOp.FindDisparityMap ( &lImg, &rImg,
                                      m_minDisparity_i * scaleFactor_f, m_maxDisparity_i, 
                                      m_winRadL0_ui, m_winRadRCLn_ui, m_winRadD_ui, 
                                      m_numIterations_ui,
                                      m_maxScaler_f,
                                      m_minConfidence_f,
                                      m_useSAD_b );
        
        CS3DImage * res_p = m_stereoOp.getDisplayDisparityImage();
        //CS3DImage * res_p = m_stereoOp.getDisparityImage();
        
        CFloatImage dispImg;
        
        dispImg.setSize ( lfImg_p -> getWidth(),
                          lfImg_p -> getHeight() );
        
        dispImg.setData ( res_p -> getPointer ( 0, 0, 0 ) );
        
        m_dispImg.copyFrom ( dispImg, true );
        
        //m_stereoOp.PrintDisparityValues ("test.dat");
    }
    
    return COperator::cycle();
}

/// Show event.
bool CZkStereoOp::show()
{    
    CDrawingList *  list_p;
    float scaleFactor_f = getCastedInputObject<CIO_float, float> ("Rectification Scale Factor", 1.f );

    float scale_f = 1./(100*scaleFactor_f);

    list_p = getDrawingList ("Disparity Image");
    list_p -> clear();
    list_p -> addImage ( m_dispImg, 0, 0, 800, 600, 1/256. );

    return COperator::show();
}

void 
CZkStereoOp::keyPressed ( CKeyEvent * f_event_p )
{
   return COperator::keyPressed ( f_event_p );    
}

