/*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
//                For Open Source Computer Vision Library
//
// 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"

#define MAX_FV_SIZE 5
#define BLOB_NUM    5

typedef struct DefBlobFVN
{
	CvBlob  blob;
	CvBlob  BlobSeq[BLOB_NUM];
	int     state;
	int     LastFrame;
	int     FrameNum;
} DefBlobFVN;

class CvBlobTrackFVGenN: public CvBlobTrackFVGen
{
private:
	CvBlobSeq       m_BlobList;
	CvMemStorage*   m_pMem;
	CvSeq*          m_pFVSeq;
	float           m_FVMax[MAX_FV_SIZE];
	float           m_FVMin[MAX_FV_SIZE];
	float           m_FVVar[MAX_FV_SIZE];
	int             m_Dim;
	CvBlob          m_BlobSeq[BLOB_NUM];
	int             m_Frame;
	int             m_State;
	int             m_LastFrame;
	int             m_ClearFlag;
	void Clear()
	{
		if(m_pMem)
		{
			cvClearMemStorage(m_pMem);
			m_pFVSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(float)*(m_Dim+1), m_pMem);
			m_ClearFlag = 1;
		}
	}
public:
	CvBlobTrackFVGenN(int dim = 2 ):m_BlobList(sizeof(DefBlobFVN))
	{
		int i;
		assert(dim <= MAX_FV_SIZE);
		m_Dim = dim;
		for(i=0; i<m_Dim; ++i)
		{
			m_FVVar[i] = 0.01f;
			m_FVMax[i] = 1;
			m_FVMin[i] = 0;
		}
		m_Frame = 0;
		m_State = 0;
		m_pMem = cvCreateMemStorage();
		m_pFVSeq = NULL;
		Clear();

		switch(dim) {
		case 2: SetModuleName("P"); break;
		case 4: SetModuleName("PV"); break;
		case 5: SetModuleName("PVS"); break;
		}
	};

	~CvBlobTrackFVGenN()
	{
		if(m_pMem)cvReleaseMemStorage(&m_pMem);
	};

	void AddBlob(CvBlob* pBlob)
	{
		float       FV[MAX_FV_SIZE+1];
		int         i;
		DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));

		if(!m_ClearFlag) Clear();

		if(pFVBlob==NULL)
		{
			DefBlobFVN BlobNew;
			BlobNew.blob = pBlob[0];
			BlobNew.LastFrame = m_Frame;
			BlobNew.state = 0;;
			BlobNew.FrameNum = 0;
			m_BlobList.AddBlob((CvBlob*)&BlobNew);
			pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));
		} /* Add new record if necessary. */

		pFVBlob->blob = pBlob[0];

		/* Shift: */
		for(i=(BLOB_NUM-1); i>0; --i)
		{
			pFVBlob->BlobSeq[i] = pFVBlob->BlobSeq[i-1];
		}

		pFVBlob->BlobSeq[0] = pBlob[0];

		if(m_Dim>0)
		{   /* Calculate FV position: */
			FV[0] = CV_BLOB_X(pBlob);
			FV[1] = CV_BLOB_Y(pBlob);
		}

		if(m_Dim<=2)
		{   /* Add new FV if position is enough: */
			*(int*)(FV+m_Dim) = CV_BLOB_ID(pBlob);
			cvSeqPush( m_pFVSeq, FV );
		}
		else if(pFVBlob->FrameNum > BLOB_NUM)
		{   /* Calculate velocity for more complex FV: */
			float       AverVx = 0;
			float       AverVy = 0;
			{   /* Average velocity: */
				CvBlob* pBlobSeq = pFVBlob->BlobSeq;
				int     i;
				for(i=1;i<BLOB_NUM;++i)
				{
					AverVx += CV_BLOB_X(pBlobSeq+i-1)-CV_BLOB_X(pBlobSeq+i);
					AverVy += CV_BLOB_Y(pBlobSeq+i-1)-CV_BLOB_Y(pBlobSeq+i);
				}
				AverVx /= BLOB_NUM-1;
				AverVy /= BLOB_NUM-1;

				FV[2] = AverVx;
				FV[3] = AverVy;
			}

			if(m_Dim>4)
			{   /* State duration: */
				float T = (CV_BLOB_WX(pBlob)+CV_BLOB_WY(pBlob))*0.01f;

				if( fabs(AverVx) < T && fabs(AverVy) < T)
					pFVBlob->state++;
				else
					pFVBlob->state=0;
				FV[4] = (float)pFVBlob->state;
			} /* State duration. */

			/* Add new FV: */
			*(int*)(FV+m_Dim) = CV_BLOB_ID(pBlob);
			cvSeqPush( m_pFVSeq, FV );

		} /* If velocity is calculated. */

		pFVBlob->FrameNum++;
		pFVBlob->LastFrame = m_Frame;
	};  /* AddBlob */

	void Process(IplImage* pImg, IplImage* /*pFG*/)
	{
		int i;
		if(!m_ClearFlag) Clear();
		for(i=m_BlobList.GetBlobNum(); i>0; --i)
		{   /* Delete unused blob: */
			DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlob(i-1);
			if(pFVBlob->LastFrame < m_Frame)
			{
				m_BlobList.DelBlob(i-1);
			}
		} /* Check next blob in list. */

		m_FVMin[0] = 0;
		m_FVMin[1] = 0;
		m_FVMax[0] = (float)(pImg->width-1);
		m_FVMax[1] = (float)(pImg->height-1);
		m_FVVar[0] = m_FVMax[0]*0.01f;
		m_FVVar[1] = m_FVMax[1]*0.01f;
		m_FVVar[2] = (float)(pImg->width-1)/1440.0f;
		m_FVMax[2] = (float)(pImg->width-1)*0.02f;
		m_FVMin[2] = -m_FVMax[2];
		m_FVVar[3] = (float)(pImg->width-1)/1440.0f;
		m_FVMax[3] = (float)(pImg->height-1)*0.02f;
		m_FVMin[3] = -m_FVMax[3];
		m_FVMax[4] = 25*32.0f; /* max state is 32 sec */
		m_FVMin[4] = 0;
		m_FVVar[4] = 10;

		m_Frame++;
		m_ClearFlag = 0;
	};
	virtual void    Release(){delete this;};
	virtual int     GetFVSize(){return m_Dim;};
	virtual int     GetFVNum()
	{
		return m_pFVSeq->total;
	};

	virtual float*  GetFV(int index, int* pFVID)
	{
		float* pFV = (float*)cvGetSeqElem( m_pFVSeq, index );
		if(pFVID)pFVID[0] = *(int*)(pFV+m_Dim);
		return pFV;
	};
	virtual float*  GetFVMin(){return m_FVMin;}; /* returned pointer to array of minimal values of FV, if return 0 then FVrange is not exist */
	virtual float*  GetFVMax(){return m_FVMax;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
	virtual float*  GetFVVar(){return m_FVVar;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
};/* CvBlobTrackFVGenN */

CvBlobTrackFVGen* cvCreateFVGenP(){return (CvBlobTrackFVGen*)new CvBlobTrackFVGenN(2);}
CvBlobTrackFVGen* cvCreateFVGenPV(){return (CvBlobTrackFVGen*)new CvBlobTrackFVGenN(4);}
CvBlobTrackFVGen* cvCreateFVGenPVS(){return (CvBlobTrackFVGen*)new CvBlobTrackFVGenN(5);}
#undef MAX_FV_SIZE

#define MAX_FV_SIZE 4
class CvBlobTrackFVGenSS: public CvBlobTrackFVGen
{
private:
	CvBlobSeq       m_BlobList;
	CvMemStorage*   m_pMem;
	CvSeq*          m_pFVSeq;
	float           m_FVMax[MAX_FV_SIZE];
	float           m_FVMin[MAX_FV_SIZE];
	float           m_FVVar[MAX_FV_SIZE];
	int             m_Dim;
	CvBlob          m_BlobSeq[BLOB_NUM];
	int             m_Frame;
	int             m_State;
	int             m_LastFrame;
	int             m_ClearFlag;
	void Clear()
	{
		cvClearMemStorage(m_pMem);
		m_pFVSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(float)*(m_Dim+1), m_pMem);
		m_ClearFlag = 1;
	}
public:
	CvBlobTrackFVGenSS(int dim = 2 ):m_BlobList(sizeof(DefBlobFVN))
	{
		int i;
		assert(dim <= MAX_FV_SIZE);
		m_Dim = dim;
		for(i=0;i<m_Dim;++i)
		{
			m_FVVar[i] = 0.01f;
			m_FVMax[i] = 1;
			m_FVMin[i] = 0;
		}
		m_Frame = 0;
		m_State = 0;
		m_pMem = cvCreateMemStorage();
		m_pFVSeq = NULL;

		SetModuleName("SS");
	};
	~CvBlobTrackFVGenSS()
	{
		if(m_pMem)cvReleaseMemStorage(&m_pMem);
	};

	void AddBlob(CvBlob* pBlob)
	{
		//float       FV[MAX_FV_SIZE+1];
		int         i;
		DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));

		if(!m_ClearFlag) Clear();

		if(pFVBlob==NULL)
		{
			DefBlobFVN BlobNew;
			BlobNew.blob = pBlob[0];
			BlobNew.LastFrame = m_Frame;
			BlobNew.state = 0;;
			BlobNew.FrameNum = 0;
			m_BlobList.AddBlob((CvBlob*)&BlobNew);
			pFVBlob = (DefBlobFVN*)m_BlobList.GetBlobByID(CV_BLOB_ID(pBlob));
		} /* Add new record if necessary. */

		/* Shift: */
		for(i=(BLOB_NUM-1); i>0; --i)
		{
			pFVBlob->BlobSeq[i] = pFVBlob->BlobSeq[i-1];
		}

		pFVBlob->BlobSeq[0] = pBlob[0];

		if(pFVBlob->FrameNum > BLOB_NUM)
		{   /* Average velocity: */
			CvBlob* pBlobSeq = pFVBlob->BlobSeq;
			float   T = (CV_BLOB_WX(pBlob)+CV_BLOB_WY(pBlob))*0.01f;
			float   AverVx = 0;
			float   AverVy = 0;
			int     i;
			for(i=1; i<BLOB_NUM; ++i)
			{
				AverVx += CV_BLOB_X(pBlobSeq+i-1)-CV_BLOB_X(pBlobSeq+i);
				AverVy += CV_BLOB_Y(pBlobSeq+i-1)-CV_BLOB_Y(pBlobSeq+i);
			}
			AverVx /= BLOB_NUM-1;
			AverVy /= BLOB_NUM-1;

			if( fabs(AverVx) < T && fabs(AverVy) < T)
				pFVBlob->state++;
			else
				pFVBlob->state=0;
		}

		if(pFVBlob->state == 5)
		{   /* Object is stopped:  */
			float   FV[MAX_FV_SIZE];
			FV[0] = pFVBlob->blob.x;
			FV[1] = pFVBlob->blob.y;
			FV[2] = pFVBlob->BlobSeq[0].x;
			FV[3] = pFVBlob->BlobSeq[0].y;
			*(int*)(FV+m_Dim) = CV_BLOB_ID(pBlob);
			cvSeqPush( m_pFVSeq, FV );
		} /* Object is stopped. */

		pFVBlob->FrameNum++;
		pFVBlob->LastFrame = m_Frame;
	};  /* AddBlob */
	void Process(IplImage* pImg, IplImage* /*pFG*/)
	{
		int i;

		if(!m_ClearFlag) Clear();

		for(i=m_BlobList.GetBlobNum();i>0;--i)
		{   /* Delete unused blob: */
			DefBlobFVN* pFVBlob = (DefBlobFVN*)m_BlobList.GetBlob(i-1);
			if(pFVBlob->LastFrame < m_Frame)
			{
				float   FV[MAX_FV_SIZE+1];
				FV[0] = pFVBlob->blob.x;
				FV[1] = pFVBlob->blob.y;
				FV[2] = pFVBlob->BlobSeq[0].x;
				FV[3] = pFVBlob->BlobSeq[0].y;
				*(int*)(FV+m_Dim) = CV_BLOB_ID(pFVBlob);
				cvSeqPush( m_pFVSeq, FV );
				m_BlobList.DelBlob(i-1);
			}
		} /* Check next blob in list. */

		/* Set max min range: */
		m_FVMin[0] = 0;
		m_FVMin[1] = 0;
		m_FVMin[2] = 0;
		m_FVMin[3] = 0;
		m_FVMax[0] = (float)(pImg->width-1);
		m_FVMax[1] = (float)(pImg->height-1);
		m_FVMax[2] = (float)(pImg->width-1);
		m_FVMax[3] = (float)(pImg->height-1);
		m_FVVar[0] = m_FVMax[0]*0.01f;
		m_FVVar[1] = m_FVMax[1]*0.01f;
		m_FVVar[2] = m_FVMax[2]*0.01f;
		m_FVVar[3] = m_FVMax[3]*0.01f;

		m_Frame++;
		m_ClearFlag = 0;
	};
	virtual void    Release(){delete this;};
	virtual int     GetFVSize(){return m_Dim;};
	virtual int     GetFVNum()
	{
		return m_pFVSeq->total;
	};

	virtual float*  GetFV(int index, int* pFVID)
	{
		float* pFV = (float*)cvGetSeqElem( m_pFVSeq, index );
		if(pFVID)pFVID[0] = *(int*)(pFV+m_Dim);
		return pFV;
	};

	virtual float*  GetFVMin(){return m_FVMin;}; /* returned pointer to array of minimal values of FV, if return 0 then FVrange is not exist */
	virtual float*  GetFVMax(){return m_FVMax;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
	virtual float*  GetFVVar(){return m_FVVar;}; /* returned pointer to array of maximal values of FV, if return 0 then FVrange is not exist */
};/* CvBlobTrackFVGenSS */

