/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

#ifdef _OPENMP
#include "omp.h"
#endif

// Uncomment to trade flexibility for speed
//#define CONST_HIST_SIZE

// Uncomment to get some performance stats in stderr
//#define REPORT_TICKS

#ifdef CONST_HIST_SIZE
#define m_BinBit 5
#define m_ByteShift 3
#endif

typedef float DefHistType;
#define DefHistTypeMat CV_32F
#define HIST_INDEX(_pData) (((_pData)[0]>>m_ByteShift) + (((_pData)[1]>>(m_ByteShift))<<m_BinBit)+((pImgData[2]>>m_ByteShift)<<(m_BinBit*2)))

class DefHist
{
public:
    CvMat*          m_pHist;
    DefHistType     m_HistVolume;
    DefHist(int BinNum=0)
    {
        m_pHist = NULL;
        m_HistVolume = 0;
        Resize(BinNum);
    }

    ~DefHist()
    {
        if(m_pHist)cvReleaseMat(&m_pHist);
    }

    void Resize(int BinNum)
    {
        if(m_pHist)cvReleaseMat(&m_pHist);
        if(BinNum>0)
        {
            m_pHist = cvCreateMat(1, BinNum, DefHistTypeMat);
            cvZero(m_pHist);
        }
        m_HistVolume = 0;
    }

    void Update(DefHist* pH, float W)
    {   /* Update histogram: */
        double  Vol, WM, WC;
        Vol = 0.5*(m_HistVolume + pH->m_HistVolume);
        WM = Vol*(1-W)/m_HistVolume;
        WC = Vol*(W)/pH->m_HistVolume;
        cvAddWeighted(m_pHist, WM, pH->m_pHist,WC,0,m_pHist);
        m_HistVolume = (float)cvSum(m_pHist).val[0];
    }   /* Update histogram: */
};

class CvBlobTrackerOneMSFG:public CvBlobTrackerOne
{
protected:
    int             m_BinNumTotal; /* protected only for parralel MSPF */
    CvSize          m_ObjSize;

    void ReAllocKernel(int  w, int h)
    {
        int     x,y;
        float   x0 = 0.5f*(w-1);
        float   y0 = 0.5f*(h-1);
        assert(w>0);
        assert(h>0);
        m_ObjSize = cvSize(w,h);

        if(m_KernelHist) cvReleaseMat(&m_KernelHist);
        if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
        m_KernelHist = cvCreateMat(h, w, DefHistTypeMat);
        m_KernelMeanShift = cvCreateMat(h, w, DefHistTypeMat);

        for(y=0; y<h; ++y) for(x=0; x<w; ++x)
        {
            double r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
//            double r2 = ((x-x0)*(x-x0)+(y-y0)*(y-y0))/((y0*y0)+(x0*x0));
            CV_MAT_ELEM(m_KernelHist[0],DefHistType, y, x) = (DefHistType)GetKernelHist(r2);
            CV_MAT_ELEM(m_KernelMeanShift[0],DefHistType, y, x) = (DefHistType)GetKernelMeanShift(r2);
        }
    }

private:
    /* Parameters: */
    int             m_IterNum;
    float           m_FGWeight;
    float           m_Alpha;
    CvMat*          m_KernelHist;
    CvMat*          m_KernelMeanShift;
#ifndef CONST_HIST_SIZE
    int             m_BinBit;
    int             m_ByteShift;
#endif
    int             m_BinNum;
    int             m_Dim;
    /*
    CvMat*          m_HistModel;
    float           m_HistModelVolume;
    CvMat*          m_HistCandidate;
    float           m_HistCandidateVolume;
    CvMat*          m_HistTemp;
    */
    DefHist         m_HistModel;
    DefHist         m_HistCandidate;
    DefHist         m_HistTemp;

    CvBlob          m_Blob;
    int             m_Collision;

    void ReAllocHist(int Dim, int BinBit)
    {
#ifndef CONST_HIST_SIZE
        m_BinBit = BinBit;
        m_ByteShift = 8-BinBit;
#endif
        m_Dim = Dim;
        m_BinNum = (1<<BinBit);
        m_BinNumTotal = cvRound(pow((double)m_BinNum,(double)m_Dim));
        /*
        if(m_HistModel) cvReleaseMat(&m_HistModel);
        if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
        if(m_HistTemp) cvReleaseMat(&m_HistTemp);
        m_HistCandidate = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
        m_HistModel = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
        m_HistTemp = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
        cvZero(m_HistCandidate);
        cvZero(m_HistModel);
        m_HistModelVolume = 0.0f;
        m_HistCandidateVolume = 0.0f;
        */
        m_HistCandidate.Resize(m_BinNumTotal);
        m_HistModel.Resize(m_BinNumTotal);
        m_HistTemp.Resize(m_BinNumTotal);
    }

    double GetKernelHist(double r2)
    {
        return (r2 < 1) ? 1 -  r2 : 0;
    }

