#include <thread>
#include "daSiamRpnTrck.h"
#include <opencv2/imgproc.hpp>
// #include "sample_comm_ive_.h"
#include "guidePlat.h"
#include "gd_alg_type.h"
#include "cJSON.h"

using namespace GUD::ALG;

void cvImageToTensor(const unsigned char* img_data, unsigned char* tensor, int channels, int height, int width) {
    int align = 2; 
    int dstStride = (width + (align - width % align) % align);
    int hs = height * dstStride;
    int iwc = 0;
    int is = 0;
    int jc = 0;
    for (int i=0; i<height; i++) {
        iwc = i * width * channels;
        is = i * dstStride;
        for (int j=0; j<width; j++) {
            jc = j * channels;
            for (int k=0; k<channels; k++) {
                const size_t offsetCv = iwc + jc + k;
                const size_t offset = k * hs + is + j;
                tensor[offset] = img_data[offsetCv];
            }
        }
    }
}

template <typename T> static
T sizeCal(const T& w, const T& h)
{
    T pad = (w + h) * T(0.5);
    T sz2 = (w + pad) * (h + pad);
    return sqrt(sz2);
}

template <>
cv::Mat sizeCal(const cv::Mat& w, const cv::Mat& h)
{
    cv::Mat pad = (w + h) * 0.5;
    cv::Mat sz2 = (w + pad).mul((h + pad));

    cv::sqrt(sz2, sz2);
    return sz2;
}

void elementMax(cv::Mat& src)
{
    int* p = src.size.p;
    int index[4] = { 0, 0, 0, 0 };
    for (int n = 0; n < *p; n++)
    {
        for (int k = 0; k < *(p + 1); k++)
        {
            for (int i = 0; i < *(p + 2); i++)
            {
                for (int j = 0; j < *(p + 3); j++)
                {
                    index[0] = n, index[1] = k, index[2] = i, index[3] = j;
                    float& v = src.at<float>(index);
                    v = fmax(v, 1.0f / v);
                }
            }
        }
    }
}

DaSiamRPNtracker::DaSiamRPNtracker(const gud_siamrpn_config_t config)
{
    initFlag_ = 0;
    ReidInit_ = 0;
    MnnLoad_  = 0;
    mnnIdx_   = 0;
    nnieIdx_  = -1;
    TempNum_  = 5;
    UpdateFrq_= 5;
    detThd_   = config.ac_score;
    acNumThd_ = config.ac_num;
    trackState.lr = config.lr_rate;

    UpdataTemp_ = config.use_update;
    UpdateFrq_ = config.update_frq;

    temple_net_.loadModel(config.nnieModel_0);
    detect_net_.loadModel(config.nnieModel_1);

    /// mnn net init  (new method to load two mnn models with double inputs dynamic weights)
    mnnDoubleBranch_ = new ForwardNet(config);
    MnnLoad_         = 1;
    tkTime_.maxTime  = 0;
    tkTime_.minTime  = 1e6;
    tkTime_.timeThd  = 18;

    #define AI_MODULE_PATH  "/app/private/extapp/data"
    if (UpdataTemp_){
        const char *uptemp_model = AI_MODULE_PATH"/nnie_model/siamrpn_v1_ch256_template127_240628_low.wk";
        uptemp_net_.loadModel(uptemp_model);
    }

    std::thread aithread(&DaSiamRPNtracker::nnieSearThrd, std::ref(*this));
    bindCpusToThread(aithread.native_handle(), A73_0,A73_1);
    //bindCpusToThread(aithread.native_handle(), A73_1,A53_1);
    aithread.detach();

    bufTmpAlloc(&tmpBuf0,991232);
    //bufTmpAlloc(&tmpBuf1,sizeof(float)*4*1805);
    imgTmpAlloc(&subImg,trackState.instanceSize,trackState.instanceSize,GUIDE_U8C3_PLANAR);
    outTnsr[0].data = (float *)tmpBuf0.virAddr;
    outTnsr[1].data = (float *)(tmpBuf0.virAddr + 495616);

    if (config.use_reid){
        reid_jsonReader(config.reid_json, &reid_config_);
        nnie_reid_  = new NNIE_REID(reid_config_);
        printf ("Using REID Model:[%s], init sucess!\n",config.reid_json);
        ReidInit_ = 1;
        ReidFrq_ = config.reid_frq;
        ReidCycleUp_ = reid_config_.cycle_update;
        ReidUpRatio_ = reid_config_.update_ratio;
    }

    if (config.use_trace){
        UseTrace_ = 1;
        TraceLen_ = (unsigned int)(config.trace_len);
    }

    traces_ = std::deque<gud_rect_t>();
    tagTemplates_ = std::deque<cv::Mat>();

}

DaSiamRPNtracker::~DaSiamRPNtracker()
{
      bufTmpFree(&tmpBuf0);
      //bufTmpFree(&tmpBuf1);
      delete mnnDoubleBranch_;
      imgTmpFree(&subImg);
      delete nnie_reid_;
}

int DaSiamRPNtracker::nnieSearThrd(){

    while(MnnLoad_) {
        if (initFlag_) {
            if (nnieIdx_ == mnnIdx_)
            {
                //printf ("NnieIDX==MnnIdx:[%d %d] do trackerUpdateNpu\n",nnieIdx_,mnnIdx_);
                trackerUpdateNpu(fullImg,outBox_,updateReg_);

            } else{
                //printf ("NnieIDX != MnnIdx:[%d %d]\n",nnieIdx_,mnnIdx_);
                usleep(200);
                if (mnnIdx_ > nnieIdx_){
                    nnieIdx_ = mnnIdx_;
                }
            }
        }
        else{
            usleep(200);
        }
    }
    return 0;
}

gud_rect_t DaSiamRPNtracker::selectTagFromDetection(gud_rect_t bbox, ai_detect_result_t *AIod_res)
{
    gud_rect_t sBox = bbox;
    /// Lock from AI-detection results if manual-Lock point in some targets'rect region
    float disMin = 1e6;
    int   sIdx = -1;
    if (AIod_res->detect_num > 0) {
        float Pcx =  static_cast<float>(bbox.cx);   /// NOTE: manual select point_center_x
        float Pcy =  static_cast<float>(bbox.cy);   /// NOTE: manual select point_center_y
        for (int i = 0; i < AIod_res->detect_num; i++) {
            int txmin = AIod_res->ai_result[i].x;
            int tymin = AIod_res->ai_result[i].y;
            int tw = AIod_res->ai_result[i].w;
            int th = AIod_res->ai_result[i].h;
            int txmax = txmin + tw;
            int tymax = tymin + th;
            float tcx = (txmin + tw / 2)*1.0;
            float tcy = (tymin + th / 2)*1.0;
            float dis =  sqrt(pow(Pcx-tcx,2.0) + pow(Pcy-tcy,2.0));

            if ((txmin <= Pcx) and (tymin <= Pcy) and (txmax >= Pcx) and (tymax >= Pcy)) {
                sIdx = i;
                break;
            }
        }
        if (sIdx!=(-1)) {
            sBox.cx      = static_cast<int>(AIod_res->ai_result[sIdx].x + AIod_res->ai_result[sIdx].w/2.0);
            sBox.cy      = static_cast<int>(AIod_res->ai_result[sIdx].y + AIod_res->ai_result[sIdx].h/2.0);
            sBox.w       = static_cast<int>(AIod_res->ai_result[sIdx].w);
            sBox.h       = static_cast<int>(AIod_res->ai_result[sIdx].h);
            sBox.label   = AIod_res->ai_result[sIdx].ID;
            sBox.used    = 1;
            printf("Select Target from DetectionResult[%d]_class:[%d]_conf:[%.2f]\n",sIdx,AIod_res->ai_result[sIdx].ID,
                   AIod_res->ai_result[sIdx].prob);
        }else{
            sBox = bbox;
        }
    }
    return sBox;
}