CvBlobTrackFVGen* cvCreateFVGenSS(){return (CvBlobTrackFVGen*)new CvBlobTrackFVGenSS;}

/*======================= TRAJECTORY ANALYZER MODULES =====================*/
/* Trajectory Analyser module */
#define SPARSE  0
#define ND      1
#define BYSIZE  -1
class DefMat
{
private:
	CvSparseMatIterator m_SparseIterator;
	CvSparseNode*       m_pSparseNode;
	int*                m_IDXs;
	int                 m_Dim;

public:
	CvSparseMat*        m_pSparse;
	CvMatND*            m_pND;
	int                 m_Volume;
	int                 m_Max;
	DefMat(int dim = 0, int* sizes = NULL, int type = SPARSE)
	{
		/* Create sparse or ND matrix but not both: */
		m_pSparseNode = NULL;
		m_pSparse = NULL;
		m_pND = NULL;
		m_Volume = 0;
		m_Max = 0;
		m_IDXs = NULL;
		m_Dim = 0;
		if(dim>0 && sizes != 0)
			Realloc(dim, sizes, type);
	}
	~DefMat()
	{
		if(m_pSparse)cvReleaseSparseMat(&m_pSparse);
		if(m_pND)cvReleaseMatND(&m_pND);
		if(m_IDXs) cvFree(&m_IDXs);
	}

