#ifndef __NEWSTEREOOP_H
#define __NEWSTEREOOP_H

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

/* INCLUDES */
#include <math.h>
#include <limits>

#include "operator.h"
#include "uShortImage.h"
#include "image.h"
#include "logger.h"
#include "stereoCamera.h"
#include "gaussianPyramid.h"

/* PROTOTYPES */

/* CONSTANTS */

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

        /// 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();

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

    /// Gets and sets.
    public:

//         ADD_PARAM_ACCESS(bool,        m_saveFiles_b,     SaveFiles )
//         ADD_PARAM_ACCESS(std::string, m_lutFilePath_str, LutFilePath )
//         ADD_PARAM_ACCESS(int,         m_cutTopRows_i,    CutTopRows );
//         ADD_PARAM_ACCESS(int,         m_cutBotRows_i,    CutBotRows )
//         ADD_PARAM_ACCESS(bool,        m_rectify_b,       Rectify );        
//         ADD_PARAM_ACCESS(float,       m_offsetU_f,       OffsetU );

    protected:
        
    private:

        /// Scale to apply to the output images.
        int                  m_downscale_i;

        /// Should the rectification be applied?
        bool                 m_rectify_b;
        
        /// Scale to apply to the output images.
        CStereoCamera        m_rectifiedCamera;

        /// Rectified left image.
        CUShortImage         m_eqLImg;

        /// Rectified right image.
        CUShortImage         m_eqRImg;

        /// Rectified left image.
        CUShortImage         m_rectLImg;

        /// Rectified right image.
        CUShortImage         m_rectRImg;

        /// Float-typed left image.
        CFloatImage          m_fltLImgSrc;

        /// Float-typed right image.
        CFloatImage          m_fltRImgSrc;

        /// Float-typed left image.
        CFloatImage          m_fltLImgDst;

        /// Float-typed right image.
        CFloatImage          m_fltRImgDst;

         /// U LUT for left.
        CImage               m_lutLU;

         /// V LUT for left.
        CImage               m_lutLV;

         /// U LUT for right.
        CImage               m_lutRU;

         /// V LUT for right.
        CImage               m_lutRV;

        /// Save files?
        bool                 m_saveFiles_b;

        /// Save files?
        std::string          m_lutFilePath_str;

        /// Cut top rows.
        int                  m_cutTopRows_i;

        /// Cut bottom rows.
        int                  m_cutBotRows_i;

        /// Gaussian pyramid in case no rectification is applied.
        CGaussianPyramid<float>
                             m_gaussPyramidLeft;

        /// Gaussian pyramid in case no rectification is applied.
        CGaussianPyramid<float>
                             m_gaussPyramidRight;

        /// Lateral rectification offset 
        int                  m_offsetU_f;
   };

    /// Mean and variance equalization of the images.
    template <class Type>
    bool CNewStereoOp::equalizeImages ( Type *       f_left_p, 
                                        Type *       f_right_p, 
                                        unsigned int f_size_ui )
    {
        if (f_size_ui <= 0) return false;
        
        Type   maxVal[2];
        double sum_d[2];
        double sum2_d[2];
        double avg_d[2];
        double var_d[2];
        //double norm_d[2];
        Type *  data_p, * endPtr_p;
        
        for (int i = 0; i < 2; ++i)
        {
            Type * data_p = i==0?f_left_p:f_right_p;
            const Type * const endPtr_p = data_p + f_size_ui;
            
            maxVal[i]  = *data_p;
            sum_d[i]   = *data_p++;
            sum2_d[i]  = (double) sum_d[i] * sum_d[i];
            
            while ( data_p != endPtr_p ) 
            {
                sum_d[i]  += *data_p;
                sum2_d[i] += ((double) *data_p) * (double) *data_p;
                
                //printf(">>>>>>> sum: %f sqsum: %f max: %f\n", sum_d, sum2_d, maxVal);
                
                if ( maxVal[i] < *data_p )
                    maxVal[i] = *data_p;
                
                ++data_p;
            }
            
            avg_d[i]  = sum_d[i] / f_size_ui;        
            var_d[i]  = sum2_d[i] / (double) f_size_ui - avg_d[i] * avg_d[i];

            if (var_d[i] <= 0)
            {
                logger::warn("The equalization is not possible because variance is <= 0");
                return false;
            }

            printf("Distribution for image %i is %f %f\n",
                   i, avg_d[i], var_d[i]);
        }        

        float scale_f = sqrt(var_d[0] / var_d[1]);
        float delta_f;
        
        /// Check if it has sense to apply an equalization.
        if ( fabs(scale_f - 1.f) < 1.e-4 &&
             fabs(scale_f * avg_d[1] / avg_d[0] - 1) < 1.e-4 )
            return true;

        if (scale_f < 1)
        {
            data_p  = f_left_p;
            scale_f = 1.f/scale_f;
            delta_f = avg_d[1] - avg_d[0] * scale_f;
            printf("Applying correction to left image scale %f offset %f\n",
                   scale_f, delta_f );
        }
        else
        {
            data_p  = f_right_p;
            delta_f = avg_d[0] - avg_d[1] * scale_f;
            printf("Applying correction to right image scale %f offset %f\n",
                   scale_f, delta_f );
      }

        endPtr_p = data_p + f_size_ui;
        
        while ( data_p != endPtr_p ) 
        {
            *data_p = (Type) (
                    std::min ( 
                            std::max ( 
                                    ((float)*data_p)*scale_f + delta_f, 
                                    0.f ),
                            (float)(std::numeric_limits<Type>::max() ) ) );
            ++data_p;
        }
        
        return true;
    }
}

#endif // __NEWSTEREOOP_H