gud_rect_f DaSiamRPNtracker::selectTagFromDetectionF(gud_rect_f bbox, ai_detect_result_t *AIod_res)
{
    gud_rect_f sBox = bbox;
    /// Lock from AI-detection results if manual-Lock point in some targets'rect region
    float disMin = 1e6;
    int   sIdx = -1;
    if (AIod_res->detect_num > 0) {
        float Pcx =  bbox.cx;   /// NOTE: manual select point_center_x
        float Pcy =  bbox.cy;   /// NOTE: manual select point_center_y
        for (int i = 0; i < AIod_res->detect_num; i++) {
            int txmin = AIod_res->ai_result[i].x;
            int tymin = AIod_res->ai_result[i].y;
            int tw = AIod_res->ai_result[i].w;
            int th = AIod_res->ai_result[i].h;
            int txmax = txmin + tw;
            int tymax = tymin + th;
            float tcx = (txmin + tw / 2)*1.0;
            float tcy = (tymin + th / 2)*1.0;
            float dis =  sqrt(pow(Pcx-tcx,2.0) + pow(Pcy-tcy,2.0));

            if ((txmin <= Pcx) and (tymin <= Pcy) and (txmax >= Pcx) and (tymax >= Pcy)) {
                sIdx = i;
                break;
            }
        }
        if (sIdx!=(-1)) {
            sBox.cx      = static_cast<float>(AIod_res->ai_result[sIdx].x + AIod_res->ai_result[sIdx].w/2.0);
            sBox.cy      = static_cast<float>(AIod_res->ai_result[sIdx].y + AIod_res->ai_result[sIdx].h/2.0);
            sBox.w       = static_cast<float>(AIod_res->ai_result[sIdx].w);
            sBox.h       = static_cast<float>(AIod_res->ai_result[sIdx].h);
            sBox.label   = AIod_res->ai_result[sIdx].ID;
            sBox.used    = 1;
            printf("Select Target from DetectionResult[%d]_class:[%d]_conf:[%.2f]\n",sIdx,AIod_res->ai_result[sIdx].ID,
                   AIod_res->ai_result[sIdx].prob);
        }else{
            sBox = bbox;
        }
    }
    return sBox;
}

void DaSiamRPNtracker::trackerInit(GuideImage *pSrcImg, gud_rect_t initBox, ai_detect_result_t *od_res)
{
    mnnIdx_   = 0;
    nnieIdx_  = -1;
    fullImg   = pSrcImg;

    gud_rect_t tagBox = selectTagFromDetection(initBox,od_res);

    int width = pSrcImg->img.au32Stride[0];
    int height = pSrcImg->img.u32Height;
    int bgrSize = width * height * 3;
    char *pSrcBgr = (char*)HI_MPI_SYS_MmapCache(pSrcImg->img.au64PhyAddr[0],bgrSize);
    cv::Mat img = cv::Mat(height,width, CV_8UC3,pSrcBgr);

    trackState.tracking_score   = 1.0;
    trackState.label            = tagBox.label;
    trackState.targetBox.x      = static_cast<float>(tagBox.cx);
    trackState.targetBox.y      = static_cast<float>(tagBox.cy);
    trackState.targetBox.width  = static_cast<float>(tagBox.w);
    trackState.targetBox.height = static_cast<float>(tagBox.h);
    printf ("LockBox:[%.2f %.2f %.2f %.2f ] label:[%d]\n",trackState.targetBox.x ,trackState.targetBox.y,trackState.targetBox.width,
                        trackState.targetBox.height,trackState.label);
    cv::Rect2f targetBox = trackState.targetBox;
    cv::Rect2f searchBox = trackState.targetBox;

    cv::Mat anchors = generateAnchors();
    trackState.anchors = anchors;
    cv::Mat windows = generateHanningWindow();

    trackState.windows = windows;
    trackState.imgSize = img.size();

    trackState.avgChans = mean(img);
    float wc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
    float hc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
    float sz = (float)cvRound(sqrt(wc * hc));
    cv::Mat zCrop = getSubwindow(img, targetBox, sz, trackState.avgChans, trackState.exemplarSize);
    HI_MPI_SYS_Munmap(pSrcBgr,bgrSize);

//    std::string regionImg = "/nfsroot/001_3559/results/init" + std::to_string(trackState.FRN) + ".jpg";
//    cv::imwrite(regionImg, zCrop);
    uint8_t *pTmpBgr = (uint8_t *)subImg.img.au64VirAddr[0];
    cvImageToTensor(zCrop.data, pTmpBgr, 3, trackState.exemplarSize, trackState.exemplarSize);

    //tt1_ = steady_clock::now();
    temple_net_.run(&subImg,outTnsr);
    //tt2_ = steady_clock::now();
    //time_span_ = duration_cast < duration < double >> (tt2_ - tt1_);
    //printf("#Run temple_net_.run time is %f ms.\n",1000 * time_span_.count());

    tem_output01 = outTnsr[0];//temple_net_.getOutputTensor(0);  // tem_output01.data shape [1, 10240, 4, 4]
    tem_output02 = outTnsr[1];//temple_net_.getOutputTensor(1);
//    printf ("Reg_TempOut01:[%d %d %d %d]\n",tem_output01.n,tem_output01.channel,tem_output01.height,tem_output01.width);
//    printf ("CLs_TempOut02:[%d %d %d %d]\n",tem_output02.n,tem_output02.channel,tem_output02.height,tem_output02.width);
//    printf ("#########  showFloatInputs(ClsKernelTensor_) ##############\n");
//    showFloatInputs(tem_output02.data,  10*256*4*4,256*4*4);
//    printf ("#########  showFloatInputs(RegKernelTensor_) ##############\n");
//    showFloatInputs(tem_output01.data,  20*256*4*4,256*4*4);
    tkTime_.maxTime      = 0;
    tkTime_.minTime      = 1e6;
    tkTime_.updateTimes  = 0;
    tkTime_.expandTimes  = 0;
    tkTime_.expandRate   = 0;
    mnnDoubleBranch_->updateTwoKernels(tem_output02.data, tem_output01.data);    // when switch new track-target, do not need load mnn models
    printf("init ------------- index:%d\n", pSrcImg->indx);

    if (UseTrace_){
        if (traces_.size()!=0)
            traces_.clear();
        traces_.push_back(tagBox);
    }


    if (ReidInit_) {
        nnie_reid_->Clear_Gallery_Featrue_Pools();
        //nnie_reid_->Init_Gallery_Featrue_Pools(pSrcImg, tagBox);
    }
    if (UpdataTemp_){
        if (tagTemplates_.size()!=0)
            tagTemplates_.clear();
        if (trkRes_.size()!=0)
            trkRes_.clear();
    }

    trackerUpdateNpu(pSrcImg, searchBox, true);
    initFlag_            = 1;
}



