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

/* INCLUDES */
#include <QWidget>


#include <QFile>
#include <QXmlStreamWriter>

#include "testOperator.h"

#include "logger.h"
#include "drawingList.h"

#include "rectifierOp.h"
#include "rangePredictor.h"
#include "normalPredictor.h"
#include "velodyneBase.h"
#include "velodyneReader.h"
#include "velodyneViewer.h"
#include "kmStereoOp.h"
//#include "zkStereo.h"
#include "normalEstFromSri.h"

#include "paramIOFile.h"

#include "paramBaseConnector.h"

#include "paramMacros.h"
/// SGM
//#include "SGM.h"


using namespace VIC;

/// Constructors.
CTestOperator::CTestOperator ( COperator * const f_parent_p )
        : COperator ( f_parent_p, "Test Operator" ),
          m_camera (                              ),
          m_testParam_b (                    true ),
          m_testParamDouble_d ( -3.1415926 * 100. ),
          m_testParamInt_i (                 -123 ),
          m_testParamFloat_f (             -456.f )
          //m_sgmOp_p (                        NULL ),
          //m_tmpBuffImgLeft_p(                NULL ),
          //m_tmpBuffImgRight_p (              NULL )
{
    logger::msg("Constructor called");

    if (f_parent_p == NULL)
    {
        addChild ( new CRectifierOp        ( this ) );
        addChild ( new CVelodyneReaderOp   ( this ) );
        addChild ( new CVelodyneBaseOp     ( this ) );
        addChild ( new CRangePredictorOp   ( this ) );
        addChild ( new CNormalPredictorOp  ( this ) );
        addChild ( new CKmStereoOp         ( this ) );
        //addChild ( new CZkStereoOp         ( this ) );
        addChild ( new CVelodyneViewerOp   ( this ) );
        //addChild ( new CNewOperator ( this ) );
        //addChild ( new CNewOperator ( this ) );
        //addChild ( new CNormalEstFromSriOp ( this ) );
    }

    CDrawingList *  list_p;
    list_p = getDrawingList ("Raw Left Image");
    list_p -> setPosition ( S2D<int> (0, 0) );
    list_p -> setVisibility ( false );

    list_p = getDrawingList ("Raw Right Image");
    list_p -> setPosition ( S2D<int> (1, 0) );
    list_p -> setVisibility ( false );

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

    
    BEGIN_PARAMETER_GROUP("Testing group name", true, CColor::red );
    
    
    ADD_BOOL_PARAMETER ( "Testing other bool param", 
                         "nice tooltip", 
                         m_testParam_b,
                         this,
                         TestParam, 
                         CTestOperator );
    
    ADD_DOUBLE_PARAMETER ( "Testing a double param", 
                           "another nice tooltip", 
                           m_testParamDouble_d,
                           this,
                           DoubleParam, 
                           CTestOperator );
    
    ADD_FLOAT_PARAMETER ( "Testing a float param", 
                           "another nice tooltip", 
                          m_testParamFloat_f,
                          this,
                          FloatParam, 
                          CTestOperator );
    /*  
    ADD_INT_PARAMETER ( "A integer parameter", 
                        "another nice tooltip", 
                        m_testParamInt_i,
                        this,
                        IntParam, 
                        CTestOperator );
    */
    END_PARAMETER_GROUP;    

    //m_sgmOp_p -> initDataStructs(false);
    //m_sgmOp_p = (SGM*) sgm_setup ( HEIGHT, WIDTH, MIN_DISP, MAX_DISP, false);
    
    printf("Loading parameters\n");
    
    CParamIOFile piofile("parameters.xml");   
    piofile.load("parameters.xml");
    m_paramSet_p->load ( piofile );
    printf("End loading parameters\n");

    
}

/// Virtual destructor.
CTestOperator::~CTestOperator ()
{
    printf("Saving parameters\n");
    CParamIOFile piofile("");   
    m_paramSet_p->save ( piofile );
    piofile.save ("parameters.xml");
}