	void Realloc(int dim, int* sizes, int type = SPARSE)
	{
		if(m_pSparse)cvReleaseSparseMat(&m_pSparse);
		if(m_pND)cvReleaseMatND(&m_pND);

		if(type == BYSIZE )
		{
			int size = 0;
			int i;
			for(size=1,i=0;i<dim;++i)
			{
				size *= sizes[i];
			}
			size *= sizeof(int);
			if(size > (2<<20))
			{ /* if size > 1M */
				type = SPARSE;
			}
			else
			{
				type = ND;
			}
		} /* Define matrix type. */

		if(type == SPARSE)
		{
			m_pSparse = cvCreateSparseMat( dim, sizes, CV_32SC1 );
			m_Dim = dim;
		}
		if(type == ND )
		{
			m_pND = cvCreateMatND( dim, sizes, CV_32SC1 );
			cvZero(m_pND);
			m_IDXs = (int*)cvAlloc(sizeof(int)*dim);
			m_Dim = dim;
		}
		m_Volume = 0;
		m_Max = 0;
	}
	void Save(const char* File)
	{
		if(m_pSparse)cvSave(File, m_pSparse );
		if(m_pND)cvSave(File, m_pND );
	}
	void Save(CvFileStorage* fs, const char* name)
	{
		if(m_pSparse)
		{
			cvWrite(fs, name, m_pSparse );
		}
		else if(m_pND)
		{
			cvWrite(fs, name, m_pND );
		}
	}
	void Load(const char* File)
	{
		CvFileStorage* fs = cvOpenFileStorage( File, NULL, CV_STORAGE_READ );
		if(fs)
		{
			void* ptr;
			if(m_pSparse) cvReleaseSparseMat(&m_pSparse);
			if(m_pND) cvReleaseMatND(&m_pND);
			m_Volume = 0;
			m_Max = 0;
			ptr = cvLoad(File);
			if(ptr && CV_IS_MATND_HDR(ptr)) m_pND = (CvMatND*)ptr;
			if(ptr && CV_IS_SPARSE_MAT_HDR(ptr)) m_pSparse = (CvSparseMat*)ptr;
			cvReleaseFileStorage(&fs);
		}
		AfterLoad();
	} /* Load. */

	void Load(CvFileStorage* fs, CvFileNode* node, const char* name)
	{
		CvFileNode* n = cvGetFileNodeByName(fs,node,name);
		void* ptr = n?cvRead(fs,n):NULL;
		if(ptr)
		{
			if(m_pSparse) cvReleaseSparseMat(&m_pSparse);
			if(m_pND) cvReleaseMatND(&m_pND);
			m_Volume = 0;
			m_Max = 0;
			if(CV_IS_MATND_HDR(ptr)) m_pND = (CvMatND*)ptr;
			if(CV_IS_SPARSE_MAT_HDR(ptr)) m_pSparse = (CvSparseMat*)ptr;
		}
		else
		{
			printf("WARNING!!! Can't load %s matrix\n",name);
		}
		AfterLoad();
	} /* Load. */

	void AfterLoad()
	{
		m_Volume = 0;
		m_Max = 0;
		if(m_pSparse)
		{   /* Calculate Volume of loaded hist: */
			CvSparseMatIterator mat_iterator;
			CvSparseNode* node = cvInitSparseMatIterator( m_pSparse, &mat_iterator );

			for( ; node != 0; node = cvGetNextSparseNode( &mat_iterator ))
			{
				int val = *(int*)CV_NODE_VAL( m_pSparse, node ); /* get value of the element
                                                                (assume that the type is CV_32SC1) */
				m_Volume += val;
				if(m_Max < val)m_Max = val;
			}
		} /* Calculate Volume of loaded hist. */

		if(m_pND)
		{   /* Calculate Volume of loaded hist: */
			CvMat   mat;
			double  max_val;
			double  vol;
			cvGetMat( m_pND, &mat, NULL, 1 );

			vol = cvSum(&mat).val[0];
			m_Volume = cvRound(vol);
			cvMinMaxLoc( &mat, NULL, &max_val);
			m_Max = cvRound(max_val);
			/* MUST BE WRITTEN LATER */
		} /* Calculate Volume of loaded hist. */
	} /* AfterLoad. */

	int* GetPtr(int* indx)
	{
		if(m_pSparse) return (int*)cvPtrND( m_pSparse, indx, NULL, 1, NULL);
		if(m_pND) return  (int*)cvPtrND( m_pND, indx, NULL, 1, NULL);
		return NULL;
	} /* GetPtr. */

