#ifndef __KMSTEREOOP_H
#define __KMSTEREOOP_H

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

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

#include "operator.h"
#include "logger.h"
#include "image.h"
#include "3DVectorImage.h"

//#include "fuzzy_icp.h"

#include "dsi.h"

#include "pyrDPStereo.h"

/* PROTOTYPES */
class kmStereo;

/* CONSTANTS */

namespace VIC
{
    class CKmStereoOp: public VIC::COperator
    {
    /// Private data type.
    private:
        
        typedef CDispSpaceImageOp<unsigned short int, float> CDsiOpFloat_t;

    /// Public data type
    public:    
        typedef enum
        {
            MFM_3x3,
            MFM_5x5
        } EMedianFilterMode_t;

        /// Constructor, Desctructors
    public:    
        
        /// Constructors.
        CKmStereoOp ( COperator * const f_parent_p = NULL );
        
        /// Virtual destructor.
        virtual ~CKmStereoOp ();

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

        /// Mouse moved.
        virtual void mouseMoved (     CMouseEvent * f_event_p );

    /// Gets and sets.
    public:
        ADD_PARAM_ACCESS ( float,         m_distCost_f,          DistCost );
        ADD_PARAM_ACCESS ( float,         m_distThreshold_f,     DistThreshold );
        ADD_PARAM_ACCESS ( float,         m_predCost_f,          PredCost );
        ADD_PARAM_ACCESS ( float,         m_predThreshold_f,     PredThreshold );
        ADD_PARAM_ACCESS ( float,         m_predCost4Prop_f,     PredCost4Prop );
        ADD_PARAM_ACCESS ( float,         m_predThreshold4Prop_f,PredThreshold4Prop );
        ADD_PARAM_ACCESS ( float,         m_initialCost_f,       InitialCost );
        ADD_PARAM_ACCESS ( float,         m_minCostValue_f,      MinCostValue );
        ADD_PARAM_ACCESS ( float,         m_maxCostValue_f,      MaxCostValue );
        ADD_PARAM_ACCESS ( bool,          m_applyMedian_b,       ApplyMedian );

        ADD_PARAM_ACCESS ( bool,          m_useGradientPred_b,   UseGradientPred );
        ADD_PARAM_ACCESS ( unsigned int,  m_mfKernelSize_ui,     MfKernelSize );
        ADD_PARAM_ACCESS ( bool,          m_followPathPrevRow_b, FollowPathPrevRow );
        ADD_PARAM_ACCESS ( bool,          m_predPathPrevRow_b,   PredPathPrevRow );
        ADD_PARAM_ACCESS ( unsigned int,  m_pathTolerance_ui,    PathTolerance );

        ADD_PARAM_ACCESS ( bool,          m_normalizeZssd_b,     NormalizeZssd );

        ADD_PARAM_ACCESS_NOTIFIER ( unsigned int,  m_zssdKernelWidth_ui,  ZssdKernelWidth,  reallocDsi );
        ADD_PARAM_ACCESS_NOTIFIER ( unsigned int,  m_zssdKernelHeight_ui, ZssdKernelHeight, reallocDsi );

        ADD_PARAM_ACCESS ( unsigned int,  m_maxLevel_ui,         MaxLevel );


        ADD_PARAM_ACCESS ( bool,   m_computeWTA_b,             ComputeWTA );
        ADD_PARAM_ACCESS ( bool,   m_computeWTAwithBounds_b,   ComputeWAwithBounds );
        ADD_PARAM_ACCESS ( bool,   m_computeDP_b,              ComputeDP );
        ADD_PARAM_ACCESS ( bool,   m_computeDPwithPred_b,      ComputeDPwithPred );
        ADD_PARAM_ACCESS ( bool,   m_computeDPwithBounds_b,    ComputeDPwithBounds );
        ADD_PARAM_ACCESS ( bool,   m_computePyrDP_b,           ComputePyrDP );
        ADD_PARAM_ACCESS ( bool,   m_computePyrDPwithBounds_b, ComputePyrDPwithBounds );

        ADD_PARAM_ACCESS ( bool,   m_computeVDP_b,             ComputeVDP );

        /// Scale factor for displaying slices of dsi.
        ADD_PARAM_ACCESS ( bool,   m_updateOptDsi_b,           UpdateOptDsi );