    double GetKernelMeanShift(double r2)
    {
        return (r2<1) ? 1 : 0;
    }

//    void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pHist, DefHistType* pHistVolume)
//    void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, DefHist* pHist)

    void CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist)
    {
        int UsePrecalculatedKernel = 0;
        int BW = cvRound(pBlob->w);
        int BH = cvRound(pBlob->h);
        DefHistType Volume = 0;
        int         x0 = cvRound(pBlob->x - BW*0.5);
        int         y0 = cvRound(pBlob->y - BH*0.5);
        int         x,y;

        UsePrecalculatedKernel = (BW == m_ObjSize.width && BH == m_ObjSize.height ) ;

        //cvZero(pHist);
        cvSet(pHist->m_pHist,cvScalar(1.0/m_BinNumTotal)); /* no zero bins, all bins have very small value*/
        Volume = 1;

        assert(BW < pImg->width);
        assert(BH < pImg->height);
        if((x0+BW)>=pImg->width) BW=pImg->width-x0-1;
        if((y0+BH)>=pImg->height) BH=pImg->height-y0-1;
        if(x0<0){ x0=0;}
        if(y0<0){ y0=0;}

        if(m_Dim == 3)
        {
            for(y=0; y<BH; ++y)
            {
                unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
                unsigned char* pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
                DefHistType* pKernelData = NULL;

                if(UsePrecalculatedKernel)
                {
                    pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0],y,0,sizeof(DefHistType)));
                }

                for(x=0; x<BW; ++x, pImgData+=3)
                {
                    DefHistType K;
                    int index = HIST_INDEX(pImgData);
                    assert(index >= 0 && index < pHist->m_pHist->cols);

                    if(UsePrecalculatedKernel)
                    {
                        K = pKernelData[x];
                    }
                    else
                    {
                        float dx = (x+x0-pBlob->x)/(pBlob->w*0.5f);
                        float dy = (y+y0-pBlob->y)/(pBlob->h*0.5f);
                        double r2 = dx*dx+dy*dy;
                        K = (float)GetKernelHist(r2);
                    }

                    if(pMaskData)
                    {
                        K *= pMaskData[x]*0.003921568627450980392156862745098f;
                    }
                    Volume += K;
                    ((DefHistType*)(pHist->m_pHist->data.ptr))[index] += K;

                }   /* Next column. */
            }   /*  Next row. */
        }   /* if m_Dim == 3. */

        pHist->m_HistVolume = Volume;

    };  /* CollectHist */

    double calcBhattacharyya(DefHist* pHM = NULL, DefHist* pHC = NULL, DefHist* pHT = NULL)
    {
        if(pHM==NULL) pHM = &m_HistModel;
        if(pHC==NULL) pHC = &m_HistCandidate;
        if(pHT==NULL) pHT = &m_HistTemp;
        if(pHC->m_HistVolume*pHM->m_HistVolume > 0)
        {
#if 0
            // Use CV functions:
            cvMul(pHM->m_pHist,pHC->m_pHist,pHT->m_pHist);
            cvPow(pHT->m_pHist,pHT->m_pHist,0.5);
            return cvSum(pHT->m_pHist).val[0] / sqrt(pHC->m_HistVolume*pHM->m_HistVolume);
#else
            // Do computations manually and let autovectorizer do the job:
            DefHistType* hm=(DefHistType *)(pHM->m_pHist->data.ptr);
            DefHistType* hc=(DefHistType *)(pHC->m_pHist->data.ptr);
            //ht=(DefHistType *)(pHT->m_pHist->data.ptr);
            int size = pHM->m_pHist->width*pHM->m_pHist->height;
            double sum = 0.;
            for(int i = 0; i < size; i++ )
            {
                sum += sqrt(hm[i]*hc[i]);
            }
            return sum / sqrt(pHC->m_HistVolume*pHM->m_HistVolume);
#endif
        }
        return 0;
    }   /* calcBhattacharyyaCoefficient. */

protected:
    // double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, float x, float y, DefHist* pHist=NULL)
    double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob, DefHist* pHist=NULL, int /*thread_number*/ = 0)
    {
        if(pHist==NULL)pHist = &m_HistTemp;
        CollectHist(pImg, pImgFG, pBlob, pHist);
        return calcBhattacharyya(&m_HistModel, pHist, pHist);
    }

    void UpdateModelHist(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob)
    {
        if(m_Alpha>0 && !m_Collision)
        {   /* Update histogram: */
            CollectHist(pImg, pImgFG, pBlob, &m_HistCandidate);
            m_HistModel.Update(&m_HistCandidate, m_Alpha);
        }   /* Update histogram. */

    }   /* UpdateModelHist */

