/*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"

typedef struct DefTrackPoint
{
    float x,y,r,vx,vy,v;
} DefTrackPoint;

class DefTrackRec
{
private:
    int     ID;
public:
    DefTrackRec(int id = 0,int BlobSize = sizeof(DefTrackPoint))
    {
        ID = id;
        m_pMem = cvCreateMemStorage();
        m_pSeq = cvCreateSeq(0,sizeof(CvSeq),BlobSize,m_pMem);
    }
    ~DefTrackRec()
    {
        cvReleaseMemStorage(&m_pMem);
    };
    inline DefTrackPoint* GetPoint(int PointIndex)
    {
        return (DefTrackPoint*)cvGetSeqElem(m_pSeq,PointIndex);
    };
    inline void DelPoint(int PointIndex)
    {
        cvSeqRemove(m_pSeq,PointIndex);
    };
    inline void Clear()
    {
        cvClearSeq(m_pSeq);
    };
    inline void AddPoint(float x, float y, float r)
    {
        DefTrackPoint   p = {x,y,r,0};
        int             Num = GetPointNum();

        if(Num > 0)
        {
            DefTrackPoint* pPrev = GetPoint(Num-1);
            float   Alpha = 0.8f;
            float  dx = x-pPrev->x;
            float  dy = y-pPrev->y;
            p.vx = Alpha*dx+(1-Alpha)*pPrev->vx;
            p.vy = Alpha*dy+(1-Alpha)*pPrev->vy;
            p.v  = Alpha*dx+(1-Alpha)*pPrev->v;
        }
        AddPoint(&p);
    }

    inline void AddPoint(DefTrackPoint* pB)
    {   /* Add point and recalculate last velocities: */
        int     wnd=3;
        int     Num;
        int     i;
        cvSeqPush(m_pSeq,pB);

        Num = GetPointNum();

        for(i=MAX(0,Num-wnd-1); i<Num; ++i)
        {   /* Next updating point: */
            DefTrackPoint*  p = GetPoint(i);
            int             j0 = i - wnd;
            int             j1 = i + wnd;

            if(j0<0) j0 = 0;
            if(j1>=Num)j1=Num-1;

            if(j1>j0)
            {
                float           dt = (float)(j1-j0);
                DefTrackPoint*  p0 = GetPoint(j0);
                DefTrackPoint*  p1 = GetPoint(j1);
                p->vx = (p1->x - p0->x) / dt;
                p->vy = (p1->y - p0->y) / dt;
                p->v = (float)sqrt(p->vx*p->vx+p->vy*p->vy);
            }
        } /* Next updating point. */

#if 0
        if(0)
        {   /* Debug: */
            int i;
            printf("Blob %d: ",ID);

            for(i=0; i<GetPointNum(); ++i)
            {
                DefTrackPoint*  p = GetPoint(i);
                printf(",(%.2f,%.2f,%f.2)",p->vx,p->vy,p->v);
            }
            printf("\n");
        }
#endif
    };
    inline int GetPointNum()
    {
        return m_pSeq->total;
    };
private:
    CvMemStorage*   m_pMem;
    CvSeq*          m_pSeq;
};

/* Fill array pIdxPairs by pair of index of correspondent blobs. */
/* Return number of pairs.                                       */
/* pIdxPairs must have size not less that 2*(pSeqNum+pSeqTNum)   */
/* pTmp is pointer to memory which size is pSeqNum*pSeqTNum*16   */
typedef struct DefMatch
{
    int     Idx;  /* Previous best blob index.          */
    int     IdxT; /* Previous best template blob index. */
    double  D;    /* Blob to blob distance sum.         */
} DefMatch;