	int GetVal(int* indx)
	{
		int* p = GetPtr(indx);
		if(p)return p[0];
		return -1;
	} /* GetVal. */

	int Add(int* indx, int val)
	{
		int  NewVal;
		int* pVal = GetPtr(indx);
		if(pVal == NULL) return -1;
		pVal[0] += val;
		NewVal = pVal[0];
		m_Volume += val;
		if(m_Max < NewVal)m_Max = NewVal;
		return NewVal;
	} /* Add. */

	void Add(DefMat* pMatAdd)
	{
		int*    pIDXS = NULL;
		int     Val = 0;
		for(Val = pMatAdd->GetNext(&pIDXS, 1 );pIDXS;Val=pMatAdd->GetNext(&pIDXS, 0 ))
		{
			Add(pIDXS,Val);
		}
	} /* Add. */

	int SetMax(int* indx, int val)
	{
		int  NewVal;
		int* pVal = GetPtr(indx);
		if(pVal == NULL) return -1;
		if(val > pVal[0])
		{
			m_Volume += val-pVal[0];
			pVal[0] = val;
		}
		NewVal = pVal[0];
		if(m_Max < NewVal)m_Max = NewVal;
		return NewVal;
	} /* Add. */

	int GetNext(int** pIDXS, int init = 0)
	{
		int     Val = 0;
		pIDXS[0] = NULL;
		if(m_pSparse)
		{
			m_pSparseNode = (init || m_pSparseNode==NULL)?
					cvInitSparseMatIterator( m_pSparse, &m_SparseIterator ):
						cvGetNextSparseNode( &m_SparseIterator );

					if(m_pSparseNode)
					{
						int* pVal = (int*)CV_NODE_VAL( m_pSparse, m_pSparseNode );
						if(pVal)Val = pVal[0];
						pIDXS[0] = CV_NODE_IDX( m_pSparse, m_pSparseNode );
					}
		}/* Sparse matrix. */

		if(m_pND)
		{
			int i;
			if(init)
			{
				for(i=0;i<m_Dim;++i)
				{
					m_IDXs[i] = cvGetDimSize( m_pND, i )-1;
				}
				pIDXS[0] = m_IDXs;
				Val = GetVal(m_IDXs);
			}
			else
			{
				for(i=0;i<m_Dim;++i)
				{
					if((m_IDXs[i]--)>0)
						break;
					m_IDXs[i] = cvGetDimSize( m_pND, i )-1;
				}
				if(i==m_Dim)
				{
					pIDXS[0] = NULL;
				}
				else
				{
					pIDXS[0] = m_IDXs;
					Val = GetVal(m_IDXs);
				}

			} /* Get next ND. */

		} /* Sparse matrix. */

		return Val;

	}; /* GetNext. */
};

#define FV_NUM 10
#define FV_SIZE 10
typedef struct DefTrackFG
{
	CvBlob                  blob;
	//    CvBlobTrackFVGen*       pFVGen;
	int                     LastFrame;
	float                   state;
	DefMat*                 pHist;
} DefTrackFG;
class CvBlobTrackAnalysisHist : public CvBlobTrackAnalysis
{
	/*---------------- Internal functions: --------------------*/
private:
	int                 m_BinNumParam;
	int                 m_SmoothRadius;
	const char*         m_SmoothKernel;
	float               m_AbnormalThreshold;
	int                 m_TrackNum;
	int                 m_Frame;
	int                 m_BinNum;
	char                m_DataFileName[1024];
	int                 m_Dim;
	int*                m_Sizes;
	DefMat              m_HistMat;
	int                 m_HistVolumeSaved;
	int*                m_pFVi;
	int*                m_pFViVar;
	int*                m_pFViVarRes;
	CvBlobSeq           m_TrackFGList;
	//CvBlobTrackFVGen*   (*m_CreateFVGen)();
	CvBlobTrackFVGen*   m_pFVGen;
	void SaveHist()
	{
		if(m_DataFileName[0])
		{
			m_HistMat.Save(m_DataFileName);
			m_HistVolumeSaved = m_HistMat.m_Volume;
		}
	};
	void LoadHist()
	{
		if(m_DataFileName[0])m_HistMat.Load(m_DataFileName);
		m_HistVolumeSaved = m_HistMat.m_Volume;
	}
	void AllocData()
	{   /* AllocData: */
		m_pFVi = (int*)cvAlloc(sizeof(int)*m_Dim);
		m_pFViVar = (int*)cvAlloc(sizeof(int)*m_Dim);
		m_pFViVarRes = (int*)cvAlloc(sizeof(int)*m_Dim);
		m_Sizes = (int*)cvAlloc(sizeof(int)*m_Dim);

		{   /* Create init sparce matrix: */
			int     i;
			for(i=0;i<m_Dim;++i)m_Sizes[i] = m_BinNum;
			m_HistMat.Realloc(m_Dim,m_Sizes,SPARSE);
			m_HistVolumeSaved = 0;
		} /* Create init sparce matrix. */
	} /* AllocData. */

	void FreeData()
	{   /* FreeData. */
		int i;
		for(i=m_TrackFGList.GetBlobNum();i>0;--i)
		{
			//DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(i-1);
			//            pF->pFVGen->Release();
			m_TrackFGList.DelBlob(i-1);
		}
		cvFree(&m_pFVi);
		cvFree(&m_pFViVar);
		cvFree(&m_pFViVarRes);
		cvFree(&m_Sizes);
	} /* FreeData. */

	virtual void ParamUpdate()
	{
		if(m_BinNum != m_BinNumParam)
		{
			FreeData();
			m_BinNum = m_BinNumParam;
			AllocData();
		}
	}
public:
	CvBlobTrackAnalysisHist(CvBlobTrackFVGen*   (*createFVGen)()):m_TrackFGList(sizeof(DefTrackFG))
	{
		m_pFVGen = createFVGen();
		m_Dim = m_pFVGen->GetFVSize();
		m_Frame = 0;
		m_pFVi = 0;
		m_TrackNum = 0;
		m_BinNum = 32;
		m_DataFileName[0] = 0;

		m_AbnormalThreshold = 0.02f;
		AddParam("AbnormalThreshold",&m_AbnormalThreshold);
		CommentParam("AbnormalThreshold","If trajectory histogram value is lesst then <AbnormalThreshold*DataBaseTrackNum> then trajectory is abnormal");

		m_SmoothRadius = 1;
		AddParam("SmoothRadius",&m_SmoothRadius);
		CommentParam("AbnormalThreshold","Radius (in bins) for histogram smoothing");

		m_SmoothKernel = "L";
		AddParam("SmoothKernel",&m_SmoothKernel);
		CommentParam("SmoothKernel","L - Linear, G - Gaussian");


		m_BinNumParam = m_BinNum;
		AddParam("BinNum",&m_BinNumParam);
		CommentParam("BinNum","Number of bin for each dimention of feature vector");

		AllocData();
		SetModuleName("Hist");

	} /* Constructor. */