/// Cycle event.
bool CTestOperator::cycle()
{
    /// 
    char inputId_p [2][8] = {"Image 0", "Image 1"};

    CImage * img_p[2];

    for (int i = 0; i < 2; ++i)
    {    
        /// Obtain input from parent.
        img_p[i] = dynamic_cast<CImage *>(getInput ( inputId_p[i] ) );
         
         if (img_p[i] && 0)
         {
             if ( img_p[i]->getBytesPerPixel() == 1 )
                 normalizeImage<unsigned char> ( (unsigned char *) img_p[i] -> getData(), 
                                                 255,
                                                 img_p[i] -> getWidth() * img_p[i] -> getHeight() );
             else
                 normalizeImage<unsigned short int> ( (unsigned short int *) img_p[i] -> getData(), 
                                                      i==0?65535:65535,
                                                      img_p[i] -> getWidth() * img_p[i] -> getHeight() );
         }
    }

    
    //m_leftImg.freeMemory();     
    m_leftImg.setSize( img_p[0]->getSize() );
    m_leftImg.setData( (unsigned short int *) img_p[0]->getData() );

    m_rightImg.setSize( img_p[1]->getSize() );
    m_rightImg.setData( (unsigned short int *) img_p[1]->getData() );

    
    registerOutput ( "Image Left",  &m_leftImg );
    registerOutput ( "Image Right", &m_rightImg );

    registerOutput ( "Camera", &m_camera );

    registerOutput ( "NULL", NULL );

    bool success_b = COperator::cycle();


    
    //registerOutput ( "Rectified Left Image",  getChild (0) -> getOutput ("Rectified Left Image") );
    //registerOutput ( "Rectified Right Image", getChild (0) -> getOutput ("Rectified Right Image") );

    //registerOutput ( "Rectified Left Image",  getInput ( "Image 0" ) );
    //registerOutput ( "Rectified Right Image", getInput ( "Image 1" ) );

    /*
    m_sgmOp_p -> initTodos(false, true);
    ResultPixel * res_p[2];

    sgm_stereo ( m_sgmOp_p, 
                 (char*)img_p[0] -> getData(), 
                 (char*)img_p[1] -> getData(), 
                 &res_p[0],
                 &res_p[1],
                 false,
                 false,
                 false,
                 2,
                 10,
                 0,
                 1.e7,
                 Regular,
                 2.e7 );

    printf("pointer to result is %i\n", res_p[0] );
    
    //m_sgmOp_p -> set_img ( Left,  (RGB *) img_p[0] -> getData() );    
    //m_sgmOp_p -> set_img ( Right, (RGB *) img_p[1] -> getData() );    

    uint8_t * data_p = (uint8_t *) m_leftRes.getData();
    ResultPixel * result_p = res_p[0];

    for (unsigned int i = 0; i < img_p[0] -> getHeight(); ++i)
    {
        for (unsigned int j = 0; j < img_p[0] -> getWidth(); ++j, ++data_p, ++result_p)
        {
            *data_p = result_p->consistent?(uint8_t) std::min(std::max(-result_p->subpixel_depth, 0.f), 255.f):0;
            printf("%f\n", result_p->subpixel_depth);
            
        
        }
    }
    */


    /// Register outputs.
    
    return success_b;
}

/// Show event.
bool CTestOperator::show()
{
    CDrawingList *  list_p;

    /// Get input from parent.
    //CImage * imgL_p = dynamic_cast<CImage *>(getInput ( "Image 0" ) );
    //CImage * imgR_p = dynamic_cast<CImage *>(getInput ( "Image 1" ) );

    list_p = getDrawingList ("Raw Left Image");
    list_p -> clear();

    float imgWidth_f  = 800;
    float imgHeight_f = 600;

    //// Draw left and right images (if visible).
    if ( list_p -> isVisible() )
    {
        list_p -> addImage ( m_leftImg, 0, 0, imgWidth_f, imgHeight_f );
    }
    
    list_p = getDrawingList ("Raw Right Image");
    list_p -> clear();

    if ( list_p -> isVisible() )
    {
        list_p -> addImage ( m_rightImg, 0, 0, imgWidth_f, imgHeight_f );
    }
    
#if defined HAVE_QGLVIEWER
        m_3dViewer_p->clear();
#endif

    return COperator::show();
}