void DaSiamRPNtracker::trackerReInit(GuideImage *pSrcImg, gud_rect_t initBox, ai_detect_result_t *od_res)
{
    mnnIdx_   = 0;
    nnieIdx_  = -1;
    fullImg   = pSrcImg;

    gud_rect_t tagBox = selectTagFromDetection(initBox,od_res);

    int width = pSrcImg->img.au32Stride[0];
    int height = pSrcImg->img.u32Height;
    int bgrSize = width * height * 3;
    char *pSrcBgr = (char*)HI_MPI_SYS_MmapCache(pSrcImg->img.au64PhyAddr[0],bgrSize);
    cv::Mat img = cv::Mat(height,width, CV_8UC3,pSrcBgr);

    trackState.tracking_score   = 1.0;
    trackState.label            = tagBox.label;
    trackState.targetBox.x      = static_cast<float>(tagBox.cx);
    trackState.targetBox.y      = static_cast<float>(tagBox.cy);
    trackState.targetBox.width  = static_cast<float>(tagBox.w);
    trackState.targetBox.height = static_cast<float>(tagBox.h);
    printf ("ReLockBox:[%.2f %.2f %.2f %.2f ] label:[%d]\n",trackState.targetBox.x ,trackState.targetBox.y,trackState.targetBox.width,
            trackState.targetBox.height,trackState.label);
    cv::Rect2f targetBox = trackState.targetBox;
    cv::Rect2f searchBox = trackState.targetBox;

    cv::Mat anchors = generateAnchors();
    trackState.anchors = anchors;
    cv::Mat windows = generateHanningWindow();

    trackState.windows = windows;
    trackState.imgSize = img.size();

    trackState.avgChans = mean(img);
    float wc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
    float hc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
    float sz = (float)cvRound(sqrt(wc * hc));
    cv::Mat zCrop = getSubwindow(img, targetBox, sz, trackState.avgChans, trackState.exemplarSize);
    HI_MPI_SYS_Munmap(pSrcBgr,bgrSize);

    uint8_t *pTmpBgr = (uint8_t *)subImg.img.au64VirAddr[0];
    cvImageToTensor(zCrop.data, pTmpBgr, 3, trackState.exemplarSize, trackState.exemplarSize);
    temple_net_.run(&subImg,outTnsr);
    tem_output01 = outTnsr[0];//temple_net_.getOutputTensor(0);  // tem_output01.data shape [1, 10240, 4, 4]
    tem_output02 = outTnsr[1];//temple_net_.getOutputTensor(1);

    tkTime_.maxTime      = 0;
    tkTime_.minTime      = 1e6;
    tkTime_.updateTimes  = 0;
    tkTime_.expandTimes  = 0;
    tkTime_.expandRate   = 0;
    mnnDoubleBranch_->updateTwoKernels(tem_output02.data, tem_output01.data);    // when switch new track-target, do not need load mnn models
    printf("init ------------- index:%d\n", pSrcImg->indx);

    if (UseTrace_){
        if (traces_.size()!=0)
            traces_.clear();
        traces_.push_back(tagBox);
    }


    if (ReidInit_) {
        nnie_reid_->Clear_Gallery_Featrue_Pools();
        //nnie_reid_->Init_Gallery_Featrue_Pools(pSrcImg, tagBox);
    }
    if (UpdataTemp_){
        if (tagTemplates_.size()!=0)
            tagTemplates_.clear();
        if (trkRes_.size()!=0)
            trkRes_.clear();
    }
    nnieIdx_++;
    initFlag_            = 1;
}


void DaSiamRPNtracker::trackerReInitFloat(GuideImage *pSrcImg, gud_rect_f initBox, ai_detect_result_t *od_res)
{
    mnnIdx_   = 0;
    nnieIdx_  = -1;
    fullImg   = pSrcImg;

    //gud_rect_f tagBox = selectTagFromDetectionF(initBox,od_res);
    gud_rect_f tagBox = initBox;
    int width = pSrcImg->img.au32Stride[0];
    int height = pSrcImg->img.u32Height;
    int bgrSize = width * height * 3;
    char *pSrcBgr = (char*)HI_MPI_SYS_MmapCache(pSrcImg->img.au64PhyAddr[0],bgrSize);
    cv::Mat img = cv::Mat(height,width, CV_8UC3,pSrcBgr);

    trackState.tracking_score   = 1.0;
    trackState.label            = tagBox.label;
    trackState.targetBox.x      = tagBox.cx;
    trackState.targetBox.y      = tagBox.cy;
    trackState.targetBox.width  = tagBox.w;
    trackState.targetBox.height = tagBox.h;
    printf ("ReLockBox:[%.2f %.2f %.2f %.2f ] label:[%d]\n",trackState.targetBox.x ,trackState.targetBox.y,trackState.targetBox.width,
            trackState.targetBox.height,trackState.label);
    cv::Rect2f targetBox = trackState.targetBox;
    cv::Rect2f searchBox = trackState.targetBox;

    //tt3_ = steady_clock::now();
    cv::Mat anchors = generateAnchors();
    trackState.anchors = anchors;
    cv::Mat windows = generateHanningWindow();
    //tt4_ = steady_clock::now();
    //time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
    //printf("#FRN:[%d] generateAnchors+generateHanningWindow time is (%f) ms.\n",pSrcImg->frmId,1000 * time_span_.count());


    trackState.windows = windows;
    trackState.imgSize = img.size();
    //tt3_ = steady_clock::now();
    trackState.avgChans = mean(img);
    float wc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
    float hc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
    float sz = (float)cvRound(sqrt(wc * hc));
    cv::Mat zCrop = getSubwindow(img, targetBox, sz, trackState.avgChans, trackState.exemplarSize);
    HI_MPI_SYS_Munmap(pSrcBgr,bgrSize);
    //tt4_ = steady_clock::now();
    //time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
    //printf("#FRN:[%d] getSubwindow time is (%f) ms.\n",pSrcImg->frmId,1000 * time_span_.count());


    //tt3_ = steady_clock::now();
    uint8_t *pTmpBgr = (uint8_t *)subImg.img.au64VirAddr[0];
    cvImageToTensor(zCrop.data, pTmpBgr, 3, trackState.exemplarSize, trackState.exemplarSize);
    temple_net_.run(&subImg,outTnsr);
    tem_output01 = outTnsr[0];//temple_net_.getOutputTensor(0);  // tem_output01.data shape [1, 10240, 4, 4]
    tem_output02 = outTnsr[1];//temple_net_.getOutputTensor(1);

    //tt4_ = steady_clock::now();
    //time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
    //printf("#FRN:[%d] temple_net_.run time is (%f) ms.\n",pSrcImg->frmId,1000 * time_span_.count());

    tkTime_.maxTime      = 0;
    tkTime_.minTime      = 1e6;
    tkTime_.updateTimes  = 0;
    tkTime_.expandTimes  = 0;
    tkTime_.expandRate   = 0;

    //tt3_ = steady_clock::now();
    mnnDoubleBranch_->updateTwoKernels(tem_output02.data, tem_output01.data);
    //tt4_ = steady_clock::now();
    //time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
    //printf("#FRN:[%d] updateTwoKernels is (%f) ms.\n",pSrcImg->frmId,1000 * time_span_.count());
    //printf("init ------------- index:%d\n", pSrcImg->indx);

    nnieIdx_++;
    initFlag_            = 1;
}