	~CvBlobTrackAnalysisHist()
	{
		SaveHist();
		FreeData();
		m_pFVGen->Release();
	} /* Destructor. */

	/*----------------- Interface: --------------------*/
	virtual void    AddBlob(CvBlob* pBlob)
	{
		DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(CV_BLOB_ID(pBlob));
		if(pF == NULL)
		{ /* create new filter */
			DefTrackFG F;
			F.state = 0;
			F.blob = pBlob[0];
			F.LastFrame = m_Frame;
			//            F.pFVGen = m_CreateFVGen();
			F.pHist = new DefMat(m_Dim,m_Sizes,SPARSE);
			m_TrackFGList.AddBlob((CvBlob*)&F);
			pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(CV_BLOB_ID(pBlob));
		}

		assert(pF);
		pF->blob = pBlob[0];
		pF->LastFrame = m_Frame;
		m_pFVGen->AddBlob(pBlob);
	};
	virtual void Process(IplImage* pImg, IplImage* pFG)
	{
		int i;
		m_pFVGen->Process(pImg, pFG);
		int SK = m_SmoothKernel[0];

		for(i=0; i<m_pFVGen->GetFVNum(); ++i)
		{
			int         BlobID = 0;
			float*      pFV = m_pFVGen->GetFV(i,&BlobID);
			float*      pFVMax = m_pFVGen->GetFVMax();
			float*      pFVMin = m_pFVGen->GetFVMin();
			DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(BlobID);
			int         HistVal = 1;

			if(pFV==NULL) break;

			pF->LastFrame = m_Frame;

			{   /* Binarize FV: */
				int         j;
			for(j=0; j<m_Dim; ++j)
			{
				int     index;
				float   f0 = pFVMin?pFVMin[j]:0;
				float   f1 = pFVMax?pFVMax[j]:1;
				assert(f1>f0);
				index = cvRound((m_BinNum-1)*(pFV[j]-f0)/(f1-f0));
				if(index<0)index=0;
				if(index>=m_BinNum)index=m_BinNum-1;
				m_pFVi[j] = index;
			}
			}

			HistVal = m_HistMat.GetVal(m_pFVi);/* get bin value*/
			pF->state = 0;
			{   /* Calculate state: */
				float   T = m_HistMat.m_Max*m_AbnormalThreshold; /* calc threshold */

				if(m_TrackNum>0) T = 256.0f * m_TrackNum*m_AbnormalThreshold;
				if(T>0)
				{
					pF->state = (T - HistVal)/(T*0.2f) + 0.5f;
				}
				if(pF->state<0)pF->state=0;
				if(pF->state>1)pF->state=1;
			}

			{   /* If it is a new FV then add it to trajectory histogram: */
				int i,flag = 1;
				int r = m_SmoothRadius;

				//                    printf("BLob %3d NEW FV [", CV_BLOB_ID(pF));
				//                    for(i=0;i<m_Dim;++i) printf("%d,", m_pFVi[i]);
				//                    printf("]");

				for(i=0; i<m_Dim; ++i)
				{
					m_pFViVar[i]=-r;
				}

				while(flag)
				{
					float   dist = 0;
					int     HistAdd = 0;
					int     i;
					int     good = 1;
					for(i=0; i<m_Dim; ++i)
					{
						m_pFViVarRes[i] = m_pFVi[i]+m_pFViVar[i];
						if(m_pFViVarRes[i]<0) good= 0;
						if(m_pFViVarRes[i]>=m_BinNum) good= 0;
						dist += m_pFViVar[i]*m_pFViVar[i];
					}/* Calculate next dimension. */

					if(SK=='G' || SK=='g')
					{
						double dist2 = dist/(r*r);
						HistAdd = cvRound(256*exp(-dist2)); /* Hist Add for (dist=1) = 25.6*/
					}
					else if(SK=='L' || SK=='l')
					{
						dist = (float)(sqrt(dist)/(r+1));
						HistAdd = cvRound(256*(1-dist));
					}
					else
					{
						HistAdd = 255; /* Flat smoothing. */
					}

					if(good && HistAdd>0)
					{   /* Update histogram: */
						assert(pF->pHist);
						pF->pHist->SetMax(m_pFViVarRes, HistAdd);
					}   /* Update histogram. */

					for(i=0; i<m_Dim; ++i)
					{   /* Next config: */
						if((m_pFViVar[i]++) < r)
							break;
						m_pFViVar[i] = -r;
					}   /* Increase next dimension variable. */
					if(i==m_Dim)break;
				}   /* Next variation. */
			} /* If new FV. */
		} /* Next FV. */

		{   /* Check all blobs on list: */
			int i;
			for(i=m_TrackFGList.GetBlobNum(); i>0; --i)
			{   /* Add histogram and delete blob from list: */
				DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(i-1);
				if(pF->LastFrame+3 < m_Frame && pF->pHist)
				{
					m_HistMat.Add(pF->pHist);
					delete pF->pHist;
					m_TrackNum++;
					m_TrackFGList.DelBlob(i-1);
				}
			}/* next blob */
		}

		m_Frame++;

		if(m_Wnd)
		{   /* Debug output: */
			int*        idxs = NULL;
			int         Val = 0;
			IplImage*   pI = cvCloneImage(pImg);

			cvZero(pI);

			for(Val = m_HistMat.GetNext(&idxs,1); idxs; Val=m_HistMat.GetNext(&idxs,0))
			{   /* Draw all elements: */
				float   vf;
				int     x,y;

				if(!idxs) break;
				if(Val == 0) continue;

				vf = (float)Val/(m_HistMat.m_Max?m_HistMat.m_Max:1);
				x = cvRound((float)(pI->width-1)*(float)idxs[0] / (float)m_BinNum);
				y = cvRound((float)(pI->height-1)*(float)idxs[1] / (float)m_BinNum);

				cvCircle(pI, cvPoint(x,y), cvRound(vf*pI->height/(m_BinNum*2)),CV_RGB(255,0,0),CV_FILLED);
				if(m_Dim > 3)
				{
					int dx = -2*(idxs[2]-m_BinNum/2);
					int dy = -2*(idxs[3]-m_BinNum/2);
					cvLine(pI,cvPoint(x,y),cvPoint(x+dx,y+dy),CV_RGB(0,cvRound(vf*255),1));
				}
				if( m_Dim==4 &&
						m_pFVGen->GetFVMax()[0]==m_pFVGen->GetFVMax()[2] &&
						m_pFVGen->GetFVMax()[1]==m_pFVGen->GetFVMax()[3])
				{
					int x = cvRound((float)(pI->width-1)*(float)idxs[2] / (float)m_BinNum);
					int y = cvRound((float)(pI->height-1)*(float)idxs[3] / (float)m_BinNum);
					cvCircle(pI, cvPoint(x,y), cvRound(vf*pI->height/(m_BinNum*2)),CV_RGB(0,0,255),CV_FILLED);
				}
			} /* Draw all elements. */

			for(i=m_TrackFGList.GetBlobNum();i>0;--i)
			{
				DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(i-1);
				DefMat* pHist = pF?pF->pHist:NULL;

				if(pHist==NULL) continue;

				for(Val = pHist->GetNext(&idxs,1);idxs;Val=pHist->GetNext(&idxs,0))
				{   /* Draw all elements: */
					float   vf;
				int     x,y;

				if(!idxs) break;
				if(Val == 0) continue;

				vf = (float)Val/(pHist->m_Max?pHist->m_Max:1);
				x = cvRound((float)(pI->width-1)*(float)idxs[0] / (float)m_BinNum);
				y = cvRound((float)(pI->height-1)*(float)idxs[1] / (float)m_BinNum);

				cvCircle(pI, cvPoint(x,y), cvRound(2*vf),CV_RGB(0,0,cvRound(255*vf)),CV_FILLED);
				if(m_Dim > 3)
				{
					int dx = -2*(idxs[2]-m_BinNum/2);
					int dy = -2*(idxs[3]-m_BinNum/2);
					cvLine(pI,cvPoint(x,y),cvPoint(x+dx,y+dy),CV_RGB(0,0,255));
				}
				if( m_Dim==4 &&
						m_pFVGen->GetFVMax()[0]==m_pFVGen->GetFVMax()[2] &&
						m_pFVGen->GetFVMax()[1]==m_pFVGen->GetFVMax()[3])
				{ /* if SS feature vector */
					int x = cvRound((float)(pI->width-1)*(float)idxs[2] / (float)m_BinNum);
				int y = cvRound((float)(pI->height-1)*(float)idxs[3] / (float)m_BinNum);
				cvCircle(pI, cvPoint(x,y), cvRound(vf*pI->height/(m_BinNum*2)),CV_RGB(0,0,255),CV_FILLED);
				}
				} /* Draw all elements. */
			} /* Next track. */

			//cvNamedWindow("Hist",0);
			//cvShowImage("Hist", pI);
			cvReleaseImage(&pI);
		}
	};

