#ifndef __NORMALESTFROMSRI_H
#define __NORMALESTFROMSRI_H

/*@@@**************************************************************************
 ** \file  normalEstFromSri.h
 * \date   Mon Jun 22 14:55:16 EDT 2009
 * \author Hernan Badino
 * \notes  
*******************************************************************************
******************************************************************************/

/* INCLUDES */

#include <math.h>

#include "colorEncoding.h"
#include "operator.h"
#include "image.h"
#include "sphericalRangeImage.h"
#include "3DVectorImage.h"
#include "3DMatrixImage.h"
#include "uByteImage.h"


/* CONSTANTS */

namespace VIC
{
    /* CLASS DEFINITION */
    class CNormalEstFromSriOp: public VIC::COperator
    {

    /// Public data types.
    public:

        typedef enum
        {
            CEM_VERTICAL,
            CEM_HORIZONTAL,
            CEM_NORMAL_X,
            CEM_NORMAL_Y,
            CEM_NORMAL_Z
        } EColorEncodingMode_t;
        
        typedef enum
        {
            NCM_NABLAOP,
            NCM_LS,
            NCM_FAST_LS,
            NCM_INV_LS,
            NCM_CROSS_PRODUCT,
            NCM_RANDOM
       } ENormalComputationMethod_t;
        
        typedef enum
        {
            GTOT_SPHERE,
            GTOT_CYLINDER,
            GTOT_N_AGONAL_PRISM,
            GTOT_ROAD,
            GTOT_REAL
        } EGTObjectType_t;

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

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


    /// Set/Gets (Parameters)
    public:
        //ADD_PARAM_ACCESS_NOTIFIER(S2D<double>,  m_roiSize,           SRISize,    createSRIs )

        ADD_PARAM_ACCESS (S2D<unsigned int>,          m_kernelSize,      KernelSize );
        ADD_PARAM_ACCESS (double,                     m_vectorLength_d,  VectorLength );
        ADD_PARAM_ACCESS (bool,                       m_applySobel_b,    ApplySobel );
        ADD_PARAM_ACCESS (bool,                       m_applyGauss_b,    ApplyGauss );
        /// Color encoding mode.
        ADD_PARAM_ACCESS (EColorEncodingMode_t,       m_encodingMode_e,  ColorEncodingMode );
        ADD_PARAM_ACCESS (ENormalComputationMethod_t, m_normalMode_e,    NormalMode );
        ADD_PARAM_ACCESS (EGTObjectType_t,            m_gtObjectType_e,  GTObjectType );
        
        ADD_PARAM_ACCESS (bool,                       m_show3DNormals_b,      Show3DNormals );
        ADD_PARAM_ACCESS (bool,                       m_encodeErrorIn3DVis_b, EncodeErrorIn3DVis );
        ADD_PARAM_ACCESS (SRgb,                       m_bgColor,              BackgroundColor );
        ADD_PARAM_ACCESS (float,                      m_pointSize_f,          PointSize );
        ADD_PARAM_ACCESS (float,                      m_lineWidth_f,          LineWidth );

        ADD_PARAM_ACCESS (double,                     m_freeParam_d,     FreeParam );
        
        ADD_PARAM_ACCESS (double,                     m_noiseStdDev_d,   NoiseStdDev );

        ADD_PARAM_ACCESS (C3DVector,                  m_normal2Ground,   Normal2Ground );

        ADD_PARAM_ACCESS (double,                     m_baseDistToObj_d, BaseDistToObj );

        ADD_PARAM_ACCESS (bool,                       m_gaussNoise_b,    GaussNoise );
   protected:
       /// Error data type
        struct SError_t
        {
            unsigned int  count_ui;
            double        mse_d;
            double        sumdiffs_d;
            SError_t ()   { clear(); }
            void clear()  { count_ui = 0u; mse_d = 0; sumdiffs_d = 0;}
        };

    protected:
        void         registerDrawingLists();
        void         allocateImages ();

        void         show3D();
        void         computeLeastSquarePlane ( const std::vector<C3DVector> & f_points_v,
                                               C3DVector &                    fr_plane );

        bool         calculateGradientsWithIPP();

        void         createSRI( );

        void         applyGaussAndMask(CFloatImage &fr_img);

        double       getGaussianNoise ( const float  f_mean_f,
                                        const float  f_standardDeviation_f );

        double       getSlashNoise ( const float  f_mean_f,
                                     const float  f_standardDeviation_f );