static int cvTrackMatch(DefTrackRec* pSeq, int MaxLen, DefTrackRec* pSeqT, int* pIdxPairs, void* pTmp)
{
    int         NumPair = 0;
    DefMatch*   pMT = (DefMatch*)pTmp;
    int         Num = pSeq->GetPointNum();
    int         NumT = pSeqT->GetPointNum();
    int         i,it;
    int         i0=0; /* Last point in the track sequence. */

    if(MaxLen > 0 && Num > MaxLen)
    {   /* Set new point seq len and new last point in this seq: */
        Num = MaxLen;
        i0 = pSeq->GetPointNum() - Num;
    }

    for(i=0; i<Num; ++i)
    {   /* For each point row: */
        for(it=0; it<NumT; ++it)
        {   /* For each point template column: */
            DefTrackPoint*  pB = pSeq->GetPoint(i+i0);
            DefTrackPoint*  pBT = pSeqT->GetPoint(it);
            DefMatch*       pMT_cur = pMT + i*NumT + it;
            double          dx = pB->x-pBT->x;
            double          dy = pB->y-pBT->y;
            double          D = dx*dx+dy*dy;
            int             DI[3][2] = {{-1,-1},{-1,0},{0,-1}};
            int             iDI;

            pMT_cur->D = D;
            pMT_cur->Idx = -1;
            pMT_cur->IdxT = 0;

            if(i==0) continue;

            for(iDI=0; iDI<3; ++iDI)
            {
                int         i_prev = i+DI[iDI][0];
                int         it_prev = it+DI[iDI][1];

                if(i_prev >= 0 && it_prev>=0)
                {
                    double D_cur = D+pMT[NumT*i_prev+it_prev].D;

                    if(pMT_cur->D > D_cur || (pMT_cur->Idx<0) )
                    {   /* Set new best local way: */
                        pMT_cur->D = D_cur;
                        pMT_cur->Idx = i_prev;
                        pMT_cur->IdxT = it_prev;
                    }
                }
            } /* Check next direction. */
        } /* Fill next colum from table. */
    } /* Fill next row. */

    {   /* Back tracking. */
        /* Find best end in template: */
        int         it_best = 0;
        DefMatch*   pMT_best = pMT + (Num-1)*NumT;
        i = Num-1; /* set current i to last position */

        for(it=1; it<NumT; ++it)
        {
            DefMatch* pMT_new = pMT + it + i*NumT;

            if(pMT_best->D > pMT_new->D)
            {
                pMT_best->D = pMT_new->D;
                it_best = it;
            }
        } /* Find best end template point. */

        /* Back tracking whole sequence: */
        for(it = it_best;i>=0 && it>=0;)
        {
            DefMatch* pMT_new = pMT + it + i*NumT;
            pIdxPairs[2*NumPair] = i+i0;
            pIdxPairs[2*NumPair+1] = it;
            NumPair++;

            it = pMT_new->IdxT;
            i = pMT_new->Idx;
        }
    } /* End back tracing. */

    return NumPair;
} /* cvTrackMatch. */

typedef struct DefTrackForDist
{
    CvBlob                  blob;
    DefTrackRec*            pTrack;
    int                     LastFrame;
    float                   state;
    /* for debug */
    int                     close;
} DefTrackForDist;

class CvBlobTrackAnalysisTrackDist : public CvBlobTrackAnalysis
{
    /*---------------- Internal functions: --------------------*/
private:
    const char*               m_pDebugAVIName; /* For debugging. */
  //CvVideoWriter*      m_pDebugAVI;     /* For debugging. */
    IplImage*           m_pDebugImg;     /* For debugging. */

    char                m_DataFileName[1024];
    CvBlobSeq           m_Tracks;
    CvBlobSeq           m_TrackDataBase;
    int                 m_Frame;
    void*               m_pTempData;
    int                 m_TempDataSize;
    int                 m_TraceLen;
    float               m_AbnormalThreshold;
    float               m_PosThreshold;
    float               m_VelThreshold;
    inline void* ReallocTempData(int Size)
    {
        if(Size <= m_TempDataSize && m_pTempData) return m_pTempData;
        cvFree(&m_pTempData);
        m_TempDataSize = 0;
        m_pTempData = cvAlloc(Size);
        if(m_pTempData) m_TempDataSize = Size;
        return m_pTempData;
    } /* ReallocTempData. */

public:
    CvBlobTrackAnalysisTrackDist():m_Tracks(sizeof(DefTrackForDist)),m_TrackDataBase(sizeof(DefTrackForDist))
    {
        m_pDebugImg = 0;
        //m_pDebugAVI = 0;
        m_Frame = 0;
        m_pTempData = NULL;
        m_TempDataSize = 0;

        m_pDebugAVIName = NULL;
        AddParam("DebugAVI",&m_pDebugAVIName);
        CommentParam("DebugAVI","Name of AVI file to save images from debug window");

        m_TraceLen = 50;
        AddParam("TraceLen",&m_TraceLen);
        CommentParam("TraceLen","Length (in frames) of trajectory part that is used for comparison");

        m_AbnormalThreshold = 0.02f;
        AddParam("AbnormalThreshold",&m_AbnormalThreshold);
        CommentParam("AbnormalThreshold","If trajectory is equal with less then <AbnormalThreshold*DataBaseTrackNum> tracks then trajectory is abnormal");

        m_PosThreshold = 1.25;
        AddParam("PosThreshold",&m_PosThreshold);
        CommentParam("PosThreshold","Minimal allowd distance in blob width that is allowed");

        m_VelThreshold = 0.5;
        AddParam("VelThreshold",&m_VelThreshold);
        CommentParam("VelThreshold","Minimal allowed relative difference between blob speed");

        SetModuleName("TrackDist");

    } /* Constructor. */