void DaSiamRPNtracker::trackerProcess(GuideImage *pSrcImg, gud_rect_t searchBox, ai_detect_result_t *od_res, AIT_OUTPUT *pAiOut)
{
    if (searchBox.used){
        outBox_.x      = static_cast<float>(searchBox.cx);
        outBox_.y      = static_cast<float>(searchBox.cy);
        outBox_.width  = static_cast<float>(searchBox.w);
        outBox_.height = static_cast<float>(searchBox.h);
        updateReg_ = true;
    }
    else{
        updateReg_ = false;
    }
    fullImg    = pSrcImg;

    //trackerUpdateNpu(pSrcImg, outBox_, updateReg_);
    trackerUpdateMnn(pAiOut);

    gud_rect_t trackRect, sOdRect;
    trackRect.cx = pAiOut->nX;
    trackRect.cy = pAiOut->nY;
    trackRect.w  = pAiOut->nObjW;
    trackRect.h  = pAiOut->nObjH;
    int trkLabel = trackState.label;


    if (ReidInit_) {
        if (nnie_reid_->gallery_status_ == 0){
            nnie_reid_->Init_Gallery_Featrue_Pools(pSrcImg, trackRect);
        }else{
            if (pSrcImg->frmId % ReidFrq_ == 0){
                tt3_ = steady_clock::now();
                nnie_reid_->Compare_QueryToGallery(pSrcImg, trackRect, &reid_result_);
                tt4_ = steady_clock::now();
                time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
                printf("#FRN:[%d]  nnie_reid_Compare_QueryToGallery time is (%f) ms.\n",pSrcImg->frmId,1000 * time_span_.count());
            }

            if (ReidCycleUp_){
                if (trackState.tracking_score > UPDATE_TEMPLATE_ANCHOR_SCORE and pAiOut->nDetectNum > UPDATE_TEMPLATE_ANCHOR_THD){
                    int poolIdx = static_cast<int>((1.0 - ReidUpRatio_) * reid_config_.gallery_num);
                    nnie_reid_->Clear_Gallery_SetIndx_Featrue_Pools(poolIdx);
                }
            }
        }
    }

    if (UpdataTemp_){
        gud_trk_res_t curTrk;
        curTrk.target      = trackRect;
        curTrk.track_score = trackState.tracking_score;
        curTrk.reid_sim    = reid_result_.avg_sim;
        curTrk.anchor_num  = pAiOut->nDetectNum;
        if (trkRes_.size() < TempNum_){
            trkRes_.push_back(curTrk);
        }
        else{
            trkRes_.pop_front();
            trkRes_.push_back(curTrk);
            if (pSrcImg->frmId % UpdateFrq_ == 0){
                int    maxIDX = 0;
                float weighScore = 0;
                float MaxScore   = 0;
                for (int i = 0; i < int(TempNum_); ++i) {
                    gud_trk_res_t tmpRes = trkRes_[i];
                    if (ReidInit_){
                        weighScore = tmpRes.reid_sim + (tmpRes.anchor_num / AIT_DETECTION_MAX_NUM)*1.0 + tmpRes.track_score;
                    }
                    else{
                        weighScore = (tmpRes.anchor_num / AIT_DETECTION_MAX_NUM)*1.0 + tmpRes.track_score;
                    }
                    if (weighScore > MaxScore){
                        MaxScore = weighScore;
                        maxIDX   = i;
                    }
                }
                gud_trk_res_t selectTrk = trkRes_[maxIDX];
                printf ("DUSE_TARGET_UPDATE0; use [%d] template to update! weightscore:[%.2f] (anchorNum:[%d],trackSocre:[%.2f] ReidSIm:[%.2f]\n",
                        maxIDX,MaxScore,selectTrk.anchor_num,selectTrk.track_score,selectTrk.reid_sim);

                if (selectTrk.track_score > UPDATE_TEMPLATE_ANCHOR_SCORE and selectTrk.anchor_num > UPDATE_TEMPLATE_ANCHOR_THD)// and selectTrk.reid_sim > 0.9)
                {
                    tt1_ = steady_clock::now();
                    cv::Mat zCrop = tagTemplates_[maxIDX];
                    uint8_t *pTmpBgr = (uint8_t *)subImg.img.au64VirAddr[0];
                    cvImageToTensor(zCrop.data, pTmpBgr, 3, trackState.exemplarSize, trackState.exemplarSize);
                    uptemp_net_.run(&subImg,outTnsr);
                    tem_output01 = outTnsr[0];
                    tem_output02 = outTnsr[1];
                    mnnDoubleBranch_->updateTwoKernels(tem_output02.data, tem_output01.data);
                    tt2_ = steady_clock::now();
                    time_span_ = duration_cast < duration < double >> (tt2_ - tt1_);
                    printf("#Run DUSE_TARGET_UPDATE1.run time is %f ms.\n",1000 * time_span_.count());
                    printf ("DUSE_TARGET_UPDATE1; use [%d] template to update! weightscore:[%.2f] (anchorNum:[%d],trackSocre:[%.2f] ReidSIm:[%.2f]\n",
                            maxIDX,MaxScore,selectTrk.anchor_num,selectTrk.track_score,selectTrk.reid_sim);
                    if (ReidInit_){
                        nnie_reid_->gallery_num_--;
                        nnie_reid_->Init_Gallery_Featrue_Pools(pSrcImg, trackRect);
                    }
                }
            }
        }
    }

    if (UseTrace_){
        if (traces_.size() < TraceLen_){
            traces_.push_back(trackRect);
        }
        else{
            traces_.pop_front();
            traces_.push_back(trackRect);
        }
    }

    if (pAiOut->nDetectNum >= acNumThd_ and reid_result_.match_status ==1) {
        pAiOut->nStatus = 3;
    } else{
        //pAiOut->nStatus = 1;
        if (UseTrace_ and (int(traces_.size()) > int(TraceLen_/2))){
            gud_rect_t boxPre = LinearSpeed_TracePredict(traces_);
            pAiOut->nX    = boxPre.cx;
            pAiOut->nY    = boxPre.cy;
            pAiOut->nObjW = boxPre.w;
            pAiOut->nObjH = boxPre.h;
            /// 跟踪不稳定时候是否使用线性预测位置来更新搜索区？待测
//            trackState.targetBox.x      =  boxPre.cx;
//            trackState.targetBox.y      =  boxPre.cy;
//            trackState.targetBox.width  =  boxPre.w;
//            trackState.targetBox.height =  boxPre.h;
        }
    }
#if (DEBUG_PRINT == 1)
    printf ("nDetectNum:[%d]?[%d], reidStatus:[%d] ( max:[%.2f] min:[%.2f] avg:[%.2f])\n",pAiOut->nDetectNum,
            acNumThd_,reid_result_.match_status,reid_result_.max_sim,reid_result_.min_sim,reid_result_.avg_sim);
#endif

}