        void         calculateError( SError_t & errorData );

        void         testIpp();

        void         compute();

        void         recomputeInvMImage( bool f_force_b = false );
        
        void         computeBRangeImage();

        void         maskNormals();

        void         createMask();

        void         computeFastLSSums();

        void         recomputeRotImage();

        void         calculateNormalsNabla(int f_par_i = 0);
        void         calculateNormalsLS();
        void         calculateNormalsFastLS();
        void         calculateNormalsInvLS(int f_par_i = 0);
        void         calculateNormalsCrossP();
        void         calculateNormalsRandom();
        
        void         recomputeSinCosLuts();

        void testConditioning(const C3DMatrix &f_matrix );
        
    /// Protected Data Types
    protected:

        /// Spherical range image
        CSphericalRangeImage    m_sri;
        
        /// Spherical range image
        CSphericalRangeImage    m_sriInv;

        /// Spherical range image
        C3DVectorImage          m_localVectorPts;

        /// Inverse 3D points.
        C3DVectorImage          m_localInvVectorPts;

        /// Summed Inverse 3D points.
        C3DVectorImage          m_sumInvVectorsPts;
        
        /// Normal image.
        C3DVectorImage          m_gtNormalImg;

        /// Normal image.
        C3DVectorImage          m_normalImg;

        /// Mask the output of the min/max filter with the invalid values.
        S2D<unsigned int>       m_kernelSize;

        /// Sobel or prewitt?
        bool                    m_applySobel_b;
        
        /// Apply gauss filter to the range image.
        bool                    m_applyGauss_b;

        /// Float range image.
        CFloatImage             m_rangeImg;

        /// Float range image.
        CFloatImage             m_bufferImg;

        /// Float range image.
        CFloatImage             m_gradXImg;

        /// Float range image.
        CFloatImage             m_gradYImg;

        /// Color encoding for sri
        CColorEncoding          m_colorEncSri;

        /// Color encoding for sobel results.
        CColorEncoding          m_colorEncGrad;
        
        /// Color encoding mode for normal vectors.
        EColorEncodingMode_t    m_encodingMode_e;
        
        /// Color encoding for normal vectors.m_gaussNoise_b;
        CColorEncoding          m_colorEncNormal;

        /// Color encoding for sri
        CColorEncoding          m_colorEncNormalErr;

        /// Display normal vector length.
        double                  m_vectorLength_d;
        
        /// Normal computation mode.
        ENormalComputationMethod_t
                                m_normalMode_e;

        /// Display normal vector length.
        bool                    m_show3DNormals_b;

        /// Color encode error in 3D visualization.
        bool                    m_encodeErrorIn3DVis_b;

        /// Background color for 3D visualization.
        SRgb                    m_bgColor;
        
        /// Point Size.
        float                   m_pointSize_f;

        /// Line Width.
        float                   m_lineWidth_f;

        /// Display normal vector length.
        double                  m_freeParam_d;

        /// Object type to test methods.
        EGTObjectType_t         m_gtObjectType_e;

        /// Display normal vector length.
        C3DVector               m_normal2Ground;

        /// Display normal vector length.
        double                  m_noiseStdDev_d;

        /// Display normal vector length.
        double                  m_baseDistToObj_d;

        /// Error Image containing the angle error.
        CDoubleImage            m_errorImg;        

        /// Trials
        unsigned int            m_trials_ui;

        /// Use gauss noise or slash
        bool                    m_gaussNoise_b;

        /// Inverse M.
        C3DMatrixImage          m_invM;
        
        /// B Img.
        C3DVectorImage          m_bImg;

        /// Mask
        CUByteImage             m_maskImg;

        // Sum of matrices for fast ls
        C3DMatrixImage          m_outerProductImg;

        // Sum of matrices for fast ls
        C3DMatrixImage          m_sumOuterProductImg;

        // Sum of vectors for fast ls
        C3DVectorImage          m_sumLocalVectorsImgs;

        // Sum of matrices for fast ls
        C3DMatrixImage          m_rotMatrix;

        /// Lut for Cos(azimuth)
        std::vector<double>    m_cosAz_v;

        /// Lut for Sin(azimuth)
        std::vector<double>    m_sinAz_v;
        
        /// Lut for Cos(elevation)
        std::vector<double>    m_cosEl_v;

        /// Lut for Sin(elevation)
        std::vector<double>    m_sinEl_v;

    private:
        
   };
}

#endif // __NORMALESTFROMSRI_H