/// Init event.
bool CTestOperator::initialize()
{
    std::string  filePath0_str = getCastedInputObject<CIOString, std::string> ("Image 0 Path", "");

    int pos_i = filePath0_str.find_last_of ("/\\");

    if ( pos_i != -1 )
        filePath0_str.erase(pos_i);
    else
        filePath0_str = std::string("."); 

    filePath0_str += "/camera.xml";

    bool fileFound_b = false;

    CParamIOFile paramFile;

    printf("Trying to open parameter file from \"%s\"\n",
           filePath0_str.c_str() );
    
    /// Try to load camera file.
    if (!paramFile.load( filePath0_str ))
    {    
        printf("NOT Found. NOW trying from local directory\n" );
       /// If does not exist, try to open camera.xml in the current directory.
        if (paramFile.load ("./camera.xml"))
            fileFound_b = true;
    }       
    else
        fileFound_b = true;
    
    if (fileFound_b)
    {
        paramFile.setCurrentCategory ( "Rectified Camera" );
        m_camera.load ( paramFile );
    }
    else
    {
        logger::warn("camera.xml file could not be found\n");
    }

    registerOutput ( "Camera", &m_camera );

    ///////////// left and right image.
    
    char inputId_p [2][8] = {"Image 0", "Image 1"};

    CImage * img_p[2];

    for (int i = 0; i < 2; ++i)
        img_p[i] = dynamic_cast<CImage *>(getInput ( inputId_p[i] ) );

    m_leftImg.setSize( img_p[0]->getSize() );
    m_leftImg.setData( (unsigned short int *) img_p[0]->getData() );

    m_rightImg.setSize( img_p[1]->getSize() );
    m_rightImg.setData( (unsigned short int *) img_p[1]->getData() );

    registerOutput ( "Image Left",  &m_leftImg );
    registerOutput ( "Image Right", &m_rightImg );

    return COperator::initialize();
}

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

bool CTestOperator::exit()
{
    logger::msg("Exit called");
    return COperator::exit();
}

void 
CTestOperator::keyPressed (     CKeyEvent * f_event_p )
{
    QKeyEvent *keyEvent_p = f_event_p -> qtKeyEvent_p;
    
    int          imgNr_i       = getCastedInputObject<CIO_ui32, int>          ("Frame Number", -1);
    std::string  filePath0_str = getCastedInputObject<CIOString, std::string> ("Image 0 Path", "");
    std::string  filePath1_str = getCastedInputObject<CIOString, std::string> ("Image 1 Path", "");

    //printf("Key %i was pressed\n", keyEvent_p -> key());

    if (keyEvent_p -> key() == Qt::Key_O)
    {
        printf("'O' was pressed on image %i with locations \"%s\" and \"%s\"\n",
               imgNr_i, filePath0_str.c_str(), filePath1_str.c_str() );
        
        char str[1024];

        sprintf(str, "cp \"%s\" /tmp/calib_20090601/", filePath0_str.c_str());
        system(str);
        sprintf(str, "cp \"%s\" /tmp/calib_20090601/", filePath1_str.c_str());
        system(str);
        
    }

    if (keyEvent_p -> key() == Qt::Key_D)
    {
        printf("'D' was pressed on image %i with locations \"%s\" and \"%s\"\n",
               imgNr_i, filePath0_str.c_str(), filePath1_str.c_str() );
        
        char str[1024];

        sprintf(str, "rm -f \"%s\"", filePath0_str.c_str());
        system(str);
        sprintf(str, "cp \"%s\"", filePath1_str.c_str());
        system(str);
        
    }
    return COperator::keyPressed( f_event_p );
}