void DaSiamRPNtracker::trackerProcessF(GuideImage *pSrcImg, gud_rect_f searchBox, ai_detect_result_t *od_res, AIT_OUTPUT *pAiOut)
{
    if (searchBox.used){
        outBox_.x      =  searchBox.cx;
        outBox_.y      =  searchBox.cy;
#if (0)
        outBox_.width  =  searchBox.w;
        outBox_.height =  searchBox.h;
#else
        outBox_.width  =  trackState.targetBox.width;
        outBox_.height =  trackState.targetBox.height;
#endif
        updateReg_ = true;
    }
    else{
        updateReg_ = false;
    }
    fullImg    = pSrcImg;

    trackerUpdateMnn(pAiOut);

    if (pAiOut->nDetectNum >= acNumThd_ and reid_result_.match_status ==1) {
        pAiOut->nStatus = 3;
    } else{
        //pAiOut->nStatus = 1;
        if (UseTrace_ and (int(traces_.size()) > int(TraceLen_/2))){
            gud_rect_t boxPre = LinearSpeed_TracePredict(traces_);
            pAiOut->nX    = boxPre.cx;
            pAiOut->nY    = boxPre.cy;
            pAiOut->nObjW = boxPre.w;
            pAiOut->nObjH = boxPre.h;
            /// 跟踪不稳定时候是否使用线性预测位置来更新搜索区？待测
//            trackState.targetBox.x      =  boxPre.cx;
//            trackState.targetBox.y      =  boxPre.cy;
//            trackState.targetBox.width  =  boxPre.w;
//            trackState.targetBox.height =  boxPre.h;
        }
    }
}

void DaSiamRPNtracker::trackerUpdateNpu(GuideImage *pSrcImg, cv::Rect2f &searRegion, bool useOutReg)
{
    cv::Rect2f targetBox;
    if (useOutReg) {
        targetBox = searRegion;
    }
    else {
        targetBox = trackState.targetBox;
    }
    float wc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
    float hc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
    float sz = sqrt(wc * hc);
    float scaleZ = trackState.exemplarSize / sz;
    float searchSize = float((trackState.instanceSize - trackState.exemplarSize) / 2);
    float pad = searchSize / scaleZ;
    float sx = sz + 2 * pad;

    int width = pSrcImg->img.au32Stride[0];
    int height = pSrcImg->img.u32Height;
    int bgrSize = width * height * 3;
    char *pSrcBgr = (char*)HI_MPI_SYS_MmapCache(pSrcImg->img.au64PhyAddr[0],bgrSize);
    cv::Mat img = cv::Mat(height,width, CV_8UC3,pSrcBgr);
    trackState.searchBox = targetBox;
    xCrop = getSubwindow(img, targetBox, (float)cvRound(sx), trackState.avgChans, trackState.instanceSize);

    tt1_ = steady_clock::now();
    uint8_t *pTmpBgr = (uint8_t *)subImg.img.au64VirAddr[0];
    cvImageToTensor(xCrop.data, pTmpBgr, 3, trackState.instanceSize, trackState.instanceSize);

    if (UpdataTemp_){
        float wc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
        float hc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
        float sz = (float)cvRound(sqrt(wc * hc));
        cv::Mat zCrop = getSubwindow(img, targetBox, sz, trackState.avgChans, trackState.exemplarSize);
        if (tagTemplates_.size() < TempNum_){
            tagTemplates_.push_back(zCrop);
        }
        else{
            tagTemplates_.pop_front();
            tagTemplates_.push_back(zCrop);
        }
    }
    HI_MPI_SYS_Munmap(pSrcBgr,bgrSize);

    detect_net_.run(&subImg,outTnsr);
    det_output01  =  outTnsr[0];   //  regBranch
    det_output02  =  outTnsr[1];   //  clsBranch
    //mnnDoubleBranch_->updateTwoMaps(det_output02.data, det_output01.data);
    memcpy(RegMap,det_output01.data, sizeof(float)*256*22*22);
    memcpy(ClsMap,det_output02.data, sizeof(float)*256*22*22);
    //usleep(DELAY_30MS);
    //printf("UpdateNpu ------------- index:%d\n", pSrcImg->indx);
    tt2_ = steady_clock::now();
    time_span_ = duration_cast < duration < double >> (tt2_ - tt1_);
    //printf("#Run detect_net_.run time is %f ms.\n",1000 * time_span_.count());
    nnieIdx_++;

}

