#ifndef __TESTOPERATOR_H
#define __TESTOPERATOR_H

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

/* INCLUDES */
#include "operator.h"
#include "image.h"
#include "uShortImage.h"
#include "math.h"
#include "stereoCamera.h"

/* PROTOTYPES */
class SGM;

/* CONSTANTS */

namespace VIC
{
    class CTestOperator: public VIC::COperator
    {
        /// Constructor, Desctructors
    public:    
        
        /// Constructors.
        CTestOperator ( COperator * const f_parent_p = NULL );
        
        /// Virtual destructor.
        virtual ~CTestOperator ();

        /// Cycle event.
        virtual bool cycle();
    
        /// Show event.
        virtual bool show();
    
        /// Init event.
        virtual bool initialize();
    
        /// Reset event.
        virtual bool reset();
    
        /// Exit event.
        virtual bool exit();

    /// Parameter set/get.
    public:
        bool setTestParam ( bool f_val_b ) { m_testParam_b = f_val_b; return true; }
        bool getTestParam ( ) const { return m_testParam_b; }
        
        bool   setDoubleParam ( double f_val_d ) { m_testParamDouble_d = f_val_d; return true; }
        double getDoubleParam ( ) const { return m_testParamDouble_d; }

        bool   setIntParam ( int f_val_i ) { m_testParamInt_i = f_val_i; return true; }
        int    getIntParam ( ) const { return m_testParamInt_i; }

        bool   setFloatParam ( float f_val_f ) { m_testParamFloat_f = f_val_f; return true; }
        float  getFloatParam ( ) const { return m_testParamFloat_f; }

    /// User Operation Events
    public:
        /// Key pressed in display.
        virtual void keyPressed (     CKeyEvent * f_event_p );

    public:
        template <class Type>
        bool normalizeImage ( Type *       f_data_p, 
                              Type         f_normVal,
                              unsigned int f_size_ui );
        
    protected:
        
    private:

        // Km Stereo Operator.
        CStereoCamera        m_camera;

        /// Sgm opereator.
        SGM *                m_sgmOp_p;

        /// U short images
        CUShortImage         m_leftImg;

        /// U short images
        CUShortImage         m_rightImg;
        
        
        //QImage *             m_tmpBuffImgLeft_p;

        //QImage *             m_tmpBuffImgRight_p;
        
        bool                   m_testParam_b;
        double                 m_testParamDouble_d;
        int                    m_testParamInt_i;
        float                  m_testParamFloat_f;

    };

    template <class Type>
    bool CTestOperator::normalizeImage ( Type *       f_data_p, 
                                         Type         f_normVal,
                                         unsigned int f_size_ui )
    {
        if (f_size_ui <= 0) return false;
        
        Type   maxVal;
        double sum_d;
        double sum2_d;
        
        Type * data_p = f_data_p;            
        const Type * const endPtr_p = f_data_p + f_size_ui;
        
        maxVal  = *data_p;
        sum_d   = *data_p++;
        sum2_d  = (double) sum_d * sum_d;

        while ( data_p != endPtr_p ) 
        {
            sum_d  += *data_p;
            sum2_d += ((double) *data_p) * (double) *data_p;
            
            //printf(">>>>>>> sum: %f sqsum: %f max: %f\n", sum_d, sum2_d, maxVal);

            if ( maxVal < *data_p )
                maxVal = *data_p;

            ++data_p;
        }

        double avg_d  = sum_d / f_size_ui;        
        double var_d  = sum2_d / (double) f_size_ui - avg_d * avg_d;
        double norm_d = (Type) (std::min (avg_d + 3 * sqrt(var_d), 
                                   (double)maxVal ));

        printf(">>>>>>> MAXIMUM VALUE OF IMAGE IS %i avg: %f std: %f normval: %f (attempt %f) \n", 
               maxVal, avg_d, sqrt(var_d), norm_d, avg_d + 3 * sqrt(var_d));

        data_p = f_data_p;
        
        while ( data_p != endPtr_p ) 
        {
            *data_p = (Type)(((double)*data_p)/norm_d * (double)f_normVal) ;
            ++data_p;
        }
            
        return true;
    }
}

#endif // __TESTOPERATOR_H
