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

#define  PIX_HIST_BIN_NUM_1  3 //number of bins for classification (not used now)
#define  PIX_HIST_BIN_NUM_2  5 //number of bins for statistic collection
#define  PIX_HIST_ALPHA      0.01f //alpha-coefficient for running avarage procedure
#define  PIX_HIST_DELTA      2 //maximal difference between descriptors(RGB)
#define  PIX_HIST_COL_QUANTS 64 //quantization level in rgb-space
#define  PIX_HIST_DELTA_IN_PIX_VAL  (PIX_HIST_DELTA * 256 / PIX_HIST_COL_QUANTS) //allowed difference in rgb-space

// Structures for background statistics estimation:
typedef struct CvPixHistBin{
    float          bin_val;
    uchar          cols[3];
} CvPixHistBin;

typedef struct CvPixHist{
    CvPixHistBin   bins[PIX_HIST_BIN_NUM_2];
} CvPixHist;

// Class for background statistics estimation:
class CvBGEstimPixHist
{
private:
    CvPixHist*  m_PixHists;
    int         m_width;
    int         m_height;

    // Function for update color histogram for one pixel:
    void update_hist_elem(int x, int y, uchar* cols )
    {
        // Find closest bin:
        int    dist = 0, min_dist = 2147483647, indx = -1;
        for( int k = 0; k < PIX_HIST_BIN_NUM_2; k++ ){

            uchar* hist_cols = m_PixHists[y*m_width+x].bins[k].cols;

            m_PixHists[y*m_width+x].bins[k].bin_val *= (1-PIX_HIST_ALPHA);

            int  l;
            for( l = 0; l < 3; l++ ){
                int val = abs( hist_cols[l] - cols[l] );
                if( val > PIX_HIST_DELTA_IN_PIX_VAL ) break;
                dist += val;
            }

            if( l == 3 && dist < min_dist ){
                min_dist = dist;
                indx = k;
            }
        }
        if( indx < 0 ){   // N2th elem in the table is replaced by a new feature.
            indx = PIX_HIST_BIN_NUM_2 - 1;
            m_PixHists[y*m_width+x].bins[indx].bin_val = PIX_HIST_ALPHA;
            for(int l = 0; l < 3; l++ ){
                m_PixHists[y*m_width+x].bins[indx].cols[l] = cols[l];
            }
        }
        else {
            //add vote!
            m_PixHists[y*m_width+x].bins[indx].bin_val += PIX_HIST_ALPHA;
        }
        // Re-sort bins by BIN_VAL:
        {
            int k;
            for(k = 0; k < indx; k++ ){
                if( m_PixHists[y*m_width+x].bins[k].bin_val <= m_PixHists[y*m_width+x].bins[indx].bin_val ){
                    CvPixHistBin tmp1, tmp2 = m_PixHists[y*m_width+x].bins[indx];
                    // Shift elements:
                    for(int l = k; l <= indx; l++ ){
                        tmp1 = m_PixHists[y*m_width+x].bins[l];
                        m_PixHists[y*m_width+x].bins[l] = tmp2;
                        tmp2 = tmp1;
                    }
                    break;
                }
            }
        }
    }   // void update_hist(...)

    // Function for calculation difference between histograms:
    float get_hist_diff(int x1, int y1, int x2, int y2)
    {
        float  dist = 0;
        for( int i = 0; i < 3; i++ ){
            dist += labs(m_PixHists[y1*m_width+x1].bins[0].cols[i] -
                                m_PixHists[y2*m_width+x2].bins[0].cols[i]);
        }
        return dist;
    }


public:
    IplImage*   bg_image;

    CvBGEstimPixHist(CvSize img_size)
    {
        m_PixHists = (CvPixHist*)cvAlloc(img_size.width*img_size.height*sizeof(CvPixHist));
        memset( m_PixHists, 0, img_size.width*img_size.height*sizeof(CvPixHist) );
        m_width = img_size.width;
        m_height = img_size.height;

        bg_image = cvCreateImage(img_size, IPL_DEPTH_8U, 3 );
    }   /* Constructor. */

    ~CvBGEstimPixHist()
    {
        cvReleaseImage(&bg_image);
        cvFree(&m_PixHists);
    }   /* Destructor. */

    // Function to update histograms and bg_image:
    void update_hists( IplImage* pImg )
    {
        for( int i = 0; i < pImg->height; i++ ){
            for( int j = 0; j < pImg->width; j++ ){
                update_hist_elem( j, i, ((uchar*)(pImg->imageData))+i*pImg->widthStep+3*j );
                ((uchar*)(bg_image->imageData))[i*bg_image->widthStep+3*j] = m_PixHists[i*m_width+j].bins[0].cols[0];
                ((uchar*)(bg_image->imageData))[i*bg_image->widthStep+3*j+1] = m_PixHists[i*m_width+j].bins[0].cols[1];
                ((uchar*)(bg_image->imageData))[i*bg_image->widthStep+3*j+2] = m_PixHists[i*m_width+j].bins[0].cols[2];
            }
        }
        // cvNamedWindow("RoadMap2",0);
        // cvShowImage("RoadMap2", bg_image);
    }
};  /* CvBGEstimPixHist */