void DaSiamRPNtracker::trackerUpdateMnn(AIT_OUTPUT *pAiOut)
{
    cv::Rect2f targetBox = trackState.targetBox;
    if (updateReg_){
        targetBox = outBox_;
        //printf ("OUTBOX:[%.2f %.2f %.2f %.2f ]\n",outBox_.x,outBox_.y,outBox_.width,outBox_.height);
    }
    float wc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
    float hc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
    float sz = sqrt(wc * hc);
    float scaleZ = trackState.exemplarSize / sz;

    tt3_ = steady_clock::now();
    //mnnDoubleBranch_->clsMnnForward(det_output01.data, ClsOutputData, true);
    //mnnDoubleBranch_->regMnnForward(det_output02.data, RegOutputData, true);
    mnnDoubleBranch_->clsMnnForward(ClsMap, ClsOutputData, false);
    mnnDoubleBranch_->regMnnForward(RegMap, RegOutputData, false);

    tt4_ = steady_clock::now();
    time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
#if (DEBUG_PRINT == 1)
    printf("#Run DoubleBranch_MnnForward Process (SumTotal)time is %f ms.\n",1000 * time_span_.count());
#endif
    double ttSpand = 1000 * time_span_.count();
    //showTrackerTimes(ttSpand);

    //tt1_ = steady_clock::now();
    cv::Mat delta(4, trackState.anchorNum * trackState.scoreSize * trackState.scoreSize, CV_32FC1, RegOutputData);
    cv::Mat score(2, trackState.anchorNum * trackState.scoreSize * trackState.scoreSize, CV_32FC1, ClsOutputData);

    score = score.reshape(0, { 2, trackState.anchorNum, trackState.scoreSize, trackState.scoreSize });
    score = score.row(1);
    score = score.reshape(0, { 5, 19, 19 });
    delta = delta.reshape(0, { 4, trackState.anchorNum, trackState.scoreSize, trackState.scoreSize });

    // post process
    delta.row(0) = delta.row(0).mul(trackState.anchors.row(2)) + trackState.anchors.row(0);
    delta.row(1) = delta.row(1).mul(trackState.anchors.row(3)) + trackState.anchors.row(1);
    exp(delta.row(2), delta.row(2));
    delta.row(2) = delta.row(2).mul(trackState.anchors.row(2));
    exp(delta.row(3), delta.row(3));
    delta.row(3) = delta.row(3).mul(trackState.anchors.row(3));

    targetBox.width *= scaleZ;
    targetBox.height *= scaleZ;

    sc = sizeCal(delta.row(2), delta.row(3)) / sizeCal(targetBox.width, targetBox.height);
    elementMax(sc);

    rc = delta.row(2).mul(1 / delta.row(3));
    rc = (targetBox.width / targetBox.height) / rc;
    elementMax(rc);

    exp(((rc.mul(sc) - 1.) * trackState.penaltyK * (-1.0)), penalty);
    penalty = penalty.reshape(0, { trackState.anchorNum, trackState.scoreSize, trackState.scoreSize });

    pscore = penalty.mul(score);
    pscore = pscore * (1.0 - trackState.windowInfluence) + trackState.windows * trackState.windowInfluence;

    int bestID[2] = { 0, 0 };
    // Find the index of best score.
    minMaxIdx(pscore.reshape(0, { trackState.anchorNum * trackState.scoreSize * trackState.scoreSize, 1 }), 0, 0, 0, bestID);
    delta = delta.reshape(0, { 4, trackState.anchorNum * trackState.scoreSize * trackState.scoreSize });
    penalty = penalty.reshape(0, { trackState.anchorNum * trackState.scoreSize * trackState.scoreSize, 1 });
    score = score.reshape(0, { trackState.anchorNum * trackState.scoreSize * trackState.scoreSize, 1 });

    int index[2] = { 0, bestID[0] };
    resBox.x = delta.at<float>(index) / scaleZ;
    index[0] = 1;
    resBox.y = delta.at<float>(index) / scaleZ;
    index[0] = 2;
    resBox.width = delta.at<float>(index) / scaleZ;
    index[0] = 3;
    resBox.height = delta.at<float>(index) / scaleZ;

    float lr = penalty.at<float>(bestID) * score.at<float>(bestID) * trackState.lr;

    resBox.x = resBox.x + targetBox.x;
    resBox.y = resBox.y + targetBox.y;
    targetBox.width /= scaleZ;
    targetBox.height /= scaleZ;

    resBox.width = targetBox.width * (1 - lr) + resBox.width * lr;
    resBox.height = targetBox.height * (1 - lr) + resBox.height * lr;
//        resBox.x += resBox.width / 2;
//        resBox.y += resBox.height / 2;

    resBox.x = float(fmax(0., fmin(float(trackState.imgSize.width), resBox.x)));
    resBox.y = float(fmax(0., fmin(float(trackState.imgSize.height), resBox.y)));
    resBox.width = float(fmax(D_MIN_TARGET_SIZE*1.0, fmin(float(trackState.imgSize.width), resBox.width)));
    resBox.height = float(fmax(D_MIN_TARGET_SIZE*1.0, fmin(float(trackState.imgSize.height), resBox.height)));


    trackState.targetBox = resBox;
    trackState.tracking_score = score.at<float>(bestID);
    //printf ("ResBox:{%.2f %.2f %.2f %.2f } bestId:{%d %d}\n",resBox.x,resBox.y,resBox.width,resBox.height,bestID[0],bestID[1]);

    cv::Mat sortID;
    cv::Mat sortScore;
    //得分排序
    cv::sort(score, sortScore, cv::SORT_EVERY_COLUMN + cv::SORT_DESCENDING);
    //序号排序
    cv::sortIdx(score, sortID, cv::SORT_EVERY_COLUMN + cv::SORT_DESCENDING);

    //获取得分靠前50个目标且score>0.85
    int nCnt = 0;
    int i = 0;
    AIT_TARGET *tTargetArray = (AIT_TARGET *)pAiOut->nDetectArray;

    cv::Rect2f targetSimilarBox = targetBox;
    for (i = 0; i < sortScore.rows; i++) {
        float fscore = sortScore.ptr<float>(i)[0];
        int nIdex = sortID.ptr<int>(i)[0];
        if (fscore > detThd_) {
            cv::Rect2f *tTarget = &tTargetArray[i].tTarget;
            int selectID[2] = { nIdex, 0 };
            int index[2] = { 0, nIdex };
            cv::Rect2f resBox = { 0, 0, 0, 0 };
            resBox.x = delta.at<float>(index) / scaleZ;
            index[0] = 1;
            resBox.y = delta.at<float>(index) / scaleZ;
            index[0] = 2;
            resBox.width = delta.at<float>(index) / scaleZ;
            index[0] = 3;
            resBox.height = delta.at<float>(index) / scaleZ;

            float lr = penalty.at<float>(selectID) * score.at<float>(selectID) * trackState.lr;

            resBox.x = resBox.x + targetSimilarBox.x;
            resBox.y = resBox.y + targetSimilarBox.y;

            resBox.width = targetSimilarBox.width  * (1 - lr) + resBox.width  * lr;
            resBox.height = targetSimilarBox.height * (1 - lr) + resBox.height * lr;

            resBox.x = float(fmax(0., fmin(float(trackState.imgSize.width), resBox.x)));
            resBox.y = float(fmax(0., fmin(float(trackState.imgSize.height), resBox.y)));
            resBox.width = float(fmax(D_MIN_TARGET_SIZE*1.0, fmin(float(trackState.imgSize.width), resBox.width)));
            resBox.height = float(fmax(D_MIN_TARGET_SIZE*1.0, fmin(float(trackState.imgSize.height), resBox.height)));
            nCnt++;

            if (nCnt == AIT_DETECTION_MAX_NUM) {
                break;
            }
            memcpy(tTarget, &resBox, sizeof(cv::Rect2f));
            tTargetArray[i].fScore = score.at<float>(selectID);
        }
    }

    pAiOut->nDetectNum = nCnt;

    if (pAiOut->nDetectNum >= acNumThd_) {
        pAiOut->nStatus = 3;
    }
//    else{
//        pAiOut->nStatus = 1;
//    }

    std::vector<cv::Rect> vec;
    for (i = 0; i < pAiOut->nDetectNum; i++) {
        cv::Rect ttTarget;
        ttTarget.x = (int)pAiOut->nDetectArray[i].tTarget.x - pAiOut->nDetectArray[i].tTarget.width / 2;
        ttTarget.y = (int)pAiOut->nDetectArray[i].tTarget.y - pAiOut->nDetectArray[i].tTarget.height / 2;
        ttTarget.width = (int)pAiOut->nDetectArray[i].tTarget.width;
        ttTarget.height = (int)pAiOut->nDetectArray[i].tTarget.height;
        vec.push_back(ttTarget);
    }
    cv::groupRectangles(vec, 1, 0.5);

    for (i = 0; i < (int)vec.size(); i++) {
        pAiOut->nSimilarArray[i].x = (int)vec[i].x + (int)vec[i].width / 2;
        pAiOut->nSimilarArray[i].y = (int)vec[i].y + (int)vec[i].height / 2;
        pAiOut->nSimilarArray[i].width = (int)vec[i].width ;
        pAiOut->nSimilarArray[i].height = (int)vec[i].height ;
    }
    pAiOut->nClusterNum = (int)vec.size();
    pAiOut->fProb   = trackState.tracking_score;
    pAiOut->nX      = trackState.targetBox.x;
    pAiOut->nY      = trackState.targetBox.y;
    pAiOut->nObjW   = trackState.targetBox.width;
    pAiOut->nObjH   = trackState.targetBox.height;

    pAiOut->fX      = trackState.targetBox.x;
    pAiOut->fY      = trackState.targetBox.y;
    pAiOut->fObjW   = trackState.targetBox.width;
    pAiOut->fObjH   = trackState.targetBox.height;
#if (DEBUG_PRINT == 1)
    printf ("*** tracking_score:{%.2f} nDetectNum:{%d} nClusterNum:{%d}\n",
            trackState.tracking_score,pAiOut->nDetectNum,pAiOut->nClusterNum);
#endif
    mnnIdx_++;
//    tt2_ = steady_clock::now();
//    time_span_ = duration_cast < duration < double >> (tt2_ - tt1_);
//    printf("#Run TotalPostProcess time is %f ms.\n",1000 * time_span_.count());
}