public:
    CvBlobTrackerOneMSFG()
    {

        /* Add several parameters for external use: */
        m_FGWeight = 2;
        AddParam("FGWeight", &m_FGWeight);
        CommentParam("FGWeight","Weight of FG mask using (0 - mask will not be used for tracking)");

        m_Alpha = 0.01f;
        AddParam("Alpha", &m_Alpha);
        CommentParam("Alpha","Coefficient for model histogram updating (0 - hist is not upated)");

        m_IterNum = 10;
        AddParam("IterNum", &m_IterNum);
        CommentParam("IterNum","Maximal number of iteration in meanshift operation");

        /* Initialize internal data: */
        m_Collision = 0;
//        m_BinBit=0;
        m_Dim = 0;
        /*
        m_HistModel = NULL;
        m_HistCandidate = NULL;
        m_HistTemp = NULL;
        */
        m_KernelHist = NULL;
        m_KernelMeanShift = NULL;
        ReAllocHist(3,5);   /* 3D hist, each dim has 2^5 bins*/

        SetModuleName("MSFG");
    }

    ~CvBlobTrackerOneMSFG()
    {
        /*
        if(m_HistModel) cvReleaseMat(&m_HistModel);
        if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
        if(m_HistTemp) cvReleaseMat(&m_HistTemp);
        */
        if(m_KernelHist) cvReleaseMat(&m_KernelHist);
        if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
    }

    /* Interface: */
    virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
    {
        int w = cvRound(CV_BLOB_WX(pBlobInit));
        int h = cvRound(CV_BLOB_WY(pBlobInit));
        if(w<CV_BLOB_MINW)w=CV_BLOB_MINW;
        if(h<CV_BLOB_MINH)h=CV_BLOB_MINH;
        if(pImg)
        {
            if(w>pImg->width)w=pImg->width;
            if(h>pImg->height)h=pImg->height;
        }
        ReAllocKernel(w,h);
        if(pImg)
            CollectHist(pImg, pImgFG, pBlobInit, &m_HistModel);
        m_Blob = pBlobInit[0];
    };

    virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
    {
        int     iter;

        if(pBlobPrev)
        {
            m_Blob = pBlobPrev[0];
        }

        {   /* Check blob size and realloc kernels if it is necessary: */
            int w = cvRound(m_Blob.w);
            int h = cvRound(m_Blob.h);
            if( w != m_ObjSize.width || h!=m_ObjSize.height)
            {
                ReAllocKernel(w,h);
                /* after this ( w != m_ObjSize.width || h!=m_ObjSize.height) shoiuld be false */
            }
        }   /* Check blob size and realloc kernels if it is necessary: */


        for(iter=0; iter<m_IterNum; ++iter)
        {
            float   newx=0,newy=0,sum=0;
            //int     x,y;
            double  B0;

            //CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
            CollectHist(pImg, NULL, &m_Blob, &m_HistCandidate);
            B0 = calcBhattacharyya();

            if(m_Wnd)if(CV_BLOB_ID(pBlobPrev)==0 && iter == 0)
            {   /* Debug: */
                IplImage*   pW = cvCloneImage(pImgFG);
                IplImage*   pWFG = cvCloneImage(pImgFG);
                int         x,y;

                cvZero(pW);
                cvZero(pWFG);

                assert(m_ObjSize.width < pImg->width);
                assert(m_ObjSize.height < pImg->height);

                /* Calculate shift vector: */
                for(y=0; y<pImg->height; ++y)
                {
                    unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y,0);
                    unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y,0)):NULL;

                    for(x=0; x<pImg->width; ++x, pImgData+=3)
                    {
                        int         xk = cvRound(x-(m_Blob.x-m_Blob.w*0.5));
                        int         yk = cvRound(y-(m_Blob.y-m_Blob.h*0.5));
                        double      HM = 0;
                        double      HC = 0;
                        double      K;
                        int index = HIST_INDEX(pImgData);
                        assert(index >= 0 && index < m_BinNumTotal);

                        if(fabs(x-m_Blob.x)>m_Blob.w*0.6) continue;
                        if(fabs(y-m_Blob.y)>m_Blob.h*0.6) continue;

                        if(xk < 0 || xk >= m_KernelMeanShift->cols) continue;
                        if(yk < 0 || yk >= m_KernelMeanShift->rows) continue;

                        if(m_HistModel.m_HistVolume>0)
                            HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index]/m_HistModel.m_HistVolume;

                        if(m_HistCandidate.m_HistVolume>0)
                            HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index]/m_HistCandidate.m_HistVolume;

                        K = *(DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],yk,xk,sizeof(DefHistType));

                        if(HC>0)
                        {
                            double  V = sqrt(HM / HC);
                            int     Vi = cvRound(V * 64);
                            if(Vi < 0) Vi = 0;
                            if(Vi > 255) Vi = 255;
                            CV_IMAGE_ELEM(pW,uchar,y,x) = (uchar)Vi;

                            V += m_FGWeight*(pMaskData?(pMaskData[x]/255.0f):0);
                            V*=K;
                            Vi = cvRound(V * 64);
                            if(Vi < 0) Vi = 0;
                            if(Vi > 255) Vi = 255;
                            CV_IMAGE_ELEM(pWFG,uchar,y,x) = (uchar)Vi;
                        }

                    }   /* Next column. */
                }   /*  Next row. */

                //cvNamedWindow("MSFG_W",0);
                //cvShowImage("MSFG_W",pW);
                //cvNamedWindow("MSFG_WFG",0);
                //cvShowImage("MSFG_WFG",pWFG);
                //cvNamedWindow("MSFG_FG",0);
                //cvShowImage("MSFG_FG",pImgFG);

                //cvSaveImage("MSFG_W.bmp",pW);
                //cvSaveImage("MSFG_WFG.bmp",pWFG);
                //cvSaveImage("MSFG_FG.bmp",pImgFG);

            }   /* Debug. */


            /* Calculate new position by meanshift: */

            /* Calculate new position: */
            if(m_Dim == 3)
            {
                int         x0 = cvRound(m_Blob.x - m_ObjSize.width*0.5);
                int         y0 = cvRound(m_Blob.y - m_ObjSize.height*0.5);
                int         x,y;

                assert(m_ObjSize.width < pImg->width);
                assert(m_ObjSize.height < pImg->height);

                /* Crop blob bounds: */
                if((x0+m_ObjSize.width)>=pImg->width) x0=pImg->width-m_ObjSize.width-1;
                if((y0+m_ObjSize.height)>=pImg->height) y0=pImg->height-m_ObjSize.height-1;
                if(x0<0){ x0=0;}
                if(y0<0){ y0=0;}

                /* Calculate shift vector: */
                for(y=0; y<m_ObjSize.height; ++y)
                {
                    unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
                    unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y+y0,x0)):NULL;
                    DefHistType* pKernelData = (DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],y,0,sizeof(DefHistType));

                    for(x=0; x<m_ObjSize.width; ++x, pImgData+=3)
                    {
                        DefHistType K = pKernelData[x];
                        double      HM = 0;
                        double      HC = 0;
                        int index = HIST_INDEX(pImgData);
                        assert(index >= 0 && index < m_BinNumTotal);

                        if(m_HistModel.m_HistVolume>0)
                            HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index]/m_HistModel.m_HistVolume;

                        if(m_HistCandidate.m_HistVolume>0)
                            HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index]/m_HistCandidate.m_HistVolume;

                        if(HC>0)
                        {
                            double V = sqrt(HM / HC);
                            if(!m_Collision && m_FGWeight>0 && pMaskData)
                            {
                                V += m_FGWeight*(pMaskData[x]/255.0f);
                            }
                            K *= (float)MIN(V,100000.);
                        }

                        sum += K;
                        newx += K*x;
                        newy += K*y;
                    }   /* Next column. */
                }   /*  Next row. */

                if(sum > 0)
                {
                    newx /= sum;
                    newy /= sum;
                }
                newx += x0;
                newy += y0;

            }   /* if m_Dim == 3. */

            /* Calculate new position by meanshift: */

            for(;;)
            {   /* Iterate using bahattcharrya coefficient: */
                double  B1;
                CvBlob  B = m_Blob;
//                CvPoint NewCenter = cvPoint(cvRound(newx),cvRound(newy));
                B.x = newx;
                B.y = newy;
                CollectHist(pImg, NULL, &B, &m_HistCandidate);
                B1 = calcBhattacharyya();
                if(B1 > B0) break;
                newx = 0.5f*(newx+m_Blob.x);
                newy = 0.5f*(newy+m_Blob.y);
                if(fabs(newx-m_Blob.x)<0.1 && fabs(newy-m_Blob.y)<0.1) break;
            }   /* Iterate using bahattcharrya coefficient. */

            if(fabs(newx-m_Blob.x)<0.5 && fabs(newy-m_Blob.y)<0.5) break;
            m_Blob.x = newx;
            m_Blob.y = newy;
        }   /* Next iteration. */

        while(!m_Collision && m_FGWeight>0)
        {   /* Update size if no collision. */
            float       Alpha = 0.04f;
            CvBlob      NewBlob;
            double      M00,X,Y,XX,YY;
            CvMoments   m;
            CvRect      r;
            CvMat       mat;

            r.width = cvRound(m_Blob.w*1.5+0.5);
            r.height = cvRound(m_Blob.h*1.5+0.5);
            r.x = cvRound(m_Blob.x - 0.5*r.width);
            r.y = cvRound(m_Blob.y - 0.5*r.height);
            if(r.x < 0) break;
            if(r.y < 0) break;
            if(r.x+r.width >= pImgFG->width) break;
            if(r.y+r.height >= pImgFG->height) break;
            if(r.height < 5 || r.width < 5) break;

            cvMoments( cvGetSubRect(pImgFG,&mat,r), &m, 0 );
            M00 = cvGetSpatialMoment( &m, 0, 0 );
            if(M00 <= 0 ) break;
            X = cvGetSpatialMoment( &m, 1, 0 )/M00;
            Y = cvGetSpatialMoment( &m, 0, 1 )/M00;
            XX = (cvGetSpatialMoment( &m, 2, 0 )/M00) - X*X;
            YY = (cvGetSpatialMoment( &m, 0, 2 )/M00) - Y*Y;
            NewBlob = cvBlob(r.x+(float)X,r.y+(float)Y,(float)(4*sqrt(XX)),(float)(4*sqrt(YY)));

            NewBlob.w = Alpha*NewBlob.w+m_Blob.w*(1-Alpha);
            NewBlob.h = Alpha*NewBlob.h+m_Blob.h*(1-Alpha);

            m_Blob.w = MAX(NewBlob.w,5);
            m_Blob.h = MAX(NewBlob.h,5);
            break;

        }   /* Update size if no collision. */

        return &m_Blob;

    };  /* CvBlobTrackerOneMSFG::Process */

    virtual double GetConfidence(CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL, IplImage* pImgUnusedReg = NULL)
    {
        double  S = 0.2;
        double  B = GetBhattacharyya(pImg, pImgUnusedReg, pBlob, &m_HistTemp);
        return exp((B-1)/(2*S));

    };  /*CvBlobTrackerOneMSFG::*/

    virtual void Update(CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG = NULL)
    {   /* Update histogram: */
        UpdateModelHist(pImg, pImgFG, pBlob?pBlob:&m_Blob);
    }   /*CvBlobTrackerOneMSFG::*/

    virtual void Release(){delete this;};
    virtual void SetCollision(int CollisionFlag)
    {
        m_Collision = CollisionFlag;
    }
    virtual void SaveState(CvFileStorage* fs)
    {
        cvWriteStruct(fs, "Blob", &m_Blob, "ffffi");
        cvWriteInt(fs,"Collision", m_Collision);
        cvWriteInt(fs,"HistVolume", cvRound(m_HistModel.m_HistVolume));
        cvWrite(fs,"Hist", m_HistModel.m_pHist);
    };
    virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
    {
        CvMat* pM;
        cvReadStructByName(fs, node, "Blob",&m_Blob, "ffffi");
        m_Collision = cvReadIntByName(fs,node,"Collision",m_Collision);
        pM = (CvMat*)cvRead(fs,cvGetFileNodeByName(fs,node,"Hist"));
        if(pM)
        {
            m_HistModel.m_pHist = pM;
            m_HistModel.m_HistVolume = (float)cvSum(pM).val[0];
        }
    };

};  /*CvBlobTrackerOneMSFG*/