	float GetState(int BlobID)
	{
		DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(BlobID);
		return pF?pF->state:0.0f;
	};

	/* Return 0 if trajectory is normal;
       rreturn >0 if trajectory abnormal. */
	virtual const char*   GetStateDesc(int BlobID)
	{
		if(GetState(BlobID)>0.5) return "abnormal";
		return NULL;
	}

	virtual void    SetFileName(const char* DataBaseName)
	{
		if(m_HistMat.m_Volume!=m_HistVolumeSaved)SaveHist();
		m_DataFileName[0] = 0;

		if(DataBaseName)
		{
			strncpy(m_DataFileName,DataBaseName,1000);
			strcat(m_DataFileName, ".yml");
		}
		LoadHist();
	};

	virtual void SaveState(CvFileStorage* fs)
	{
		int b, bN = m_TrackFGList.GetBlobNum();
		cvWriteInt(fs,"BlobNum",bN);
		cvStartWriteStruct(fs,"BlobList",CV_NODE_SEQ);

		for(b=0; b<bN; ++b)
		{
			DefTrackFG* pF = (DefTrackFG*)m_TrackFGList.GetBlob(b);
			cvStartWriteStruct(fs,NULL,CV_NODE_MAP);
			cvWriteStruct(fs,"Blob", &(pF->blob), "ffffi");
			cvWriteInt(fs,"LastFrame",pF->LastFrame);
			cvWriteReal(fs,"State",pF->state);
			pF->pHist->Save(fs, "Hist");
			cvEndWriteStruct(fs);
		}
		cvEndWriteStruct(fs);
		m_HistMat.Save(fs, "Hist");
	};

	virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
	{
		CvFileNode* pBLN = cvGetFileNodeByName(fs,node,"BlobList");

		if(pBLN && CV_NODE_IS_SEQ(pBLN->tag))
		{
			int b, bN = pBLN->data.seq->total;
			for(b=0; b<bN; ++b)
			{
				DefTrackFG* pF = NULL;
				CvBlob      Blob;
				CvFileNode* pBN = (CvFileNode*)cvGetSeqElem(pBLN->data.seq,b);

				assert(pBN);
				cvReadStructByName(fs, pBN, "Blob", &Blob, "ffffi");
				AddBlob(&Blob);
				pF = (DefTrackFG*)m_TrackFGList.GetBlobByID(Blob.ID);
				if(pF==NULL) continue;
				assert(pF);
				pF->state = (float)cvReadIntByName(fs,pBN,"State",cvRound(pF->state));
				assert(pF->pHist);
				pF->pHist->Load(fs,pBN,"Hist");
			}
		}

		m_HistMat.Load(fs, node, "Hist");
	}; /* LoadState */


	virtual void    Release(){ delete this; };

};



CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistP()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenP);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistPV()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenPV);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistPVS()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenPVS);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisHistSS()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisHist(cvCreateFVGenSS);}

typedef struct DefTrackSVM
{
	CvBlob                  blob;
	//    CvBlobTrackFVGen*       pFVGen;
	int                     LastFrame;
	float                   state;
	CvBlob                  BlobLast;
	CvSeq*                  pFVSeq;
	CvMemStorage*           pMem;
} DefTrackSVM;