cv::Mat DaSiamRPNtracker::generateAnchors()
{
    int totalStride = trackState.totalStride, scales = trackState.scale, scoreSize = trackState.scoreSize;
    std::vector<float> ratios = trackState.ratios;
    std::vector<cv::Rect2f> baseAnchors;
    int anchorNum = int(ratios.size());
    int size = totalStride * totalStride;
    float ori = -(float(scoreSize / 2)) * float(totalStride);

    for (auto i = 0; i < anchorNum; i++)
    {
        int ws = int(sqrt(size / ratios[i]));
        int hs = int(ws * ratios[i]);

        float wws = float(ws) * scales;
        float hhs = float(hs) * scales;
        cv::Rect2f anchor = { 0, 0, wws, hhs };
        baseAnchors.push_back(anchor);
    }

    int anchorIndex[4] = { 0, 0, 0, 0 };
    const int sizes[4] = { 4, (int)ratios.size(), scoreSize, scoreSize };

    cv::Mat anchors(4, sizes, CV_32F);

    for (auto i = 0; i < scoreSize; i++)
    {
        for (auto j = 0; j < scoreSize; j++)
        {
            for (auto k = 0; k < anchorNum; k++)
            {
                anchorIndex[0] = 1, anchorIndex[1] = k, anchorIndex[2] = i, anchorIndex[3] = j;
                anchors.at<float>(anchorIndex) = ori + totalStride * i;

                anchorIndex[0] = 0;
                anchors.at<float>(anchorIndex) = ori + totalStride * j;

                anchorIndex[0] = 2;
                anchors.at<float>(anchorIndex) = baseAnchors[k].width;

                anchorIndex[0] = 3;
                anchors.at<float>(anchorIndex) = baseAnchors[k].height;
            }
        }
    }
    return anchors;
}

cv::Mat DaSiamRPNtracker::generateHanningWindow()
{
    cv::Mat baseWindows, HanningWindows;
    cv::createHanningWindow(baseWindows, cv::Size(trackState.scoreSize, trackState.scoreSize), CV_32F);
    baseWindows = baseWindows.reshape(0, { 1, trackState.scoreSize, trackState.scoreSize });
    HanningWindows = baseWindows.clone();
    for (int i = 1; i < trackState.anchorNum; i++)
    {
        HanningWindows.push_back(baseWindows);
    }
    return HanningWindows;
}

cv::Mat DaSiamRPNtracker::getSubwindow(cv::Mat& img, const cv::Rect2f& targetBox, float originalSize,
                                       cv::Scalar avgChans, const int& out_sz)
{
    struct timeval tStart0, tStart;

    cv::Mat zCrop, dst;

    cv::Size imgSize = img.size();
    float c = (originalSize + 1) / 2;
    float xMin = (float)cvRound(targetBox.x - c);
    float xMax = xMin + originalSize - 1;
    float yMin = (float)cvRound(targetBox.y - c);
    float yMax = yMin + originalSize - 1;

    int leftPad = (int)(fmax(0., -xMin));
    int topPad = (int)(fmax(0., -yMin));
    int rightPad = (int)(fmax(0., xMax - imgSize.width + 1));
    int bottomPad = (int)(fmax(0., yMax - imgSize.height + 1));

    xMin = xMin + leftPad;
    xMax = xMax + leftPad;
    yMax = yMax + topPad;
    yMin = yMin + topPad;

    if (topPad == 0 && bottomPad == 0 && leftPad == 0 && rightPad == 0)
    {
        img(cv::Rect(int(xMin), int(yMin), int(xMax - xMin + 1), int(yMax - yMin + 1))).copyTo(zCrop);

    }
    else
    {
        cv::copyMakeBorder(img, dst, topPad, bottomPad, leftPad, rightPad, cv::BORDER_CONSTANT, avgChans);
        dst(cv::Rect(int(xMin), int(yMin), int(xMax - xMin + 1), int(yMax - yMin + 1))).copyTo(zCrop);
    }

    trackState.searchBox.x = int(xMin);
    trackState.searchBox.y = int(yMin);
    trackState.searchBox.width = int(xMax - xMin + 1);
    trackState.searchBox.height =  int(yMax - yMin + 1);

//    draw_rect(img,cv::Rect(int(xMin), int(yMin), int(xMax - xMin + 1), int(yMax - yMin + 1)),2);
//    std::string regionImg = "/nfsroot/001_3559/results/sear_" + std::to_string(trackState.FRN) + ".jpg";
//    cv::imwrite(regionImg, img);

    cv::Size model_sz = {out_sz, out_sz};
    cv::Mat resizedPatch(out_sz, out_sz, CV_8UC3);
    cv::resize(zCrop, resizedPatch, model_sz, 0, 0, cv::INTER_LINEAR);

    return resizedPatch;
}


void DaSiamRPNtracker::showFloatInputs(float* input, int size, int diff)
{
    printf("******* Inputs total size:{%d} **********\n",size);
    int lenN = 1;
    for (int i = 0; i < size; ++i)
    {
        printf ("%.3f ",input[i]);
        if (i%(diff) == 0 and i!=0){
            printf("\n#[%d]#\n",lenN);
            lenN++;
        }
    }
    printf("******* Inputs showDataEnd: **********\n");
}

void DaSiamRPNtracker::showTrackerTimes(double timeEsp)
{
    tkTime_.updateTimes++;
    if (timeEsp > tkTime_.timeThd)
    {
        tkTime_.expandTimes++;
    }
    if (timeEsp > tkTime_.maxTime)
        tkTime_.maxTime = timeEsp;
    if (timeEsp  < tkTime_.minTime)
        tkTime_.minTime = timeEsp;
    if (tkTime_.updateTimes!=0)
        tkTime_.expandRate = (tkTime_.expandTimes*1.0)/tkTime_.updateTimes;
    //printf ("*** CurrentUpdateTime:{%.2f}(ms) maxTime:{%.2f} minTime:{%.2f} expand [%.2f](ms)->[%d]/[%d] = rate:[%.5f]\n",
    //        timeEsp,tkTime_.maxTime,tkTime_.minTime,tkTime_.timeThd,tkTime_.expandTimes,tkTime_.updateTimes,tkTime_.expandRate);
}

void  DaSiamRPNtracker::draw_rect(cv::Mat frame, cv::Rect targetBox, int type)
{
    cv::Point  pt1, pt2;
    int tw, th;
    {
        pt1.x = int(targetBox.x);
        pt1.y = int(targetBox.y);
        pt2.x = int(targetBox.x + targetBox.width);
        pt2.y = int(targetBox.y + targetBox.height);
        tw =  int(targetBox.width);
        th =  int(targetBox.height);
    }

    if (tw * th != 0){
        if (type == 0)
            cv::rectangle(frame, pt1, pt2, cv::Scalar(255, 0, 0), 3); //blue
        if (type == 1)
            cv::rectangle(frame, pt1, pt2, cv::Scalar(0, 255, 0), 3); // green
        if (type == 2)
            cv::rectangle(frame, pt1, pt2, cv::Scalar(0, 0, 255), 3);  //red
        if (type == 3)
            cv::rectangle(frame, pt1, pt2, cv::Scalar(0, 255, 255), 3); //yellow
    }
}