    ~CvBlobTrackAnalysisTrackDist()
    {
        int i;
        for(i=m_Tracks.GetBlobNum(); i>0; --i)
        {
            DefTrackForDist* pF = (DefTrackForDist*)m_Tracks.GetBlob(i-1);
            delete pF->pTrack;
        }
        if(m_pDebugImg) cvReleaseImage(&m_pDebugImg);
        //if(m_pDebugAVI) cvReleaseVideoWriter(&m_pDebugAVI);
    } /* Destructor. */

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

        if(pF == NULL)
        {   /* Create new TRack record: */
            DefTrackForDist F;
            F.state = 0;
            F.blob = pBlob[0];
            F.LastFrame = m_Frame;
            F.pTrack = new DefTrackRec(CV_BLOB_ID(pBlob));
            m_Tracks.AddBlob((CvBlob*)&F);
            pF = (DefTrackForDist*)m_Tracks.GetBlobByID(CV_BLOB_ID(pBlob));
        }

        assert(pF);
        assert(pF->pTrack);
        pF->pTrack->AddPoint(pBlob->x,pBlob->y,pBlob->w*0.5f);
        pF->blob = pBlob[0];
        pF->LastFrame = m_Frame;
    };

    virtual void Process(IplImage* pImg, IplImage* /*pFG*/)
    {
        int i;
        double          MinTv = pImg->width/1440.0; /* minimal threshold for speed difference */
        double          MinTv2 = MinTv*MinTv;

        for(i=m_Tracks.GetBlobNum(); i>0; --i)
        {
            DefTrackForDist* pF = (DefTrackForDist*)m_Tracks.GetBlob(i-1);
            pF->state = 0;

            if(pF->LastFrame == m_Frame || pF->LastFrame+1 == m_Frame)
            {   /* Process one blob trajectory: */
                int NumEq = 0;
                int it;

                for(it=m_TrackDataBase.GetBlobNum(); it>0; --it)
                {   /* Check template: */
                    DefTrackForDist*   pFT = (DefTrackForDist*)m_TrackDataBase.GetBlob(it-1);
                    int         Num = pF->pTrack->GetPointNum();
                    int         NumT = pFT->pTrack->GetPointNum();
                    int*        pPairIdx = (int*)ReallocTempData(sizeof(int)*2*(Num+NumT)+sizeof(DefMatch)*Num*NumT);
                    void*       pTmpData = pPairIdx+2*(Num+NumT);
                    int         PairNum = 0;
                    int         k;
                    int         Equal = 1;
                    int         UseVel = 0;
                    int         UsePos = 0;

                    if(i==it) continue;

                    /* Match track: */
                    PairNum = cvTrackMatch( pF->pTrack, m_TraceLen, pFT->pTrack, pPairIdx, pTmpData );
                    Equal = MAX(1,cvRound(PairNum*0.1));

                    UseVel = 3*pF->pTrack->GetPointNum() > m_TraceLen;
                    UsePos = 10*pF->pTrack->GetPointNum() > m_TraceLen;

                    {   /* Check continues: */
                        float   D;
                        int     DI = pPairIdx[0*2+0]-pPairIdx[(PairNum-1)*2+0];
                        int     DIt = pPairIdx[0*2+1]-pPairIdx[(PairNum-1)*2+1];
                        if(UseVel && DI != 0)
                        {
                            D = (float)(DI-DIt)/(float)DI;
                            if(fabs(D)>m_VelThreshold)Equal=0;
                            if(fabs(D)>m_VelThreshold*0.5)Equal/=2;
                        }
                    }   /* Check continues. */

                    for(k=0; Equal>0 && k<PairNum; ++k)
                    {   /* Compare with threshold: */
                        int             j = pPairIdx[k*2+0];
                        int             jt = pPairIdx[k*2+1];
                        DefTrackPoint*  pB = pF->pTrack->GetPoint(j);
                        DefTrackPoint*  pBT = pFT->pTrack->GetPoint(jt);
                        double          dx = pB->x-pBT->x;
                        double          dy = pB->y-pBT->y;
                        double          dvx = pB->vx - pBT->vx;
                        double          dvy = pB->vy - pBT->vy;
                      //double          dv = pB->v - pBT->v;
                        double          D = dx*dx+dy*dy;
                        double          Td = pBT->r*m_PosThreshold;
                        double          dv2 = dvx*dvx+dvy*dvy;
                        double          Tv2 = (pBT->vx*pBT->vx+pBT->vy*pBT->vy)*m_VelThreshold*m_VelThreshold;
                        double          Tvm = pBT->v*m_VelThreshold;


                        if(Tv2 < MinTv2) Tv2 = MinTv2;
                        if(Tvm < MinTv) Tvm = MinTv;

                        /* Check trajectory position: */
                        if(UsePos && D > Td*Td)
                        {
                            Equal--;
                        }
                        else
                        /* Check trajectory velocity. */
                        /* Don't consider trajectory tail because its unstable for velocity computation. */
                        if(UseVel && j>5 && jt>5 && dv2 > Tv2 )
                        {
                            Equal--;
                        }
                    } /* Compare with threshold. */

                    if(Equal>0)
                    {
                        NumEq++;
                        pFT->close++;
                    }
                } /* Next template. */

                {   /* Calculate state: */
                    float   T = m_TrackDataBase.GetBlobNum() * m_AbnormalThreshold; /* calc threshold */

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

                    /*if(0)if(pF->state>0)
                    {// if abnormal blob
                        printf("Abnormal blob(%d) %d < %f, state=%f\n",CV_BLOB_ID(pF),NumEq,T, pF->state);
                    }*/
                }   /* Calculate state. */
            }   /*  Process one blob trajectory. */
            else
            {   /* Move track to tracks data base: */
                m_TrackDataBase.AddBlob((CvBlob*)pF);
                m_Tracks.DelBlob(i-1);
            }
        } /* Next blob. */


        if(m_Wnd)
        {   /* Debug output: */
            int i;

            if(m_pDebugImg==NULL)
                m_pDebugImg = cvCloneImage(pImg);
            else
                cvCopy(pImg, m_pDebugImg);

            for(i=m_TrackDataBase.GetBlobNum(); i>0; --i)
            {   /* Draw all elements in track data base:  */
                int         j;
                DefTrackForDist*   pF = (DefTrackForDist*)m_TrackDataBase.GetBlob(i-1);
                CvScalar    color = CV_RGB(0,0,0);
                if(!pF->close) continue;
                if(pF->close)
                {
                    color = CV_RGB(0,0,255);
                }
                else
                {
                    color = CV_RGB(0,0,128);
                }

                for(j=pF->pTrack->GetPointNum(); j>0; j--)
                {
                    DefTrackPoint* pB = pF->pTrack->GetPoint(j-1);
                    int r = 0;//MAX(cvRound(pB->r),1);
                    cvCircle(m_pDebugImg, cvPoint(cvRound(pB->x),cvRound(pB->y)), r, color);
                }
                pF->close = 0;
            }   /* Draw all elements in track data base. */

            for(i=m_Tracks.GetBlobNum(); i>0; --i)
            {   /* Draw all elements for all trajectories: */
                DefTrackForDist*    pF = (DefTrackForDist*)m_Tracks.GetBlob(i-1);
                int                 j;
                int                 c = cvRound(pF->state*255);
                CvScalar            color = CV_RGB(c,255-c,0);
                CvPoint             p = cvPointFrom32f(CV_BLOB_CENTER(pF));
                int                 x = cvRound(CV_BLOB_RX(pF)), y = cvRound(CV_BLOB_RY(pF));
                CvSize              s = cvSize(MAX(1,x), MAX(1,y));

                cvEllipse( m_pDebugImg,
                    p,
                    s,
                    0, 0, 360,
                    CV_RGB(c,255-c,0), cvRound(1+(0*c)/255) );

                for(j=pF->pTrack->GetPointNum(); j>0; j--)
                {
                    DefTrackPoint* pB = pF->pTrack->GetPoint(j-1);
                    if(pF->pTrack->GetPointNum()-j > m_TraceLen) break;
                    cvCircle(m_pDebugImg, cvPoint(cvRound(pB->x),cvRound(pB->y)), 0, color);
                }
                pF->close = 0;

            }   /* Draw all elements for all trajectories. */

            //cvNamedWindow("Tracks",0);
            //cvShowImage("Tracks", m_pDebugImg);
        } /* Debug output. */

#if 0
        if(m_pDebugImg && m_pDebugAVIName)
        {
            if(m_pDebugAVI==NULL)
            {   /* Create avi file for writing: */
                m_pDebugAVI = cvCreateVideoWriter(
                    m_pDebugAVIName,
                    CV_FOURCC('x','v','i','d'),
                    25,
                    cvSize(m_pDebugImg->width,m_pDebugImg->height));

                if(m_pDebugAVI == NULL)
                {
                    printf("WARNING!!! Can not create AVI file %s for writing\n",m_pDebugAVIName);
                }
            }   /* Create avi file for writing. */

            if(m_pDebugAVI)cvWriteFrame( m_pDebugAVI, m_pDebugImg );
        }   /* Write debug window to AVI file. */
#endif
        m_Frame++;
    };
    float GetState(int BlobID)
    {
        DefTrackForDist* pF = (DefTrackForDist*)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)
    {
        m_DataFileName[0] = 0;
        if(DataBaseName)
        {
            strncpy(m_DataFileName,DataBaseName,1000);
            strcat(m_DataFileName, ".yml");
        }
    };

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



CvBlobTrackAnalysis* cvCreateModuleBlobTrackAnalysisTrackDist()
{return (CvBlobTrackAnalysis*) new CvBlobTrackAnalysisTrackDist;}