#if 0
void CvBlobTrackerOneMSFG::CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist)
{
    int UsePrecalculatedKernel = 0;
    int BW = cvRound(pBlob->w);
    int BH = cvRound(pBlob->h);
    DefHistType Volume = 0;
    int         x0 = cvRound(pBlob->x - BW*0.5);
    int         y0 = cvRound(pBlob->y - BH*0.5);
    int         x,y;

    UsePrecalculatedKernel = (BW == m_ObjSize.width && BH == m_ObjSize.height ) ;

    //cvZero(pHist);
    cvSet(pHist->m_pHist,cvScalar(1.0/m_BinNumTotal)); /* no zero bins, all bins have very small value*/
    Volume = 1;

    assert(BW < pImg->width);
    assert(BH < pImg->height);
    if((x0+BW)>=pImg->width) BW=pImg->width-x0-1;
    if((y0+BH)>=pImg->height) BH=pImg->height-y0-1;
    if(x0<0){ x0=0;}
    if(y0<0){ y0=0;}

    if(m_Dim == 3)
    {
        for(y=0; y<BH; ++y)
        {
            unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
            unsigned char* pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
            DefHistType* pKernelData = NULL;

            if(UsePrecalculatedKernel)
            {
                pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0],y,0,sizeof(DefHistType)));
            }

            for(x=0; x<BW; ++x, pImgData+=3)
            {
                DefHistType K;
                int index = HIST_INDEX(pImgData);
                assert(index >= 0 && index < pHist->m_pHist->cols);

                if(UsePrecalculatedKernel)
                {
                    K = pKernelData[x];
                }
                else
                {
                    float dx = (x+x0-pBlob->x)/(pBlob->w*0.5);
                    float dy = (y+y0-pBlob->y)/(pBlob->h*0.5);
                    double r2 = dx*dx+dy*dy;
                    K = GetKernelHist(r2);
                }

                if(pMaskData)
                {
                    K *= pMaskData[x]*0.003921568627450980392156862745098;
                }
                Volume += K;
                ((DefHistType*)(pHist->m_pHist->data.ptr))[index] += K;

            }   /* Next column. */
        }   /*  Next row. */
    }   /*  if m_Dim == 3. */

    pHist->m_HistVolume = Volume;

};  /* CollectHist */
#endif