gud_rect_t DaSiamRPNtracker::LinearSpeed_TracePredict(std::deque<gud_rect_t> &trace)
{
    int diff_x = 0;
    int diff_y = 0;
    int dirct_x = 0;
    int dirct_y = 0;

    int t_num = trace.size();
    if (t_num < 2)
        return trace.at(0);

    gud_rect_t start_rect = trace.at(0);
    gud_rect_t end_rect = trace.at(t_num-1);

    int start_cx = start_rect.cx;
    int start_cy = start_rect.cy;
    int end_cx   = end_rect.cx;
    int end_cy   = end_rect.cy;

    for (int i = 1; i < t_num; ++i) {
        gud_rect_t tag_old = trace.at(i-1);
        gud_rect_t tag_new = trace.at(i);
        int ocx = tag_old.cx;
        int ocy = tag_old.cy;
        int ncx = tag_new.cx;
        int ncy = tag_new.cy;

        diff_x += abs(ncx - ocx);
        diff_y += abs(ncy - ocy);

        if (abs(ncx - ocx)!=0){
            if ((ncx - ocx) > 0){
                dirct_x++;
            }else{
                dirct_x--;
            }
        }
        if (abs(ncy - ocy)!=0){
            if ((ncy - ocy) > 0){
                dirct_y++;
            }else{
                dirct_y--;
            }
        }
    }
    if (dirct_x!=0){
        if (dirct_x > 0)
            dirct_x = 1;
        else
            dirct_x = -1;
    }
    if (dirct_y!=0){
        if (dirct_y > 0)
            dirct_y = 1;
        else
            dirct_y = -1;
    }

    float vx = dirct_x * diff_x * 1.0 / (t_num - 1);
    float vy = dirct_y * diff_y * 1.0 / (t_num - 1);

#if (DEBUG_PRINT == 1)
    gud_rect_t pre_rect;
    pre_rect.cx = end_rect.cx + (int)(ceil(vx));
    pre_rect.cy = end_rect.cy + (int)(ceil(vy));
    pre_rect.w  = end_rect.w;
    pre_rect.h  = end_rect.h;
    printf ("\n#### linearspeed_by_trace X:%f Y:%f preCX:%d preCY:%d\n",vx,vy,pre_rect.cx,pre_rect.cy);
#endif
    return end_rect;
}

int DaSiamRPNtracker::calRectIOU(gud_rect_t re1, gud_rect_t re2)
{
    int iou = 0;
    int re1_ul_x = static_cast<int>(re1.cx - re1.w/2);
    int re1_ul_y = static_cast<int>(re1.cy - re1.h/2);
    int re1_lr_x = re1_ul_x + re1.w;
    int re1_lr_y = re1_ul_y + re1.h;

    int re2_ul_x = static_cast<int>(re2.cx - re2.w/2);
    int re2_ul_y = static_cast<int>(re2.cy - re2.h/2);
    int re2_lr_x = re2_ul_x + re2.w;
    int re2_lr_y = re2_ul_y + re2.h;

    int max_left = MAX(re1_ul_x, re2_ul_x);
    int min_right = MIN(re1_lr_x, re2_lr_x);
    int max_up = MAX(re1_ul_y, re2_ul_y);
    int min_down = MIN(re1_lr_y, re2_lr_y);

    if (min_right - max_left < 0 || min_down - max_up < 0)
    {
        return 0;
    }

    else
    {
        int overflow_area = (min_right - max_left) * (min_down - max_up);
        int re1_area = (re1_lr_x - re1_ul_x) * (re1_lr_y - re1_ul_y);
        int re2_area = (re2_lr_x - re2_ul_x) * (re2_lr_y - re2_ul_y);
        if (overflow_area!=0 and re1_area!=0){
            iou = static_cast<int>(overflow_area  * 100 / (re1_area + re2_area - overflow_area));
            return iou;
        }
        else
            return 0;
    }
}

void DaSiamRPNtracker::reid_jsonReader(const char *reid_path, gud_reid_config_t *reid_config)
{
    /******************* OT_Json File Read *******************/

    FILE *fp;
    cJSON *json;
    fp = fopen(reid_path, "rb");
    fseek(fp, 0, SEEK_END);
    long len = ftell(fp);
    fseek(fp, 0, SEEK_SET);
    char *content = (char *) malloc(sizeof(char) * (len + 1));
    //printf("***** 0jsonRead okokokok*****\n");
    fread(content, 1, len, fp);
    fclose(fp);

    /******************* Reid_Json File Read *******************/
    json = cJSON_Parse(content);

    if (json == NULL) {
        cout << "cJSON is readed failed!\n";
        exit(1);
    }

    cJSON *json_input_n ,*json_input_c, *json_input_w, *json_input_h,*json_cycleup;
    cJSON *json_output_n ,*json_output_c, *json_output_w, *json_output_h,*json_upratio;
    cJSON  *json_sim_thd, *json_feat_len, *json_gallery_num, *json_model;

    json_input_n = cJSON_GetObjectItem(json, "input_n");
    json_input_c = cJSON_GetObjectItem(json, "input_c");
    json_input_w = cJSON_GetObjectItem(json, "input_w");
    json_input_h = cJSON_GetObjectItem(json, "input_h");
    json_output_n = cJSON_GetObjectItem(json, "output_n");
    json_output_c = cJSON_GetObjectItem(json, "output_c");
    json_output_w = cJSON_GetObjectItem(json, "output_w");
    json_output_h = cJSON_GetObjectItem(json, "output_h");

    json_cycleup = cJSON_GetObjectItem(json, "cycle_update");
    json_upratio = cJSON_GetObjectItem(json, "update_ratio");

    json_sim_thd = cJSON_GetObjectItem(json, "sim_thd");
    json_feat_len = cJSON_GetObjectItem(json, "feat_len");
    json_gallery_num = cJSON_GetObjectItem(json, "gallery_num");
    json_model = cJSON_GetObjectItem(json, "model");

    reid_config->input_n = json_input_n->valueint;
    reid_config->input_c = json_input_c->valueint;
    reid_config->input_w = json_input_w->valueint;
    reid_config->input_h = json_input_h->valueint;

    reid_config->output_n = json_output_n->valueint;
    reid_config->output_c = json_output_c->valueint;
    reid_config->output_w = json_output_w->valueint;
    reid_config->output_h = json_output_h->valueint;

    reid_config->cycle_update = json_cycleup->valueint;
    reid_config->update_ratio = json_upratio->valuedouble;

    reid_config->sim_thd = json_sim_thd->valuedouble;
    reid_config->feat_len = json_feat_len->valueint;
    reid_config->gallery_num = json_gallery_num->valueint;
    reid_config->model = json_model->valuestring;
#if 1
    printf("*****GUD_ALG_REID jsonReader is started*****\n");
    printf ("net_w:%d net_h:%d feat_len:%d g_num:%d sim_thd:%f\n",reid_config->input_w,reid_config->input_h,reid_config->feat_len,
          reid_config->gallery_num, reid_config->sim_thd);
    printf("cycle_update:[%d] update_ratio[%.2f] model:%s\n",reid_config->cycle_update,reid_config->update_ratio,reid_config->model);
    printf("*****GUD_ALG_REID jsonReader is END*****\n");
#endif
}