        ADD_PARAM_ACCESS ( double, m_dsiSliceScale_d,          DsiSliceScale );
        ADD_PARAM_ACCESS ( double, m_dsiSliceOffset_d,         DsiSliceOffset );
        ADD_PARAM_ACCESS ( bool,   m_showOptDsi_b,             ShowOptDsi );
        ADD_PARAM_ACCESS ( bool,   m_refineDispMap_b,          RefineDispMap );

        ADD_PARAM_ACCESS ( uint32_t,  m_deltaDisp_ui,          DeltaDisp );

        ADD_PARAM_ACCESS ( bool,      m_useRRI_b,              UseRRI );

        ADD_PARAM_ACCESS ( bool,      m_preApplyMedianFilter_b,  PreApplyMedianFilter );
        ADD_PARAM_ACCESS ( bool,      m_postApplyMedianFilter_b, PostApplyMedianFilter );
        ADD_PARAM_ACCESS ( EMedianFilterMode_t, 
                                      m_medianFilterMode_e,      MedianFilterMode );
        ADD_PARAM_ACCESS ( bool,      m_checkMinDsi_b,           CheckMinDsi );
        ADD_PARAM_ACCESS ( bool,      m_useOptDsi_b,             UseOptDsi );
        ADD_PARAM_ACCESS ( float,     m_maxDispDiff_f,           MaxDispDiff );

        ADD_PARAM_ACCESS ( bool,      m_computeResiduum_b,       ComputeResiduum );

        ADD_PARAM_ACCESS ( float,     m_displayResiduumNorm_f,  DisplayResiduumNorm );
 
        ADD_PARAM_ACCESS ( bool,      m_show3DPoints_b,         Show3DPoints );
        ADD_PARAM_ACCESS ( bool,      m_show3DMesh_b,           Show3DMesh );
        ADD_PARAM_ACCESS ( float,     m_maxDist4Mesh_f,         MaxDist4Mesh );
        ADD_PARAM_ACCESS ( float,     m_maxDisp4Mesh_f,         MaxDisp4Mesh );
        

   protected:
        
        void testDSI();
        void reallocDsi();

        void computeWTA();
        void computeWTAwithBounds();
        void computeDP();
        void computeDPWithPrediction();
        void computeVerticalDP();
        void computeDPwithBounds( CFloatImage         &fr_dispImg,
                                  const CDsiOpFloat_t &f_dsi,
                                  const std::string    f_idMinDisp_str,
                                  const std::string    f_idMaxDisp_str,
                                  const std::string    f_idDispPred_str,
                                  const std::string    f_idFirstDisp_str );
        void computePyrDP();
        void computePyrDPwithBounds();
        
        void updateOptDsi ( const int f_row_i, 
                            const CDynamicProgrammingOp::CNodeImage & f_graphImg);

        void refineDispMap ( CFloatImage         &fr_dispImg,
                             const CDsiOpFloat_t &f_dsi );

        void show3D();
        
        void postProcess ( CFloatImage &fr_dispImg );

        void remapDsi();

        void doLR_RL_Check( CFloatImage       &fr_lDispImg,
                            const CFloatImage &f_rDispImg );

        void computeResiduum ( const CFloatImage & f_dispImg);

   private:


        /// DSI for storing ZSSD values.
        CDsiOpFloat_t              m_dsi;

        /// DSI for storing the result of optimization.
        CDsiOpFloat_t              m_optDsi;

        /// DSI for computing stereo from left to right.
        CDsiOpFloat_t              m_rightDsi;

        /// Km Stereo Operator.
        kmStereo *                 m_kmStereoOp_p;

        /// Testing result of sgm.
        CFloatImage                m_leftRes;

        /// Maximum disparity.
        int                        m_maxDisparity_i;

        //// ZSSD Kernel width.
        unsigned int               m_zssdKernelWidth_ui;

        //// ZSSD Kernel height.
        unsigned int               m_zssdKernelHeight_ui;

        //// Normalized zero=mean sum of square differences?
        bool                       m_normalizeZssd_b;

        /// Max levels.
        unsigned int               m_maxLevel_ui;

        /// Distance cost.
        float                      m_distCost_f;

        /// Distance threshold.
        float                      m_distThreshold_f;

        /// Prediction cost.
        float                      m_predCost_f;