CvBlobTrackerOne* cvCreateBlobTrackerOneMSFG()
{
    return (CvBlobTrackerOne*) new CvBlobTrackerOneMSFG;
}

CvBlobTracker* cvCreateBlobTrackerMSFG()
{
    return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSFG);
}

/* Create specific tracker without FG
 * usin - just simple mean-shift tracker: */
class CvBlobTrackerOneMS:public CvBlobTrackerOneMSFG
{
public:
    CvBlobTrackerOneMS()
    {
        SetParam("FGWeight",0);
        DelParam("FGWeight");
        SetModuleName("MS");
    };
};

CvBlobTrackerOne* cvCreateBlobTrackerOneMS()
{
    return (CvBlobTrackerOne*) new CvBlobTrackerOneMS;
}

CvBlobTracker* cvCreateBlobTrackerMS()
{
    return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMS);
}

typedef struct DefParticle
{
    CvBlob  blob;
    float   Vx,Vy;
    double  W;
} DefParticle;

class CvBlobTrackerOneMSPF:public CvBlobTrackerOneMS
{
private:
    /* parameters */
    int             m_ParticleNum;
    float           m_UseVel;
    float           m_SizeVar;
    float           m_PosVar;

    CvSize          m_ImgSize;
    CvBlob          m_Blob;
    DefParticle*    m_pParticlesPredicted;
    DefParticle*    m_pParticlesResampled;
    CvRNG           m_RNG;
#ifdef _OPENMP
    int             m_ThreadNum;
    DefHist*        m_HistForParalel;
#endif

public:
    virtual void SaveState(CvFileStorage* fs)
    {
        CvBlobTrackerOneMS::SaveState(fs);
        cvWriteInt(fs,"ParticleNum",m_ParticleNum);
        cvWriteStruct(fs,"ParticlesPredicted",m_pParticlesPredicted,"ffffiffd",m_ParticleNum);
        cvWriteStruct(fs,"ParticlesResampled",m_pParticlesResampled,"ffffiffd",m_ParticleNum);
    };

    virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
    {
        //CvMat* pM;
        CvBlobTrackerOneMS::LoadState(fs,node);
        m_ParticleNum = cvReadIntByName(fs,node,"ParticleNum",m_ParticleNum);
        if(m_ParticleNum>0)
        {
            Realloc();
            printf("sizeof(DefParticle) is %d\n", (int)sizeof(DefParticle));
            cvReadStructByName(fs,node,"ParticlesPredicted",m_pParticlesPredicted,"ffffiffd");
            cvReadStructByName(fs,node,"ParticlesResampled",m_pParticlesResampled,"ffffiffd");
        }
    };
    CvBlobTrackerOneMSPF()
    {
        m_pParticlesPredicted = NULL;
        m_pParticlesResampled = NULL;
        m_ParticleNum = 200;

        AddParam("ParticleNum",&m_ParticleNum);
        CommentParam("ParticleNum","Number of particles");
        Realloc();

        m_UseVel = 0;
        AddParam("UseVel",&m_UseVel);
        CommentParam("UseVel","Percent of particles which use velocity feature");

        m_SizeVar = 0.05f;
        AddParam("SizeVar",&m_SizeVar);
        CommentParam("SizeVar","Size variation (in object size)");

        m_PosVar = 0.2f;
        AddParam("PosVar",&m_PosVar);
        CommentParam("PosVar","Position variation (in object size)");

        m_RNG = cvRNG(0);

        SetModuleName("MSPF");

#ifdef _OPENMP
        {
            m_ThreadNum = omp_get_num_procs();
            m_HistForParalel = new DefHist[m_ThreadNum];
        }
#endif
    }

    ~CvBlobTrackerOneMSPF()
    {
        if(m_pParticlesResampled)cvFree(&m_pParticlesResampled);
        if(m_pParticlesPredicted)cvFree(&m_pParticlesPredicted);
#ifdef _OPENMP
        if(m_HistForParalel) delete[] m_HistForParalel;
#endif
    }

private:
    void Realloc()
    {
        if(m_pParticlesResampled)cvFree(&m_pParticlesResampled);
        if(m_pParticlesPredicted)cvFree(&m_pParticlesPredicted);
        m_pParticlesPredicted = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
        m_pParticlesResampled = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
    };  /* Realloc*/