class CvBlobTrackAnalysisSVM : public CvBlobTrackAnalysis
{
	/*---------------- Internal functions: --------------------*/
private:
	CvMemStorage*       m_pMem;
	int                 m_TrackNum;
	int                 m_Frame;
	char                m_DataFileName[1024];
	int                 m_Dim;
	float*              m_pFV;
	//CvStatModel*        m_pStatModel;
	void*               m_pStatModel;
	CvBlobSeq           m_Tracks;
	CvMat*              m_pTrainData;
	int                 m_LastTrainDataSize;
	//    CvBlobTrackFVGen*   (*m_CreateFVGen)();
	CvBlobTrackFVGen*   m_pFVGen;
	float               m_NU;
	float               m_RBFWidth;
	IplImage*           m_pStatImg; /* for debug purpose */
	CvSize              m_ImgSize;
	void RetrainStatModel()
	{
		///////// !!!!! TODO !!!!! Repair /////////////
#if 0
		float               nu = 0;
		CvSVMModelParams    SVMParams = {0};
		CvStatModel*        pM = NULL;


		memset(&SVMParams,0,sizeof(SVMParams));
		SVMParams.svm_type = CV_SVM_ONE_CLASS;
		SVMParams.kernel_type = CV_SVM_RBF;
		SVMParams.gamma = 2.0/(m_RBFWidth*m_RBFWidth);
		SVMParams.nu = m_NU;
		SVMParams.degree = 3;
		SVMParams.criteria = cvTermCriteria(CV_TERMCRIT_EPS, 100, 1e-3 );
		SVMParams.C = 1;
		SVMParams.p = 0.1;


		if(m_pTrainData == NULL) return;
		{
			int64       TickCount = cvGetTickCount();
			printf("Frame: %d\n           Retrain SVM\nData Size = %d\n",m_Frame, m_pTrainData->rows);
			pM = cvTrainSVM( m_pTrainData,CV_ROW_SAMPLE, NULL, (CvStatModelParams*)&SVMParams, NULL, NULL);
			TickCount = cvGetTickCount() - TickCount ;
			printf("SV Count = %d\n",((CvSVMModel*)pM)->sv_total);
			printf("Processing Time = %.1f(ms)\n",TickCount/(1000*cvGetTickFrequency()));

		}
		if(pM==NULL) return;
		if(m_pStatModel) cvReleaseStatModel(&m_pStatModel);
		m_pStatModel = pM;

		if(m_pTrainData && m_Wnd)
		{
			float       MaxVal = 0;
			IplImage*   pW = cvCreateImage(m_ImgSize,IPL_DEPTH_32F,1);
			IplImage*   pI = cvCreateImage(m_ImgSize,IPL_DEPTH_8U,1);
			float*      pFVVar = m_pFVGen->GetFVVar();
			int     i;
			cvZero(pW);

			for(i=0; i<m_pTrainData->rows; ++i)
			{   /* Draw all elements: */
				float*          pFV = (float*)(m_pTrainData->data.ptr + m_pTrainData->step*i);
				int             x = cvRound(pFV[0]*pFVVar[0]);
				int             y = cvRound(pFV[1]*pFVVar[1]);
				float           r;

				if(x<0)x=0;
				if(x>=pW->width)x=pW->width-1;
				if(y<0)y=0;
				if(y>=pW->height)y=pW->height-1;

				r = ((float*)(pW->imageData + y*pW->widthStep))[x];
				r++;
				((float*)(pW->imageData + y*pW->widthStep))[x] = r;

				if(r>MaxVal)MaxVal=r;
			} /* Next point. */

			if(MaxVal>0)cvConvertScale(pW,pI,255/MaxVal,0);
			cvNamedWindow("SVMData",0);
			cvShowImage("SVMData",pI);
			cvSaveImage("SVMData.bmp",pI);
			cvReleaseImage(&pW);
			cvReleaseImage(&pI);
		} /* Prepare for debug. */

		if(m_pStatModel && m_Wnd && m_Dim == 2)
		{
			float*      pFVVar = m_pFVGen->GetFVVar();
			int x,y;
			if(m_pStatImg==NULL)
			{
				m_pStatImg = cvCreateImage(m_ImgSize,IPL_DEPTH_8U,1);
			}
			cvZero(m_pStatImg);

			for(y=0; y<m_pStatImg->height; y+=1) for(x=0; x<m_pStatImg->width; x+=1)
			{   /* Draw all elements: */
				float           res;
			uchar*  pData = (uchar*)m_pStatImg->imageData + x + y*m_pStatImg->widthStep;
			CvMat           FVmat;
			float           xy[2] = {x/pFVVar[0],y/pFVVar[1]};
			cvInitMatHeader( &FVmat, 1, 2, CV_32F, xy );
			res = cvStatModelPredict( m_pStatModel, &FVmat, NULL );
			pData[0]=((res>0.5)?255:0);
			} /* Next point. */

			cvNamedWindow("SVMMask",0);
			cvShowImage("SVMMask",m_pStatImg);
			cvSaveImage("SVMMask.bmp",m_pStatImg);
		} /* Prepare for debug. */
#endif
	};
	void SaveStatModel()
	{
		if(m_DataFileName[0])
		{
			if(m_pTrainData)cvSave(m_DataFileName, m_pTrainData);
		}
	};
	void LoadStatModel()
	{
		if(m_DataFileName[0])
		{
			CvMat* pTrainData = (CvMat*)cvLoad(m_DataFileName);
			if(CV_IS_MAT(pTrainData) && pTrainData->width == m_Dim)
			{
				if(m_pTrainData) cvReleaseMat(&m_pTrainData);
				m_pTrainData = pTrainData;
				RetrainStatModel();
			}
		}
	}
public:
	CvBlobTrackAnalysisSVM(CvBlobTrackFVGen*   (*createFVGen)()):m_Tracks(sizeof(DefTrackSVM))
	{
		m_pFVGen = createFVGen();
		m_Dim = m_pFVGen->GetFVSize();
		m_pFV = (float*)cvAlloc(sizeof(float)*m_Dim);
		m_Frame = 0;
		m_TrackNum = 0;
		m_pTrainData = NULL;
		m_pStatModel = NULL;
		m_DataFileName[0] = 0;
		m_pStatImg = NULL;
		m_LastTrainDataSize = 0;

		m_NU = 0.2f;
		AddParam("Nu",&m_NU);
		CommentParam("Nu","Parameters that tunes SVM border elastic");

		m_RBFWidth = 1;
		AddParam("RBFWidth",&m_RBFWidth);
		CommentParam("RBFWidth","Parameters that tunes RBF kernel function width.");

		SetModuleName("SVM");

	} /* Constructor. */

	~CvBlobTrackAnalysisSVM()
	{
		int i;
		SaveStatModel();
		for(i=m_Tracks.GetBlobNum();i>0;--i)
		{
			DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlob(i-1);
			if(pF->pMem) cvReleaseMemStorage(&pF->pMem);
			//pF->pFVGen->Release();
		}
		if(m_pStatImg)cvReleaseImage(&m_pStatImg);
		cvFree(&m_pFV);
	} /* Destructor. */