        /// Prediction cost.
        float                      m_predThreshold_f;

        /// Prediction cost for propagation strategy.
        float                      m_predCost4Prop_f;

        /// Prediction th for propagation strategy.
        float                      m_predThreshold4Prop_f;

        /// Initial cost.
        float                      m_initialCost_f;
        
        /// Min cost value.
        float                      m_minCostValue_f;

        /// Max Cost value.
        float                      m_maxCostValue_f;

        /// Apply median filter?
        bool                       m_applyMedian_b;

        /// Use gradient information?
        bool                       m_useGradientPred_b;

        /// Initial cost.
        unsigned int               m_mfKernelSize_ui;

        /// Follow path from result of previous row?
        bool                       m_followPathPrevRow_b;
        
        /// Predict result from previous row?
        bool                       m_predPathPrevRow_b;

        /// Follow path tolerance
        unsigned int               m_pathTolerance_ui;

        /// Delta disparity to add to range for bounded versions.
        unsigned int               m_deltaDisp_ui;

        /// Computation of WTA 
        bool                       m_computeWTA_b;

        /// result of WTA
        CFloatImage                m_dispImgWTA;

        /// Computation of WTA with bounds
        bool                       m_computeWTAwithBounds_b;
        
        /// result of WTA
        CFloatImage                m_dispImgWTAwithBounds;
        
        /// Computation of DP.
        bool                       m_computeDP_b;

        /// Computation of DP.
        bool                       m_computeDPwithPred_b;

        /// Computation of vertical DP.
        bool                       m_computeVDP_b;

        /// result of DP.
        CFloatImage                m_dispImgDP;

        /// result of DP.
        CFloatImage                m_dispImgDPwithPred;

        /// result of DP.
        CFloatImage                m_dispImgVDP;

        /// Computation of DP with bounds.
        bool                       m_computeDPwithBounds_b;

        /// result of DP with bounds.
        CFloatImage                m_dispImgDPwithBounds;

        /// Computation of pyramidal DP.
        bool                       m_computePyrDP_b;

        /// result of DP.
        CFloatImage                m_dispImgPyrDP;

        /// Computation of pyramidal DP with bounds.
        bool                       m_computePyrDPwithBounds_b;

        /// result of DP.
        CFloatImage                m_dispImgPyrDPwithBounds;

        /// result of DP.
        CFloatImage                m_auxDispMap;

        /// Update optimized dsi costs?
        bool                       m_updateOptDsi_b;

        /// Pyramidal Stereo operator.
        CPyramidalDynProgStereo    m_stereoOp;

        /// Max threads.
        const int                  m_numThreads_ci;

        /// Scale factor for displaying slices of dsi.
        double                     m_dsiSliceScale_d;

        /// Offset for displaying slices of dsi.
        double                     m_dsiSliceOffset_d;
        
        /// Show optimized or standard DSI?
        double                     m_showOptDsi_b;

        /// Refine the disparity map?
        bool                       m_refineDispMap_b;
        
        /// Pre-apply MF?
        bool                       m_preApplyMedianFilter_b;

        /// Post-apply MF?
        bool                       m_postApplyMedianFilter_b;

        /// MF mode.
        EMedianFilterMode_t        m_medianFilterMode_e;

        /// Post-apply MF?
        bool                       m_checkMinDsi_b;

        /// Use Optimized DSI?
        bool                       m_useOptDsi_b;

        /// Max Disp difference in LR-RL consistency check.
        float                      m_maxDispDiff_f;

        /// Use remapped right image?
        bool                       m_useRRI_b;

        /// Use remapped right image?
        bool                       m_computeResiduum_b;

        /// Residuum disparity image.
        CFloatImage                m_dispImgResiduum;

        /// Scale factor for displaying residuum image.
        float                      m_displayResiduumNorm_f;

        /// 3D Vector Image 4 3D Display.
        C3DVectorImage             m_vectorImg;

        /// Show 3D points.
        bool                       m_show3DPoints_b;
        
        /// Show 3D Mesh
        bool                       m_show3DMesh_b;
        
        /// Max distance for showing triangles in mesh.
        float                      m_maxDist4Mesh_f;
        
        /// Max disparity distance for showing triangles in mesh.
        float                      m_maxDisp4Mesh_f;
    };

}

#endif // __RECTIFIEROP_H