    void DrawDebug(IplImage* pImg, IplImage* /*pImgFG*/)
    {
        int k;
        for(k=0; k<2; ++k)
        {
            DefParticle*    pBP = k?m_pParticlesResampled:m_pParticlesPredicted;
            //const char*   name = k?"MSPF resampled particle":"MSPF Predicted particle";
            IplImage*       pI = cvCloneImage(pImg);
            int             h,hN = m_ParticleNum;
            CvBlob          C = cvBlob(0,0,0,0);
            double          WS = 0;
            for(h=0; h<hN; ++h)
            {
                CvBlob  B = pBP[h].blob;
                int     CW = cvRound(255*pBP[h].W);
                CvBlob* pB = &B;
                int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
                CvSize  s = cvSize(MAX(1,x), MAX(1,y));
                double  W = pBP[h].W;
                C.x += pB->x;
                C.y += pB->y;
                C.w += pB->w;
                C.h += pB->h;
                WS+=W;

                s = cvSize(1,1);
                cvEllipse( pI,
                    cvPointFrom32f(CV_BLOB_CENTER(pB)),
                    s,
                    0, 0, 360,
                    CV_RGB(CW,0,0), 1 );

            }   /* Next hypothesis. */

            C.x /= hN;
            C.y /= hN;
            C.w /= hN;
            C.h /= hN;

            cvEllipse( pI,
                cvPointFrom32f(CV_BLOB_CENTER(&C)),
                cvSize(cvRound(C.w*0.5),cvRound(C.h*0.5)),
                0, 0, 360,
                CV_RGB(0,0,255), 1 );

            cvEllipse( pI,
                cvPointFrom32f(CV_BLOB_CENTER(&m_Blob)),
                cvSize(cvRound(m_Blob.w*0.5),cvRound(m_Blob.h*0.5)),
                0, 0, 360,
                CV_RGB(0,255,0), 1 );

            //cvNamedWindow(name,0);
            //cvShowImage(name,pI);
            cvReleaseImage(&pI);
        } /*  */

        //printf("Blob %d, point (%.1f,%.1f) size (%.1f,%.1f)\n",m_Blob.ID,m_Blob.x,m_Blob.y,m_Blob.w,m_Blob.h);
    } /* ::DrawDebug */

private:
    void Prediction()
    {
        int p;
        for(p=0; p<m_ParticleNum; ++p)
        {   /* "Prediction" of particle: */
            //double  t;
            float   r[5];
            CvMat   rm = cvMat(1,5,CV_32F,r);
            cvRandArr(&m_RNG,&rm,CV_RAND_NORMAL,cvScalar(0),cvScalar(1));

            m_pParticlesPredicted[p] = m_pParticlesResampled[p];

            if(cvRandReal(&m_RNG)<0.5)
            {   /* Half of particles will predict based on external blob: */
                m_pParticlesPredicted[p].blob = m_Blob;
            }

            if(cvRandReal(&m_RNG)<m_UseVel)
            {   /* Predict moving particle by usual way by using speed: */
                m_pParticlesPredicted[p].blob.x += m_pParticlesPredicted[p].Vx;
                m_pParticlesPredicted[p].blob.y += m_pParticlesPredicted[p].Vy;
            }
            else
            {   /* Stop several particles: */
                m_pParticlesPredicted[p].Vx = 0;
                m_pParticlesPredicted[p].Vy = 0;
            }

            {   /* Update position: */
                float S = (m_Blob.w + m_Blob.h)*0.5f;
                m_pParticlesPredicted[p].blob.x += m_PosVar*S*r[0];
                m_pParticlesPredicted[p].blob.y += m_PosVar*S*r[1];

                /* Update velocity: */
                m_pParticlesPredicted[p].Vx += (float)(m_PosVar*S*0.1*r[3]);
                m_pParticlesPredicted[p].Vy += (float)(m_PosVar*S*0.1*r[4]);
            }

            /* Update size: */
            m_pParticlesPredicted[p].blob.w *= (1+m_SizeVar*r[2]);
            m_pParticlesPredicted[p].blob.h *= (1+m_SizeVar*r[2]);

            /* Truncate size of particle: */
            if(m_pParticlesPredicted[p].blob.w > m_ImgSize.width*0.5f)
            {
                m_pParticlesPredicted[p].blob.w = m_ImgSize.width*0.5f;
            }

            if(m_pParticlesPredicted[p].blob.h > m_ImgSize.height*0.5f)
            {
                m_pParticlesPredicted[p].blob.h = m_ImgSize.height*0.5f;
            }

            if(m_pParticlesPredicted[p].blob.w < 1 )
            {
                m_pParticlesPredicted[p].blob.w = 1;
            }

            if(m_pParticlesPredicted[p].blob.h < 1)
            {
                m_pParticlesPredicted[p].blob.h = 1;
            }
        }   /* "Prediction" of particle. */
    }   /* Prediction */

    void UpdateWeightsMS(IplImage* pImg, IplImage* /*pImgFG*/)
    {
        int p;
#ifdef _OPENMP
        if( m_HistForParalel[0].m_pHist==NULL || m_HistForParalel[0].m_pHist->cols != m_BinNumTotal)
        {
            int t;
            for(t=0; t<m_ThreadNum; ++t)
                m_HistForParalel[t].Resize(m_BinNumTotal);
        }
#endif

#ifdef _OPENMP
#pragma omp parallel for num_threads(m_ThreadNum) schedule(runtime)
#endif
        for(p=0;p<m_ParticleNum;++p)
        {   /* Calculate weights for particles: */
            double  S = 0.2;
            double  B = 0;
#ifdef _OPENMP
            assert(omp_get_thread_num()<m_ThreadNum);
#endif

            B = GetBhattacharyya(
                pImg, NULL,
                &(m_pParticlesPredicted[p].blob)
#ifdef _OPENMP
                ,&(m_HistForParalel[omp_get_thread_num()])
#endif
                );
            m_pParticlesPredicted[p].W *= exp((B-1)/(2*S));

        }   /* Calculate weights for particles. */
    }