	/*----------------- Interface: --------------------*/
	virtual void    AddBlob(CvBlob* pBlob)
	{
		DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlobByID(CV_BLOB_ID(pBlob));

		m_pFVGen->AddBlob(pBlob);

		if(pF == NULL)
		{   /* Create new record: */
			DefTrackSVM F;
			F.state = 0;
			F.blob = pBlob[0];
			F.LastFrame = m_Frame;
			//F.pFVGen = m_CreateFVGen();
			F.pMem = cvCreateMemStorage();
			F.pFVSeq = cvCreateSeq(0,sizeof(CvSeq),sizeof(float)*m_Dim,F.pMem);

			F.BlobLast.x = -1;
			F.BlobLast.y = -1;
			F.BlobLast.w = -1;
			F.BlobLast.h = -1;
			m_Tracks.AddBlob((CvBlob*)&F);
			pF = (DefTrackSVM*)m_Tracks.GetBlobByID(CV_BLOB_ID(pBlob));
		}

		assert(pF);
		pF->blob = pBlob[0];
		pF->LastFrame = m_Frame;
	};

	virtual void Process(IplImage* pImg, IplImage* pFG)
	{
		int     i;
		float*  pFVVar = m_pFVGen->GetFVVar();

		m_pFVGen->Process(pImg, pFG);
		m_ImgSize = cvSize(pImg->width,pImg->height);

		for(i=m_pFVGen->GetFVNum(); i>0; --i)
		{
			int             BlobID = 0;
			float*          pFV = m_pFVGen->GetFV(i,&BlobID);
			DefTrackSVM*    pF = (DefTrackSVM*)m_Tracks.GetBlobByID(BlobID);

			if(pF && pFV)
			{   /* Process: */
				float   dx,dy;
			CvMat   FVmat;

			pF->state = 0;

			if(m_pStatModel)
			{
				int j;
				for(j=0; j<m_Dim; ++j)
				{
					m_pFV[j] = pFV[j]/pFVVar[j];
				}

				cvInitMatHeader( &FVmat, 1, m_Dim, CV_32F, m_pFV );
				//pF->state = cvStatModelPredict( m_pStatModel, &FVmat, NULL )<0.5;
				pF->state = 1.f;
			}

			dx = (pF->blob.x - pF->BlobLast.x);
			dy = (pF->blob.y - pF->BlobLast.y);

			if(pF->BlobLast.x<0 || (dx*dx+dy*dy) >= 2*2)
			{   /* Add feature vector to train data base: */
				pF->BlobLast = pF->blob;
				cvSeqPush(pF->pFVSeq,pFV);
			}
			} /* Process one blob. */
		} /* Next FV. */

		for(i=m_Tracks.GetBlobNum(); i>0; --i)
		{   /* Check each blob record: */
			DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlob(i-1);

			if(pF->LastFrame+3 < m_Frame )
			{   /* Retrain stat model and delete blob filter: */
				int                 mult = 1+m_Dim;
				int                 old_height = m_pTrainData?m_pTrainData->height:0;
				int                 height = old_height + pF->pFVSeq->total*mult;
				CvMat*              pTrainData = cvCreateMat(height, m_Dim, CV_32F);
				int                 j;
				if(m_pTrainData && pTrainData)
				{   /* Create new train data matrix: */
					int h = pTrainData->height;
					pTrainData->height = MIN(pTrainData->height, m_pTrainData->height);
					cvCopy(m_pTrainData,pTrainData);
					pTrainData->height = h;
				}

				for(j=0; j<pF->pFVSeq->total; ++j)
				{   /* Copy new data to train data: */
					float*  pFVVar = m_pFVGen->GetFVVar();
					float*  pFV = (float*)cvGetSeqElem(pF->pFVSeq,j);
					int     k;

					for(k=0; k<mult; ++k)
					{
						int t;
						float*  pTD = (float*)CV_MAT_ELEM_PTR( pTrainData[0], old_height+j*mult+k, 0);
						memcpy(pTD,pFV,sizeof(float)*m_Dim);

						if(pFVVar)for(t=0;t<m_Dim;++t)
						{   /* Scale FV: */
							pTD[t] /= pFVVar[t];
						}

						if(k>0)
						{   /* Variate: */
							for(t=0; t<m_Dim; ++t)
							{
								pTD[t] += m_RBFWidth*0.5f*(1-2.0f*rand()/(float)RAND_MAX);
							}
						}
					}
				} /* Next new datum. */

				if(m_pTrainData) cvReleaseMat(&m_pTrainData);
				m_pTrainData = pTrainData;

				/* delete track record */
				cvReleaseMemStorage(&pF->pMem);
				m_TrackNum++;
				m_Tracks.DelBlob(i-1);

			} /* End delete. */
		} /* Next track. */

		/* Retrain data each 1 minute if new data exist: */
		if(m_Frame%(25*60) == 0 && m_pTrainData && m_pTrainData->rows > m_LastTrainDataSize)
		{
			RetrainStatModel();
		}

		m_Frame++;

		if(m_Wnd && m_Dim==2)
		{   /* Debug output: */
			int         x,y;
			IplImage*   pI = cvCloneImage(pImg);

			if(m_pStatModel && m_pStatImg)

				for(y=0; y<pI->height; y+=2)
				{
					uchar*  pStatData = (uchar*)m_pStatImg->imageData + y*m_pStatImg->widthStep;
					uchar*  pData = (uchar*)pI->imageData + y*pI->widthStep;

					for(x=0;x<pI->width;x+=2)
					{   /* Draw all elements: */
						int d = pStatData[x];
						d = (d<<8) | (d^0xff);
						*(ushort*)(pData + x*3) = (ushort)d;
					}
				} /* Next line. */

			//cvNamedWindow("SVMMap",0);
			//cvShowImage("SVMMap", pI);
			cvReleaseImage(&pI);
		} /* Debug output. */
	};
	float GetState(int BlobID)
	{
		DefTrackSVM* pF = (DefTrackSVM*)m_Tracks.GetBlobByID(BlobID);
		return pF?pF->state:0.0f;
	};

	/* Return 0 if trajectory is normal;
       return >0 if trajectory abnormal. */
	virtual const char*   GetStateDesc(int BlobID)
	{
		if(GetState(BlobID)>0.5) return "abnormal";
		return NULL;
	}

	virtual void    SetFileName(char* DataBaseName)
	{
		if(m_pTrainData)SaveStatModel();
		m_DataFileName[0] = 0;
		if(DataBaseName)
		{
			strncpy(m_DataFileName,DataBaseName,1000);
			strcat(m_DataFileName, ".yml");
		}
		LoadStatModel();
	};


	virtual void    Release(){ delete this; };

}; /* CvBlobTrackAnalysisSVM. */


CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMP()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenP);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMPV()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenPV);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMPVS()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenPVS);}

CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisSVMSS()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisSVM(cvCreateFVGenSS);}
