#include "Arith_CustomTracker.h"
#include "Arith_AIDMonitor.h"
#include "API_KCF_Tracker.h"
#include "Arith_timer.h"


CustomTracker::CustomTracker(SINT32 nWidth, SINT32 nHeight)
{
    m_pAITracker = nullptr;
    m_pKCFTracker = nullptr;

    m_pKCFTracker = API_KCF_Tracker::Create(56, 64);

}



CustomTracker::~CustomTracker()
{
    if (m_pKCFTracker)
    {
        API_KCF_Tracker::Destroy(m_pKCFTracker);
    }
}


bool CustomTracker::Init(GD_VIDEO_FRAME_S img, PIPE* pLockPipe, GLB_INPUT* p_GLB_Input)
{
    // 锁定管道传指针
    m_LockingPipe = pLockPipe;


    if (m_pKCFTracker)
    {
        m_pKCFTracker->KCF_InitObjectParameters(img, pLockPipe, p_GLB_Input);
    }



    return true;
}

bool CustomTracker::Track(GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input, API_MOT_PIPE* g_GLB_PipeProc)
{
    // 跟踪成功状态
    bool bTrackStatus = false;
    bool bTrackKCFStatus = false;
    bool bTrackAIStatus = false;

    //KCF跟踪
    if (m_pKCFTracker)
    {
        // 运行KCF
        m_pKCFTracker->KCF_Run_Detect(img, p_GLB_Input);
        //更新KCF运动模型
        m_pKCFTracker->UpdateTrackerMotion(p_GLB_Input);
    }


    // 跟踪决策及模型更新
    bTrackStatus = TrackDecision(img, p_GLB_Input);


    if (bTrackStatus)
    {

    }
    else
    {
        // 跟踪失败，解锁
        Cancle();
        // 返回跟踪状态
        return bTrackStatus;
    }

    // 返回跟踪状态
    return bTrackStatus;
}



void CustomTracker::Cancle()
{
    PIPE* pPipe = m_LockingPipe;

    if (pPipe)
    {
        pPipe->bTrackingPipe = false;
    }

    memset(&ObjStatus, 0, sizeof(OBJECTSTATUS));
}



bool CustomTracker::TrackDecision(GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input)
{
    UBYTE8 ubSuccessFlag = TRUE;
    //若同时执行KCF+AI跟踪，则执行面目标跟踪决策
    if (m_pKCFTracker && m_pAITracker)
    {
        //ubSuccessFlag = TO_TrackDecisionOfMiddleObj_KCF_AI(img, p_GLB_Input);
    }
    //若仅执行KCF跟踪，则直接将KCF结果赋给跟踪决策结果
    else if (m_pKCFTracker)
    {
        //20171218，更新kcf模板
        if (m_pKCFTracker->GetTrackeStatus()->unContiLostCnt < 1)
        {
            m_pKCFTracker->KCF_ModelUpdate(img);
        }
        // 取KCF跟踪结果输出
        memcpy(&ObjStatus, m_pKCFTracker->GetTrackeStatus(), sizeof(OBJECTSTATUS));
        ObjStatus.nDeciStatus = KCF_Deci;
    }
    else if (m_pAITracker)
    {
        memcpy(&ObjStatus, m_pAITracker->GetTrackeStatus(), sizeof(OBJECTSTATUS));
        ObjStatus.nDeciStatus = AIT_Deci;
    }
    else
    {
        ObjStatus.bObjMiss = true;
    }

    //若决策目标丢失，则标记跟踪失败
    ubSuccessFlag = TRUE;
    if (ObjStatus.bObjMiss)
    {
        ubSuccessFlag = FALSE;
    }

    //决策结果指导AI跟踪位置
    if (m_pAITracker)
    {
        // AIT_OUTPUT* pAITOut = m_pAITracker->GetAIPara();
        // if (ObjStatus.nOcclude_flag == FULL_OCC)
        // {
        //     //记忆阶段取TLD检测结果传入
        //     if (m_pAITracker->GetTrackeStatus()->unContiLostCnt > p_GLB_Input->unFreq && m_pTLDTracker)
        //     {
        //         RECT32S rsTLDBestRect = m_pTLDTracker->TLD_GetPara()->rsBestNNRect;
        //         m_pAITracker->m_Ai_TkCmd.TargetBox.cx = rsTLDBestRect.x + rsTLDBestRect.w / 2;
        //         m_pAITracker->m_Ai_TkCmd.TargetBox.cy = rsTLDBestRect.y + rsTLDBestRect.h / 2;
        //         m_pAITracker->m_Ai_TkCmd.TargetBox.w = rsTLDBestRect.w;
        //         m_pAITracker->m_Ai_TkCmd.TargetBox.h = rsTLDBestRect.h;
        //     }
        //     else
        //     {

        //         m_pAITracker->m_Ai_TkCmd.TargetBox.cx = pAITOut->fX;
        //         m_pAITracker->m_Ai_TkCmd.TargetBox.cy = pAITOut->fY;
        //         if (pAITOut->fObjW > 1)
        //         {
        //             m_pAITracker->m_Ai_TkCmd.TargetBox.w = pAITOut->fObjW;
        //             m_pAITracker->m_Ai_TkCmd.TargetBox.h = pAITOut->fObjH;
        //         }
        //         else
        //         {
        //             m_pAITracker->m_Ai_TkCmd.TargetBox.w = ObjStatus.sfSize.w;
        //             m_pAITracker->m_Ai_TkCmd.TargetBox.h = ObjStatus.sfSize.h;
        //         }
        //     }
        // }
        // else
        // {
        //     m_pAITracker->m_Ai_TkCmd.TargetBox.cx = ObjStatus.ptPos.x;
        //     m_pAITracker->m_Ai_TkCmd.TargetBox.cy = ObjStatus.ptPos.y;
        //     if (pAITOut->fObjW > 1)
        //     {
        //         m_pAITracker->m_Ai_TkCmd.TargetBox.w = pAITOut->fObjW;
        //         m_pAITracker->m_Ai_TkCmd.TargetBox.h = pAITOut->fObjH;
        //     }
        //     else
        //     {
        //         m_pAITracker->m_Ai_TkCmd.TargetBox.w = ObjStatus.sfSize.w;
        //         m_pAITracker->m_Ai_TkCmd.TargetBox.h = ObjStatus.sfSize.h;
        //     }
        // }
    }

    return ubSuccessFlag;
}