    void UpdateWeightsCC(IplImage* /*pImg*/, IplImage* /*pImgFG*/)
    {
        int p;
#ifdef _OPENMP
#pragma omp parallel for
#endif
        for(p=0; p<m_ParticleNum; ++p)
        {   /* Calculate weights for particles: */
            double W = 1;
            m_pParticlesPredicted[p].W *= W;
        }   /* Calculate weights for particles. */
    }

    void Resample()
    {   /* Resample particle: */
        int         p;
        double      Sum = 0;

        for(p=0; p<m_ParticleNum; ++p)
        {
            Sum += m_pParticlesPredicted[p].W;
        }

        for(p=0; p<m_ParticleNum; ++p)
        {
            double  T = Sum * cvRandReal(&m_RNG);   /* Set current random threshold for cululative weight. */
            int     p2;
            double  Sum2 = 0;

            for(p2=0; p2<m_ParticleNum; ++p2)
            {
                Sum2 += m_pParticlesPredicted[p2].W;
                if(Sum2 >= T)break;
            }

            if(p2>=m_ParticleNum)p2=m_ParticleNum-1;
            m_pParticlesResampled[p] = m_pParticlesPredicted[p2];
            m_pParticlesResampled[p].W = 1;

        }   /* Find next particle. */
    }   /*  Resample particle. */


public:
    virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
    {
        int i;
        CvBlobTrackerOneMSFG::Init(pBlobInit, pImg, pImgFG);
        DefParticle PP;
        PP.W = 1;
        PP.Vx = 0;
        PP.Vy = 0;
        PP.blob = pBlobInit[0];
        for(i=0;i<m_ParticleNum;++i)
        {
            m_pParticlesPredicted[i] = PP;
            m_pParticlesResampled[i] = PP;
        }
        m_Blob = pBlobInit[0];

    }   /* CvBlobTrackerOneMSPF::Init*/

    virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
    {
        int p;

        m_ImgSize.width = pImg->width;
        m_ImgSize.height = pImg->height;


        m_Blob = pBlobPrev[0];

        {   /* Check blob size and realloc kernels if it is necessary: */
            int w = cvRound(m_Blob.w);
            int h = cvRound(m_Blob.h);
            if( w != m_ObjSize.width || h!=m_ObjSize.height)
            {
                ReAllocKernel(w,h);
                /* After this ( w != m_ObjSize.width || h!=m_ObjSize.height) should be false. */
            }
        }   /* Check blob size and realloc kernels if it is necessary. */

        Prediction();

#ifdef REPORT_TICKS
        int64 ticks = cvGetTickCount();
#endif

        UpdateWeightsMS(pImg, pImgFG);

#ifdef REPORT_TICKS
        ticks = cvGetTickCount() - ticks;
        fprintf(stderr, "PF UpdateWeights, %d ticks\n",  (int)ticks);
        ticks = cvGetTickCount();
#endif

        Resample();

#ifdef REPORT_TICKS
        ticks = cvGetTickCount() - ticks;
        fprintf(stderr, "PF Resampling, %d ticks\n",  (int)ticks);
#endif

        {   /* Find average result: */
            float   x = 0;
            float   y = 0;
            float   w = 0;
            float   h = 0;
            float   Sum = 0;

            DefParticle* pP = m_pParticlesResampled;

            for(p=0; p<m_ParticleNum; ++p)
            {
                float W = (float)pP[p].W;
                x += W*pP[p].blob.x;
                y += W*pP[p].blob.y;
                w += W*pP[p].blob.w;
                h += W*pP[p].blob.h;
                Sum += W;
            }

            if(Sum>0)
            {
                m_Blob.x = x / Sum;
                m_Blob.y = y / Sum;
                m_Blob.w = w / Sum;
                m_Blob.h = h / Sum;
            }
        }   /* Find average result. */

        if(m_Wnd)
        {
            DrawDebug(pImg, pImgFG);
        }

        return &m_Blob;

    }   /* CvBlobTrackerOneMSPF::Process */

    virtual void SkipProcess(CvBlob* pBlob, IplImage* /*pImg*/, IplImage* /*pImgFG*/ = NULL)
    {
        int p;
        for(p=0; p<m_ParticleNum; ++p)
        {
            m_pParticlesResampled[p].blob = pBlob[0];
            m_pParticlesResampled[p].Vx = 0;
            m_pParticlesResampled[p].Vy = 0;
            m_pParticlesResampled[p].W = 1;
        }
    }

    virtual void Release(){delete this;};
    virtual void ParamUpdate()
    {
        Realloc();
    }

};  /* CvBlobTrackerOneMSPF */

CvBlobTrackerOne* cvCreateBlobTrackerOneMSPF()
{
    return (CvBlobTrackerOne*) new CvBlobTrackerOneMSPF;
}

CvBlobTracker* cvCreateBlobTrackerMSPF()
{
    return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSPF);
}