/*======================= TRACKER LIST SHELL =====================*/
typedef struct DefBlobTrackerL
{
    CvBlob                  blob;
    CvBlobTrackerOne*       pTracker;
    int                     Frame;
    int                     Collision;
    CvBlobTrackPredictor*   pPredictor;
    CvBlob                  BlobPredict;
    CvBlobSeq*              pBlobHyp;
} DefBlobTrackerL;

class CvBlobTrackerList : public CvBlobTracker
{
private:
    CvBlobTrackerOne*       (*m_Create)();
    CvBlobSeq               m_BlobTrackerList;
//    int                     m_LastID;
    int                     m_Collision;
    int                     m_ClearHyp;
    float                   m_BGImageUsing;
    CvBGEstimPixHist*       m_pBGImage;
    IplImage*               m_pImgFG;
    IplImage*               m_pImgReg; /* mask for multiblob confidence calculation */

public:
    CvBlobTrackerList(CvBlobTrackerOne* (*create)()):m_BlobTrackerList(sizeof(DefBlobTrackerL))
    {
        //int i;
        CvBlobTrackerOne* pM = create();
//        m_LastID = 0;
        m_Create = create;
        m_ClearHyp = 0;
        m_pImgFG = 0;
        m_pImgReg = NULL;

        TransferParamsFromChild(pM,NULL);

        pM->Release();

        m_Collision = 1; /* if 1 then collistion will be detected and processed */
        AddParam("Collision",&m_Collision);
        CommentParam("Collision", "if 1 then collision cases are processed in special way");

        m_pBGImage = NULL;
        m_BGImageUsing = 50;
        AddParam("BGImageUsing", &m_BGImageUsing);
        CommentParam("BGImageUsing","Weight of using BG image in update hist model (0 - BG dies not use 1 - use)");

        SetModuleName("List");
    }

    ~CvBlobTrackerList()
    {
        int i;
        if(m_pBGImage) delete m_pBGImage;
        if(m_pImgFG) cvReleaseImage(&m_pImgFG);
        if(m_pImgReg) cvReleaseImage(&m_pImgReg);
        for(i=m_BlobTrackerList.GetBlobNum();i>0;--i)
        {
            m_BlobTrackerList.DelBlob(i-1);
        }
    };

    CvBlob* AddBlob(CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG )
    {   /* Create new tracker: */
        DefBlobTrackerL F;
        F.blob = pBlob[0];
//        F.blob.ID = m_LastID++;
        F.pTracker = m_Create();
        F.pPredictor = cvCreateModuleBlobTrackPredictKalman();
        F.pBlobHyp = new CvBlobSeq;
        F.Frame = 0;
        TransferParamsToChild(F.pTracker,NULL);

        F.pTracker->Init(pBlob,pImg, pImgFG);
        m_BlobTrackerList.AddBlob((CvBlob*)&F);
        return m_BlobTrackerList.GetBlob(m_BlobTrackerList.GetBlobNum()-1);
    };

    void DelBlob(int BlobIndex)
    {
        DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
        if(pF == NULL) return;
        pF->pTracker->Release();
        pF->pPredictor->Release();
        delete pF->pBlobHyp;
        m_BlobTrackerList.DelBlob(BlobIndex);
    }

    void DelBlobByID(int BlobID)
    {
        DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlobByID(BlobID);
        if(pF == NULL) return;
        pF->pTracker->Release();
        pF->pPredictor->Release();
        delete pF->pBlobHyp;
        m_BlobTrackerList.DelBlobByID(BlobID);
    }

    virtual void Process(IplImage* pImg, IplImage* pImgFG = NULL)
    {
        int i;
        if(pImgFG)
        {
            if(m_pImgFG) cvCopy(pImgFG,m_pImgFG);
            else         m_pImgFG = cvCloneImage(pImgFG);
        }

        if(m_pBGImage==NULL && m_BGImageUsing>0)
        {
            m_pBGImage = new CvBGEstimPixHist(cvSize(pImg->width,pImg->height));
        }

        if(m_Collision)
        for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
        {   /* Update predictor: */
            DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
            pF->pPredictor->Update((CvBlob*)pF);
        }   /* Update predictor. */

        if(m_pBGImage && m_pImgFG)
        {   /* Weighting mask mask: */
            int x,y,yN=pImg->height,xN=pImg->width;
            IplImage* pImgBG = NULL;
            m_pBGImage->update_hists(pImg);
            pImgBG = m_pBGImage->bg_image;

            for(y=0; y<yN; ++y)
            {
                unsigned char* pI = (unsigned char*)pImg->imageData + y*pImg->widthStep;
                unsigned char* pBG = (unsigned char*)pImgBG->imageData + y*pImgBG->widthStep;
                unsigned char* pFG = (unsigned char*)m_pImgFG->imageData +y*m_pImgFG->widthStep;

                for(x=0; x<xN; ++x)
                {
                    if(pFG[x])
                    {
                        int D1 = (int)(pI[3*x+0])-(int)(pBG[3*x+0]);
                        int D2 = (int)(pI[3*x+1])-(int)(pBG[3*x+1]);
                        int D3 = (int)(pI[3*x+2])-(int)(pBG[3*x+2]);
                        int DD = D1*D1+D2*D2+D3*D3;
                        double  D = sqrt((float)DD);
                        double  DW = 25;
                        double  W = 1/(exp(-4*(D-m_BGImageUsing)/DW)+1);
                        pFG[x] = (uchar)cvRound(W*255);
                    }
                }   /* Next mask pixel. */
            }   /*  Next mask line. */
            /*if(m_Wnd)
            {
                cvNamedWindow("BlobList_FGWeight",0);
                cvShowImage("BlobList_FGWeight",m_pImgFG);
            }*/
        }   /* Weighting mask mask. */

        for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
        {   /* Predict position. */
            DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
            CvBlob*         pB = pF->pPredictor->Predict();
            if(pB)
            {
                pF->BlobPredict = pB[0];
                pF->BlobPredict.w = pF->blob.w;
                pF->BlobPredict.h = pF->blob.h;
            }
        }   /* Predict position. */

        if(m_Collision)
        for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
        {   /* Predict collision. */
            int             Collision = 0;
            int             j;
            DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);

            for(j=m_BlobTrackerList.GetBlobNum(); j>0; --j)
            {   /* Predict collision. */
                CvBlob* pB1;
                CvBlob* pB2;
                DefBlobTrackerL* pF2 = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(j-1);
                if(i==j) continue;
                pB1 = &pF->BlobPredict;
                pB2 = &pF2->BlobPredict;
                if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
                    fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Collision = 1;
                pB1 = &pF->blob;
                pB2 = &pF2->blob;
                if( fabs(pB1->x-pB2->x)<0.5*(pB1->w+pB2->w) &&
                    fabs(pB1->y-pB2->y)<0.5*(pB1->h+pB2->h) ) Collision = 1;
                if(Collision) break;
            }   /* Check next blob to cross current. */

            pF->Collision = Collision;
            pF->pTracker->SetCollision(Collision);

        }   /* Predict collision. */

        for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
        {   /* Track each blob. */
            DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
            if(pF->pBlobHyp->GetBlobNum()>0)
            {   /* Track all hypothesis. */
                int h,hN = pF->pBlobHyp->GetBlobNum();
                for(h=0;h<hN;++h)
                {
                    CvBlob*     pB = pF->pBlobHyp->GetBlob(h);
                    CvBlob*     pNewBlob = pF->pTracker->Process(pB,pImg,m_pImgFG);
                    int         BlobID = CV_BLOB_ID(pB);
                    if(pNewBlob)
                    {
                        pB[0] = pNewBlob[0];
                        pB->w = MAX(CV_BLOB_MINW,pNewBlob->w);
                        pB->h = MAX(CV_BLOB_MINH,pNewBlob->h);
                        CV_BLOB_ID(pB) = BlobID;
                    }
                }   /* Next hypothesis. */

            }   /* Track all hypotheses. */

            pF->Frame++;

        }   /* Next blob. */

#if 0
        for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
        {   /* Update predictor: */
            DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
            if((m_Collision && !pF->Collision) || !m_Collision)
            {
                pF->pPredictor->Update((CvBlob*)pF);
            }
            else
            {   /* pravilnyp putem idete tovarischy!!! */
                pF->pPredictor->Update(&(pF->BlobPredict));
            }
        }   /* Update predictor. */
#endif
        m_ClearHyp = 1;
    };


    /* Process on blob (for multi hypothesis tracing) */
    virtual void ProcessBlob(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL)
    {
        int                 ID = pBlob->ID;
        DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
        CvBlob*             pNewBlob = pF->pTracker->Process(pBlob?pBlob:&(pF->blob),pImg,m_pImgFG);
        if(pNewBlob)
        {
            pF->blob = pNewBlob[0];
            pF->blob.w = MAX(CV_BLOB_MINW,pNewBlob->w);
            pF->blob.h = MAX(CV_BLOB_MINH,pNewBlob->h);
            pBlob[0] = pF->blob;
        }
        pBlob->ID = ID;
    };

    virtual double  GetConfidence(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG = NULL)
    {
        DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
        if(pF==NULL) return 0;
        if(pF->pTracker==NULL) return 0;
        return pF->pTracker->GetConfidence(pBlob?pBlob:(&pF->blob), pImg, pImgFG, NULL);
    };

    virtual double GetConfidenceList(CvBlobSeq* pBlobList, IplImage* pImg, IplImage* pImgFG = NULL)
    {
        double  W = 1;
        int     b,bN = pBlobList->GetBlobNum();

        if(m_pImgReg == NULL)
        {
            m_pImgReg = cvCreateImage(cvSize(pImg->width,pImg->height),IPL_DEPTH_8U,1);
        }
        assert(pImg);

        cvSet(m_pImgReg,cvScalar(255));

        for(b=0; b<bN; ++b)
        {
            CvBlob* pB = pBlobList->GetBlob(b);
            DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlobByID(pB->ID);
            if(pF==NULL || pF->pTracker==NULL) continue;
            W *= pF->pTracker->GetConfidence(pB, pImg, pImgFG, m_pImgReg );
            cvEllipse(
                m_pImgReg,
                cvPoint(cvRound(pB->x*256),cvRound(pB->y*256)), cvSize(cvRound(pB->w*128),cvRound(pB->h*128)),
                0, 0, 360,
                cvScalar(0), CV_FILLED, 8, 8 );
//            cvNamedWindow("REG",0);
//            cvShowImage("REG",m_pImgReg);
//            cvWaitKey(0);
        }
        return W;
    };

    virtual void UpdateBlob(int BlobIndex, CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL)
    {
        DefBlobTrackerL*    pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
        if(pF)
        {
            pF->pTracker->Update(pBlob?pBlob:&(pF->blob),pImg,m_pImgFG);
        }
    };

    int     GetBlobNum(){return m_BlobTrackerList.GetBlobNum();};
    CvBlob* GetBlob(int index){return m_BlobTrackerList.GetBlob(index);};

    void  SetBlob(int BlobIndex, CvBlob* pBlob)
    {
        CvBlob* pB = m_BlobTrackerList.GetBlob(BlobIndex);
        if(pB)
        {
            pB[0] = pBlob[0];
            pB->w = MAX(CV_BLOB_MINW, pBlob->w);
            pB->h = MAX(CV_BLOB_MINH, pBlob->h);
        }
    }

    void    Release(){delete this;};

    /* Additional functionality: */
    CvBlob* GetBlobByID(int BlobID){return m_BlobTrackerList.GetBlobByID(BlobID);}

    /*  ===============  MULTI HYPOTHESIS INTERFACE ==================  */
    /* Return number of position hypotheses of currently tracked blob: */
    virtual int     GetBlobHypNum(int BlobIdx)
    {
        DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIdx);
        assert(pF->pBlobHyp);
        return pF->pBlobHyp->GetBlobNum();
    };  /* CvBlobtrackerList::GetBlobHypNum() */

    /* Return pointer to specified blob hypothesis by index blob: */
    virtual CvBlob* GetBlobHyp(int BlobIndex, int hypothesis)
    {
        DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
        assert(pF->pBlobHyp);
        return pF->pBlobHyp->GetBlob(hypothesis);
    };  /* CvBlobtrackerList::GetBlobHyp() */

    /* Set new parameters for specified (by index) blob hyp (can be called several times for each hyp )*/
    virtual void    SetBlobHyp(int BlobIndex, CvBlob* pBlob)
    {
        if(m_ClearHyp)
        {   /* Clear all hypotheses: */
            int b, bN = m_BlobTrackerList.GetBlobNum();
            for(b=0; b<bN; ++b)
            {
                DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(b);
                assert(pF->pBlobHyp);
                pF->pBlobHyp->Clear();
            }
            m_ClearHyp = 0;
        }
        {   /* Add hypothesis: */
            DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(BlobIndex);
            assert(pF->pBlobHyp);
            pF->pBlobHyp->AddBlob(pBlob);
        }
    };  /* CvBlobtrackerList::SetBlobHyp */

private:
public:
    void ParamUpdate()
    {
        int i;
        for(i=m_BlobTrackerList.GetBlobNum(); i>0; --i)
        {
            DefBlobTrackerL* pF = (DefBlobTrackerL*)m_BlobTrackerList.GetBlob(i-1);
            TransferParamsToChild(pF->pTracker);
            pF->pTracker->ParamUpdate();
        }
    }
};  /* CvBlobTrackerList */

CvBlobTracker* cvCreateBlobTrackerList(CvBlobTrackerOne* (*create)())
{
    return (CvBlobTracker*) new CvBlobTrackerList(create);
}