#include <iostream>
#include <cstdlib>
#include <string>
#include <fstream>
#include <dirent.h>
#include <unistd.h>
#include <sys/stat.h>
#include <thread>
#include "ff_decode.hpp"
#include <cstdio>
#include <cstdint>
#include "demo_utils.h"
#include "Arith_Nano_Tracker_API.h"
#include "detection_type_api.h"
#include "NeoArithStandardDll.h"
#include "AI_API.h"

#define D_CPU_DEBUG          (0)
#define D_UPDATE_KERNEL      (0)
#define D_DEBUG_SAVE_VIDEO   (1)
#define D_USE_TRACKER_API    (1)    // always set 1, use ai_track to init kcf tracker
#define D_USE_DETECTER_API   (1)
#define D_DO_DETECT_FRQ      (10)
#define D_AI_PARALLEL        (1)

using namespace AIGO;
using namespace std;
using namespace std::chrono;
unsigned char pSrc[1920*1080]   = {0};
static int DETECT_MODEL_TYPE = 0;
static int IMAGEWIDTH        = 0;
static int IMAGEHEIGHT       = 0;

void Usage(char *pchPrgName) {
    printf("Usage : %s  \n", pchPrgName);
    printf("\t 0) Usage: OD_OT_Demo.soc  + detectModel(0:full_1280x1024 1:part_640x512) + sequencePath\n");
}



#if (D_AI_PARALLEL == 0)
void track_detection_demo_serial(const char* jsPath, const char *video_path)
{
    gud_nano_config_t nanoConf;
    ot_v2_jsonReader(jsPath,&nanoConf);

    char *img_list = new char[400];
    char *label_txt = new char[400];

    strcpy(img_list, video_path);
    strcat(img_list, "/imglist.txt");
    strcpy(label_txt, video_path);
    strcat(label_txt, "/init.json");
    steady_clock::time_point t1;
    steady_clock::time_point t2;
    duration<double> time_span_;

    string imgTxtPath = img_list;
    std::cout<<"Reading img_list:"<< imgTxtPath << std::endl;
    vector <string> vecImgs = ImgsTxtReader(imgTxtPath);
#if (D_USE_TRACKER_API == 1)
    ArithHandle nTracker;
    GUD_ALG_AI_TRACK_Create(&nTracker, &nanoConf);
#endif
    // AI_Track算法输入部分
    ARIDLL_INPUTPARA stInputPara = { 0 };
    stInputPara.unFrmId++;
    stInputPara.stCameraInfo.fPixelSize = 15;
    stInputPara.stCameraInfo.nFocus = 300;

    // kcf_track init
    // 创建算法句柄
    ArithHandle pTracker = STD_CreatEOArithHandle();
    ARIDLL_OUTPUT pstOutput = { 0 };           //算法结果
    // 算法输出部分
    ARIDLL_OUTPUT stOutput = { 0 };

    // 构建图像类型
    GD_VIDEO_FRAME_S img = { 0 };
    img.enPixelFormat = GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_BGR_PACKED;
    GD_VIDEO_FRAME_S imgY8 = { 0 };

#if (D_USE_DETECTER_API == 1)
    static bool TESTVIS = false;
    static bool TESTIR = false;
    std::string visdetmodeFull = "../../models/Detect/1684x/vis_common_headers_1280x1024_bm1684x_F16_false.bmodel";
    std::string visdetmodePart = "../../models/Detect/1684x/vis_common_headers_640x512_bm1684x_F16_false.bmodel";

    // 设置队列大小，默认调用即可
    int ret = 0;
    ret = Init_ACL();
    if(ret){
        printf("Init_ACL error\n");
    }
    ret = set_deque_size(5,5,5,5,5,5,5,5,9,9,9,9);
    if(ret){
        printf("set_deque_size error\n");
    }
    printf("deque setted\n");
    // 设置红外图像分辨率
    ret = set_stream_size(IMAGEWIDTH, IMAGEHEIGHT, IR);
    if(ret){
        printf("set_stream_size error\n");
    }
    // 设置可见光图像分辨率
    ret = set_stream_size(IMAGEWIDTH, IMAGEHEIGHT, VIS);
    if(ret){
        printf("set_stream_size error\n");
    }
    //设置类别标签， 标签标请咨询对应的算法工程师
    uint32_t modelclass[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14};
    uint32_t standerclass[] = {10000, 20000, 30000, 40000, 50000,
                               60000, 70000, 80000, 90000, 100000,
                               110000, 120000, 130000, 140000, 150000};
    ret = setLabelMap(modelclass, standerclass, sizeof(modelclass)/sizeof(uint32_t));

    if(ret){
        printf("setLabelMap error with ret=%d\n", ret);
    }
    // 设置数据流格式，NV12 or RGB
    ret = set_StreamMode(AIGO::INPUT_LAYOUT::NV12);
    if(ret){
        printf("set_StreamMode error with ret=%d\n", ret);
    }
    // 初始化模型
    if (DETECT_MODEL_TYPE == 0){
        ret = Init_AI(visdetmodeFull.c_str(), visdetmodeFull.c_str(), NULL, NULL);
    }else{
        ret = Init_AI(visdetmodePart.c_str(), visdetmodePart.c_str(), NULL, NULL);
    }

    if(ret){
        printf("init error\n");
    }
    TESTVIS = true;
    TESTIR = false;

    ret = set_dropout_param(false);
    if(ret){
        printf("set dropout error\n");
    }
    // ret = switch_vis_to_ir();
    printf("get det threshold\n");
    int cNum = sizeof(modelclass)/sizeof(uint32_t);
    for (int z = 0; z < cNum; ++z) {
        ret = set_det_threshold(0.45, standerclass[z]);
    }

    if(ret){
        printf("set det threshold error\n");
    }
    //std::vector<objinfo> visdets;
    TenMat * maskImg = (TenMat*)malloc(sizeof(TenMat));
    void * maskdata = (void*)malloc(IMAGEWIDTH*IMAGEHEIGHT*sizeof(int64_t));
    printf ("==========AI DETECTION INIT SUCESS=========\n");
#endif

#if (D_DEBUG_SAVE_VIDEO == 1)
    VideoWriter writer;
    Size size = cv::Size(1280, 720);
    writer.open("../../results/detect_track_serial_bm1684x.avi", cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), 25, size, true);
    printf("Saving Track Video.....\n");
#endif

    gud_rect_t1 initBox;
    resolve_labeled_json(label_txt, &initBox);
    int x1 = initBox.ul.x;
    int y1 = initBox.ul.y;
    int w1 = initBox.lr.x - x1;
    int h1 = initBox.lr.y - y1;
    cv::Rect trackWindow =cv::Rect(x1,y1,w1,h1);         // 固定值
    cv::Mat show_frame,frame;
    // Initialize tracker with first frame and rect.
    //State state;
    int frn = 0;
    int outTn = 0;
    double avgT = 0;
    float avgTScore = 1.0;
    float ac_score = 0.7;
    float upKerThd = 0.95;
    int dq_size = 10;
    std::deque<float> ot_scores;
    std::vector<objinfo> odRes;
    std::vector<objinfo> visdets;
    AIT_OUTPUT otRes;

    for (int i = 0; i < vecImgs.size(); i++){
        frn = i;
        printf("\n~~~~~~~~~~~~~~~Frame:[%d/%d]~~~~~~~~~~~~~~~~~\n", frn,vecImgs.size());
        const char *imgfile = vecImgs[i].data();
        frame = cv::imread(imgfile, cv::IMREAD_COLOR);
        int imgW = frame.cols;  // img_w
        int imgH = frame.rows; // img_h

        img.u32Width = imgW;
        img.u32Height = imgH;
        img.u32Stride[0] = img.u32Width * 3;
        img.u64VirAddr[0] = frame.data;

        stInputPara.unFrmId = frn;
        stInputPara.stServoInfo.fServoAz = 0.01;
        stInputPara.stServoInfo.fServoPt = 0.01;

        cv::Mat srcGray(imgW,imgH,CV_8UC1);
        cv::cvtColor(frame,srcGray,cv::COLOR_RGB2GRAY);
        memcpy(pSrc, srcGray.data,sizeof(unsigned char)*imgW*imgH);

        imgY8.enPixelFormat = GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_GRAY_Y8;
        imgY8.u32Width      = imgW;
        imgY8.u32Height     = imgH;
        imgY8.u32Stride[0]  = img.u32Width * 1;
        imgY8.u64VirAddr[0] = pSrc;

        printf("Reading image %s Sucess IMG_SZIE:(%d %d)!\n", imgfile,imgW,imgH);
        if (frame.empty())
            break;

#if (D_USE_DETECTER_API == 1)
        if (frn % D_DO_DETECT_FRQ == 0){
            int ww = frame.cols;
            int hh = frame.rows;
            //cv::Mat imgpad;
            //cv::copyMakeBorder(frame, imgpad, 0, (std::max(ww,hh)-hh), 0, (std::max(ww, hh) - ww), cv::BORDER_CONSTANT, cv::Scalar(0,0,0));
            // cv::Mat imgRGB;
            // cv::cvtColor(imgpad, imgRGB, cv::COLOR_BGR2RGB);
            int UpPx, UpPy;
            cv::Mat resizedImg;
            if (DETECT_MODEL_TYPE==0){
                cv::resize(frame, resizedImg, cv::Size(IMAGEWIDTH, IMAGEHEIGHT));
            }else{
                UpPx = imgW/2-IMAGEWIDTH/2;
                UpPy = imgH/2-IMAGEHEIGHT/2;
                cv::Rect region_rect(UpPx,UpPy, IMAGEWIDTH,IMAGEHEIGHT);
                resizedImg = frame(region_rect).clone();
            }

            cv::Mat NV12Mat;
            BGR2YUV_nv12(resizedImg, NV12Mat);

            ImgMat imgvis;
            imgvis.data = NV12Mat.data;
            imgvis.width = resizedImg.cols;
            imgvis.height = resizedImg.rows;
            imgvis.depth = 1;
            imgvis.layout = HWC;
            imgvis.inlayout = AIGO::INPUT_LAYOUT::NV12;
            imgvis.mode = VIS;
            imgvis.timestamp = i;//(long long)imgnames.at(i).c_str();
            // printf("\nimg.timestamp: %d\n", imgvis.timestamp);

            long long timestamp = 0;
            if(TESTVIS){
                maskImg->data = maskdata;
                t1 = steady_clock::now();
                ret = Det_VL(imgvis, visdets);
                t2 = steady_clock::now();
                time_span_ = duration_cast < duration < double >> (t2 - t1);
                std::cout << "==========================" << std::endl;
                printf("# GUD_ALG_AI_DETECTION time is %f ms.\n",1000 * time_span_.count());
                printf ("**** F:[%d] AIOD_Detect num:[%d] timestamp: %d\n",frn,visdets.size(),timestamp);
                odRes.clear();
                float ratioW, ratioH;
                if (DETECT_MODEL_TYPE==0){
                    ratioW = (imgW *1.0) / IMAGEWIDTH;
                    ratioH = (imgH *1.0) / IMAGEHEIGHT;}
                else{
                    ratioW = 1.0;
                    ratioH = 1.0;
                }
                if (visdets.size()>0 and ret == 0) {
                    for (int j = 0; j < visdets.size(); ++j) {
                        objinfo reMaptag = visdets[j];
                        std::vector<int> color = get_color(visdets[j].classNum);
                        cv::Rect odres, odRemap;
                        odres.x = visdets[j].x1;
                        odres.y = visdets[j].y1;
                        odres.width = visdets[j].x2 - odres.x;
                        odres.height = visdets[j].y2 - odres.y;

                        if (DETECT_MODEL_TYPE==0){
                            odRemap.x = static_cast<int>(odres.x * ratioW);
                            odRemap.y = static_cast<int>(odres.y * ratioH);
                            odRemap.width = static_cast<int>(odres.width * ratioW);
                            odRemap.height = static_cast<int>(odres.height * ratioH);

                            reMaptag.x1 = visdets[j].x1 * ratioW;
                            reMaptag.y1 = visdets[j].y1 * ratioH;
                            reMaptag.x2 = visdets[j].x2 * ratioW;
                            reMaptag.y2 = visdets[j].y2 * ratioH;

                        }
                        else{
                            odRemap.x = static_cast<int>(odres.x + UpPx);
                            odRemap.y = static_cast<int>(odres.y + UpPy);
                            odRemap.width = static_cast<int>(odres.width * ratioW);
                            odRemap.height = static_cast<int>(odres.height * ratioH);

                            reMaptag.x1 = visdets[j].x1 + UpPx;
                            reMaptag.y1 = visdets[j].y1 + UpPy;
                            reMaptag.x2 = visdets[j].x2 + UpPx;
                            reMaptag.y2 = visdets[j].y2 + UpPy;
                        }

                        if (visdets[j].classNum == 60000){
                            cv::putText(resizedImg,to_string(j), cv::Point(odres.x, odres.y-8),
                                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(color[0], color[1], color[2]));
                            cv::rectangle(resizedImg, odres , cv::Scalar(color[0], color[1], color[2]));

                            cv::putText(frame,to_string(j), cv::Point(odRemap.x, odRemap.y-8),
                                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(color[0], color[1], color[2]));
                            cv::rectangle(frame, odRemap , cv::Scalar(color[0], color[1], color[2]));
                        }

                        printf ("od_[%d] class:[%d]\n",j,visdets[j].classNum);
                        odRes.push_back(reMaptag);
                    }
                }
                std::string full_out_img0 = "../../results/" + to_string(frn) + "_det.jpg";
                //cv::imwrite(full_out_img0, resizedImg);
            }
        }
#endif

        if (i==0)
        {
            gud_rect_f initRect;
            initRect.w = (float)trackWindow.width;
            initRect.h = (float)trackWindow.height;
            initRect.cx = (float)(trackWindow.x + trackWindow.width/2);
            initRect.cy = (float)(trackWindow.y + trackWindow.height/2);
            initRect.used = 1;
            initRect.label = 60000;   // manual set label for init track target from detection resuluts
#if (D_USE_TRACKER_API == 1)
            t1 = steady_clock::now();
            GUD_ALG_AI_TRACK_Init(nTracker, img, initRect, odRes, &otRes);
            t2 = steady_clock::now();
            time_span_ = duration_cast < duration < double >> (t2 - t1);
            std::cout << "==========================" << std::endl;
            std::cout << "GUD_ALG_AI_TRACK_Track_Init done!" << std::endl;
            printf("# Nanotracker init time is %f ms.\n",1000 * time_span_.count());
            std::cout << "==========================" << std::endl;
            std::cout << std::endl;
#endif
            for (int j = 0; j < int(dq_size*0.5); ++j)
            {
                ot_scores.push_back(1.0);
            }

            cv::putText(frame,"INIT", cv::Point(trackWindow.x, trackWindow.y-8),
                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
            cv::rectangle(frame, trackWindow , cv::Scalar(0, 0,255));

            // 初始化为凝视-对地模式
            ARIDLL_EOArithInitWithMode(pTracker,imgW,imgH,GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_GRAY_Y8,
                                       GLB_SYS_MODE::GLB_SYS_STARE,GLB_SCEN_MODE::GLB_SCEN_GROUND);

        }else{
            // Start timer
            double t = (double)cv::getTickCount();
            // Update tracker
            float tkscore = 0;
#if (D_USE_TRACKER_API == 1)
            t1 = steady_clock::now();
            gud_rect_f searchBox;
            searchBox.used = 0;
            GUD_ALG_AI_TRACK_Process(nTracker, img, searchBox, odRes, &otRes);
            cv::Rect rect;
            rect.width  = otRes.nObjW;
            rect.height = otRes.nObjH;
            rect.x = otRes.nX - otRes.nObjW/2;
            rect.y = otRes.nY - otRes.nObjH/2;
            tkscore = otRes.fProb;
            t2 = steady_clock::now();
            printf ("#F:[%d], AI_trackbbox:[%d %d %d %d] score:[%f]\n",frn,rect.x,rect.y,rect.width,rect.height,tkscore);
            time_span_ = duration_cast < duration < double >> (t2 - t1);
            printf("# GUD_ALG_AI_TRACK_Process (total) time is %f ms.\n",1000 * time_span_.count());
#endif

            //1仿真下发锁定指令
            if (frn == 3) {
#if (D_USE_TRACKER_API == 1)
                int SelectX = otRes.nX;
                int SelectY = otRes.nY;
                int setLockBoxW = otRes.nObjW;
                int setLockBoxH = otRes.nObjH;
#else

                int setLockBoxW = (int)trackWindow.width;
                int setLockBoxH = (int)trackWindow.height;
                int SelectX = (int)(trackWindow.x + setLockBoxW/2);
                int SelectY = (int)(trackWindow.y + setLockBoxH/2);
#endif
                ARIDLL_LockCommand(pTracker,SelectX, SelectY, setLockBoxW, setLockBoxH);
            }

            t1 = steady_clock::now();
            // 调用TLD流程
            //ARIDLL_RunTLDTracker(pTracker, img);
            ARIDLL_RunController(pTracker, imgY8, stInputPara, &stOutput);
            t2 = steady_clock::now();
            time_span_ = duration_cast < duration < double >> (t2 - t1);

            // Calculate Frames per second (FPS)
            double fps = cv::getTickFrequency() / ((double)cv::getTickCount() - t);

            auto trackerOut = stOutput.stTrackers[0];
            cv::Rect outRect;
            outRect.width = MAX(15,(int)trackerOut.nObjW);
            outRect.height= MAX(15, (int)trackerOut.nObjW);
            outRect.x = (int)trackerOut.nX-outRect.width/2;
            outRect.y = (int)trackerOut.nY-outRect.height/2;

            //****************** kcf_tld results **********************************
            int x1 = outRect.x;
            int y1 = outRect.y;
            int w  = outRect.width;
            int h  = outRect.height;
            float score = trackerOut.fConf;
            if (frn>3){
                printf("#F:[%d] KCF_trackbbox:[%d %d %d %d] prob:[%.2f] time:[%.2f] (ms)\n",frn,x1,y1,w,h,score,1000*time_span_);
                // draw kcf_tld track results
                cv::putText(frame,"KCF", cv::Point(outRect.x, outRect.y-8),
                            cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 0, 255));
                cv::rectangle(frame, outRect , cv::Scalar(0, 0,255));
                cv::putText(frame,std::to_string(score), cv::Point(outRect.x+outRect.width, outRect.y+outRect.height),
                            cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 0, 255),1);

            }
#if (D_USE_TRACKER_API == 1)
            if(avgTScore<=ac_score)
            {
                cv::putText(frame,"LOSS", cv::Point(rect.x, rect.y-8),
                            cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 0));
                cv::rectangle(frame, rect,cv::Scalar(255, 0,0));
                cv::putText(frame,std::to_string(tkscore), cv::Point(rect.x+rect.width, rect.y+rect.height),
                            cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 0, 0),1);
            }
            else
            {
                cv::putText(frame,"AI", cv::Point(rect.x, rect.y-8),
                            cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
                cv::rectangle(frame, rect , cv::Scalar(0, 255,0));
                cv::putText(frame,std::to_string(tkscore), cv::Point(rect.x+rect.width, rect.y+rect.height),
                            cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0),1);
            }

            float tS = 1000 * time_span_.count();
            if (tS>40){
                outTn+=1;
                printf ("over time:[%d]\n",outTn);
            }
            avgT += tS*1.0;

            // Boundary judgment.
            cv::Mat track_window;
            if (0 <= rect.x && 0 <= rect.width && rect.x + rect.width <= frame.cols && 0 <= rect.y && 0 <= rect.height && rect.y + rect.height <= frame.rows)
            {
                //cv::rectangle(frame, rect, cv::Scalar(0, 255, 0));

            }

            bool track_status_smooth = UpdateTrackScoreDeque(ot_scores, tkscore, dq_size, ac_score, &avgTScore);
            printf ("#F:[%d],currectScore:[%.4f] smooth-score:[%.4f]\n",frn,tkscore,avgTScore);
#endif

#if (D_UPDATE_KERNEL==1)
            if (avgTScore > upKerThd and frn%10==0)
            {
                trackWindow = ExpandRect(rect, 1.1, imgW, imgH);
                //siam_tracker->init(frame, trackWindow);
                printf("F:[%d] smooth-score:[%.4f]> [%.4f],update Template Feature\n",frn,avgTScore,upKerThd);
            }
#endif

            // Display FPS
            std::cout << "FPS: " << fps << std::endl;
            std::cout << std::endl;
            cv::putText(frame,"FPS "+ std::to_string( fps), cv::Point(128, 20),
                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 0, 255),1);

            cv::putText(frame,"Frn: "+ std::to_string( frn), cv::Point(28, 20),
                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 0, 255),1);


            if (i >= vecImgs.size())
            {
                break;
            }

        }
#if (D_DEBUG_SAVE_VIDEO == 1)
        cv::resize(frame, show_frame, size);
        writer.write(show_frame);
#endif
    }
    printf ("{%d}/{%d} ,averageTime:%f (ms),rate:[%f]\n",outTn,vecImgs.size(),avgT/frn,outTn*1.0/vecImgs.size());
    writer.release();
#if (D_USE_TRACKER_API == 1)
    GUD_ALG_AI_TRACK_Release(nTracker);
    printf ("GUD_ALG_AI_TRACK_Release Sucess!\n");
#endif
#if (D_USE_DETECTER_API == 1)
    ret = Uninit_AI();
    if (maskdata!=NULL)
        free(maskdata);
    if (maskImg!=NULL)
        free(maskImg);
    printf ("Uninit_AI DETECTER Sucess!\n");
#endif

}
#else

static ArithHandle nTracker;
static int AITRACK_INIT    = 0;
static int AIDETECT_INIT   = 0;
static int G_FRAME_NUM     = 0;
static int G_AI_TRACK_IDX  = 0;
static int G_AI_DETECT_IDX = 0;
static GD_VIDEO_FRAME_S img = { 0 };
static AIT_OUTPUT otRes;
std::vector<objinfo> odRes;
static  ImgMat imgvis;
static  bool   ODIMG_READY = false;
static  bool   OTIMG_READY = false;

void AI_Track_Thrd(){
    while(1) {
        if (AITRACK_INIT) {
            if (G_FRAME_NUM == G_AI_TRACK_IDX)
            {
                //printf ("NnieIDX==MnnIdx:[%d %d] do trackerUpdateNpu\n",G_FRAME_NUM,G_AI_TRACK_IDX);
                gud_rect_f searchBox;
                searchBox.used = 0;
                if (OTIMG_READY){
                    GUD_ALG_AI_TRACK_Process(nTracker, img, searchBox, odRes, &otRes);
                    printf ("#F:[%d], AI_trackbbox:[%d %d %d %d] score:[%f]\n",G_AI_TRACK_IDX,otRes.nX,otRes.nY,otRes.nObjW,otRes.nObjH,otRes.fProb);
                    G_AI_TRACK_IDX++;
                }else{
                    usleep(200);
                }
            } else{
                usleep(200);
                if (G_FRAME_NUM > G_AI_TRACK_IDX){
                    G_AI_TRACK_IDX = G_FRAME_NUM;
                }
            }
        }
        else{
            usleep(200);
        }
    }
}

void AI_Detect_Thrd(){
    while(1) {
        if (AIDETECT_INIT) {
            if (G_FRAME_NUM == G_AI_DETECT_IDX)
            {
                if (ODIMG_READY){
                    int ret1 = runAsync(imgvis, true, false, false);
                    if(ret1){
                        printf("runAsync error\n");
                        //return;
                    }
                    G_AI_DETECT_IDX++;
                } else{
                    usleep(200);
                }
            } else{
                usleep(200);
                if (G_FRAME_NUM > G_AI_DETECT_IDX){
                    G_AI_DETECT_IDX = G_FRAME_NUM;
                }
            }
        }
        else{
            usleep(200);
        }
    }
}

void track_detection_demo_parallel(const char* jsPath, const char *video_path)
{
    gud_nano_config_t nanoConf;
    ot_v2_jsonReader(jsPath,&nanoConf);

    char *img_list = new char[400];
    char *label_txt = new char[400];

    strcpy(img_list, video_path);
    strcat(img_list, "/imglist.txt");
    strcpy(label_txt, video_path);
    strcat(label_txt, "/init.json");
    steady_clock::time_point t1;
    steady_clock::time_point t2;
    duration<double> time_span_;

    string imgTxtPath = img_list;
    std::cout<<"Reading img_list:"<< imgTxtPath << std::endl;
    vector <string> vecImgs = ImgsTxtReader(imgTxtPath);

#if (D_USE_TRACKER_API == 1)
    GUD_ALG_AI_TRACK_Create(&nTracker, &nanoConf);
    printf ("==========AI TRACKER INIT SUCESS=========\n");
#endif
    // AI_Track算法输入部分
    ARIDLL_INPUTPARA stInputPara = { 0 };
    stInputPara.unFrmId++;
    stInputPara.stCameraInfo.fPixelSize = 15;
    stInputPara.stCameraInfo.nFocus = 300;

    // kcf_track init
    // 创建算法句柄
    ArithHandle pTracker = STD_CreatEOArithHandle();
    ARIDLL_OUTPUT pstOutput = { 0 };           //算法结果
    // 算法输出部分
    ARIDLL_OUTPUT stOutput = { 0 };

    // 构建图像类型

    img.enPixelFormat = GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_BGR_PACKED;
    GD_VIDEO_FRAME_S imgY8 = { 0 };


#if (D_USE_DETECTER_API == 1)
    static bool TESTVIS = false;
    static bool TESTIR = false;
    std::string visdetmodeFull = "../models/Detect/1684x/vis_common_headers_1280x1024_bm1684x_F16_false.bmodel";
    std::string visdetmodePart = "../models/Detect/1684x/vis_common_headers_640x512_bm1684x_F16_false.bmodel";

    //std::string irdetmodelname = "../../models/Detect/1684x/vis_common_headers_1280x1024_bm1684x_F16_false.bmodel";
    // 设置队列大小，默认调用即可
    int ret = 0;
    ret = Init_ACL();
    if(ret){
        printf("Init_ACL error\n");
    }
    ret = set_deque_size(5,5,5,5,5,5,5,5,9,9,9,9);
    if(ret){
        printf("set_deque_size error\n");
    }
    printf("deque setted\n");
    // 设置红外图像分辨率
    ret = set_stream_size(IMAGEWIDTH, IMAGEHEIGHT, IR);
    if(ret){
        printf("set_stream_size error\n");
    }
    // 设置可见光图像分辨率
    ret = set_stream_size(IMAGEWIDTH, IMAGEHEIGHT, VIS);
    if(ret){
        printf("set_stream_size error\n");
    }
    //设置类别标签， 标签标请咨询对应的算法工程师
    uint32_t modelclass[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14};
    uint32_t standerclass[] = {10000, 20000, 30000, 40000, 50000,
                               60000, 70000, 80000, 90000, 100000,
                               110000, 120000, 130000, 140000, 150000};
    ret = setLabelMap(modelclass, standerclass, sizeof(modelclass)/sizeof(uint32_t));

    if(ret){
        printf("setLabelMap error with ret=%d\n", ret);
    }
    // 设置数据流格式，NV12 or RGB
    ret = set_StreamMode(AIGO::INPUT_LAYOUT::NV12);
    if(ret){
        printf("set_StreamMode error with ret=%d\n", ret);
    }
    // 初始化模型
    if (DETECT_MODEL_TYPE == 0){
        ret = Init_AI(visdetmodeFull.c_str(), visdetmodeFull.c_str(), NULL, NULL);
    }else{
        ret = Init_AI(visdetmodePart.c_str(), visdetmodePart.c_str(), NULL, NULL);
    }
    if(ret){
        printf("init error\n");
    }
    TESTVIS = true;
    TESTIR = false;

    ret = set_dropout_param(false);
    if(ret){
        printf("set dropout error\n");
    }
    // ret = switch_vis_to_ir();
    printf("get det threshold\n");
    int cNum = sizeof(modelclass)/sizeof(uint32_t);
    for (int z = 0; z < cNum; ++z) {
        ret = set_det_threshold(0.45, standerclass[z]);
    }

    if(ret){
        printf("set det threshold error\n");
    }
    std::vector<objinfo> visdets;
    TenMat * maskImg = (TenMat*)malloc(sizeof(TenMat));
    void * maskdata = (void*)malloc(IMAGEWIDTH*IMAGEHEIGHT*sizeof(int64_t));
    AIDETECT_INIT = 1;
    printf ("==========AI DETECTION INIT SUCESS=========\n");
    std::thread ai_detection(AI_Detect_Thrd);
    printf ("========== AI_Detect_Thrd CREATE SUCESS=========\n");
#endif

#if (D_USE_TRACKER_API == 1)
    std::thread ai_tracking(AI_Track_Thrd);
    printf ("========== AI_Track_Thrd CREATE SUCESS=========\n");
#endif

//#if (D_DEBUG_SAVE_VIDEO == 1)
    VideoWriter writer;
    Size size = cv::Size(1280, 720);
    bool bflag = writer.open("detect_track_parallel_bm1684x.avi", cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), 25, size, true);
    if(bflag){
        printf("Saving Track Video.....\n");
    }
    else{
        printf("open video failed\n");
    }
    
//#endif

    gud_rect_t1 initBox;
    resolve_labeled_json(label_txt, &initBox);
    int x1 = initBox.ul.x;
    int y1 = initBox.ul.y;
    int w1 = initBox.lr.x - x1;
    int h1 = initBox.lr.y - y1;
    cv::Rect trackWindow =cv::Rect(x1,y1,w1,h1);         // 固定值
    cv::Mat show_frame,frame;
    // Initialize tracker with first frame and rect.
    //State state;
    int frn = 0;
    int outTn = 0;
    double avgT = 0;
    float avgTScore = 1.0;
    float ac_score = 0.5;
    float upKerThd = 0.95;
    int dq_size = 10;
    std::deque<float> ot_scores;

    for (int i = 0; i < vecImgs.size(); i++){
        frn = i;
        printf("\n~~~~~~~~~~~~~~~Frame:[%d/%d]~~~~~~~~~~~~~~~~~\n", frn,vecImgs.size());
        const char *imgfile = vecImgs[i].data();

        //printf("reading path %s\n",imgfile);

        frame = cv::imread(imgfile, cv::IMREAD_COLOR);
        int imgW = frame.cols;  // img_w
        int imgH = frame.rows; // img_h
        OTIMG_READY = false;
        img.u32Width = imgW;
        img.u32Height = imgH;
        img.u32Stride[0] = img.u32Width * 3;
        img.u64VirAddr[0] = frame.data;
        OTIMG_READY = true;

        stInputPara.unFrmId = frn;
        stInputPara.stServoInfo.fServoAz = 0.01;
        stInputPara.stServoInfo.fServoPt = 0.01;
        stInputPara.stCameraInfo.fAglReso = 0.003;
        stInputPara.stCameraInfo.fPixelSize = 25;
        stInputPara.stCameraInfo.nFocus = 400;


        cv::Mat srcGray(imgW,imgH,CV_8UC1);
        cv::cvtColor(frame,srcGray,cv::COLOR_RGB2GRAY);
        memcpy(pSrc, srcGray.data,sizeof(unsigned char)*imgW*imgH);

        imgY8.enPixelFormat = GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_GRAY_Y8;
        imgY8.u32Width      = imgW;
        imgY8.u32Height     = imgH;
        imgY8.u32Stride[0]  = img.u32Width * 1;
        imgY8.u64VirAddr[0] = pSrc;

        printf("Reading image %s Sucess IMG_SZIE:(%d %d)!\n", imgfile,imgW,imgH);
        if (frame.empty())
            break;
        G_FRAME_NUM = frn;
#if (D_USE_DETECTER_API == 1)
        ODIMG_READY = false;
        if (frn % D_DO_DETECT_FRQ == 0){

            int UpPx, UpPy;
            cv::Mat resizedImg;
            if (DETECT_MODEL_TYPE==0){
                cv::resize(frame, resizedImg, cv::Size(IMAGEWIDTH, IMAGEHEIGHT));
            }else{
                UpPx = imgW/2-IMAGEWIDTH/2;
                UpPy = imgH/2-IMAGEHEIGHT/2;
                cv::Rect region_rect(UpPx,UpPy, IMAGEWIDTH,IMAGEHEIGHT);
                resizedImg = frame(region_rect).clone();
            }

            cv::Mat NV12Mat;
            BGR2YUV_nv12(resizedImg, NV12Mat);

            imgvis.data = NV12Mat.data;
            imgvis.width = resizedImg.cols;
            imgvis.height = resizedImg.rows;
            imgvis.depth = 1;
            imgvis.layout = HWC;
            imgvis.inlayout = AIGO::INPUT_LAYOUT::NV12;
            imgvis.mode = VIS;
            imgvis.timestamp = i;//(long long)imgnames.at(i).c_str();
            // printf("\nimg.timestamp: %d\n", imgvis.timestamp);
            ODIMG_READY = true;

            long long timestamp = frn;
            if(TESTVIS){
                maskImg->data = maskdata;
                visdets.clear();
                ret = get_result(timestamp, visdets, maskImg, true, VIS);
                printf ("**** F:[%d] AIOD_Detect num:[%d] timestamp: %d\n",frn,visdets.size(),timestamp);
                odRes.clear();
                float ratioW, ratioH;
                if (DETECT_MODEL_TYPE==0){
                    ratioW = (imgW *1.0) / IMAGEWIDTH;
                    ratioH = (imgH *1.0) / IMAGEHEIGHT;}
                else{
                    ratioW = 1.0;
                    ratioH = 1.0;
                }
                if (visdets.size()>0 and ret == 0) {
                    for (int j = 0; j < visdets.size(); ++j) {
                        objinfo reMaptag = visdets[j];
                        std::vector<int> color = get_color(visdets[j].classNum);
                        cv::Rect odres, odRemap;
                        odres.x = visdets[j].x1;
                        odres.y = visdets[j].y1;
                        odres.width = visdets[j].x2 - visdets[j].x1;
                        odres.height = visdets[j].y2 - visdets[j].y1;

                        if (DETECT_MODEL_TYPE==0){
                            odRemap.x = static_cast<int>(odres.x * ratioW);
                            odRemap.y = static_cast<int>(odres.y * ratioH);
                            odRemap.width = static_cast<int>(odres.width * ratioW);
                            odRemap.height = static_cast<int>(odres.height * ratioH);

                            reMaptag.x1 = visdets[j].x1 * ratioW;
                            reMaptag.y1 = visdets[j].y1 * ratioH;
                            reMaptag.x2 = visdets[j].x2 * ratioW;
                            reMaptag.y2 = visdets[j].y2 * ratioH;

                        }
                        else{
                            odRemap.x = static_cast<int>(odres.x + UpPx);
                            odRemap.y = static_cast<int>(odres.y + UpPy);
                            odRemap.width = static_cast<int>(odres.width * ratioW);
                            odRemap.height = static_cast<int>(odres.height * ratioH);

                            reMaptag.x1 = visdets[j].x1 + UpPx;
                            reMaptag.y1 = visdets[j].y1 + UpPy;
                            reMaptag.x2 = visdets[j].x2 + UpPx;
                            reMaptag.y2 = visdets[j].y2 + UpPy;
                        }
                        if (visdets[j].classNum == 60000){
                            cv::putText(resizedImg,to_string(j), cv::Point(odres.x, odres.y-8),
                                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(color[0], color[1], color[2]));
                            cv::rectangle(resizedImg, odres , cv::Scalar(color[0], color[1], color[2]));

                            cv::putText(frame,to_string(j), cv::Point(odRemap.x, odRemap.y-8),
                                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(color[0], color[1], color[2]));
                            cv::rectangle(frame, odRemap , cv::Scalar(color[0], color[1], color[2]));
                        }
                        odRes.push_back(reMaptag);
                        printf ("od_[%d] class:[%d]\n",j,visdets[j].classNum);
                    }
                }
                std::string full_out_img0 = "../results/" + to_string(frn) + "_det.jpg";
                //cv::imwrite(full_out_img0, resizedImg);
            }
        }
#endif

        if (i==0)
        {
            t1 = steady_clock::now();
            gud_rect_f initRect;
            initRect.w = (float)trackWindow.width;
            initRect.h = (float)trackWindow.height;
            initRect.cx = (float)(trackWindow.x + trackWindow.width/2);
            initRect.cy = (float)(trackWindow.y + trackWindow.height/2);
            initRect.used = 1;
            initRect.label = 10001;
            //siam_tracker->init(frame, trackWindow);
#if (D_USE_TRACKER_API == 1)
            GUD_ALG_AI_TRACK_Init(nTracker, img, initRect, odRes, &otRes);
            t2 = steady_clock::now();
            time_span_ = duration_cast < duration < double >> (t2 - t1);
            std::cout << "==========================" << std::endl;
            std::cout << "GUD_ALG_AI_TRACK_Track_Init done!" << std::endl;
            printf("# Nanotracker init time is %f ms.\n",1000 * time_span_.count());
            std::cout << "==========================" << std::endl;
            std::cout << std::endl;
            AITRACK_INIT = 1;
#endif
            for (int j = 0; j < int(dq_size*0.5); ++j)
            {
                ot_scores.push_back(1.0);
            }

            cv::putText(frame,"INIT", cv::Point(trackWindow.x, trackWindow.y-8),
                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
            cv::rectangle(frame, trackWindow , cv::Scalar(0, 0,255));

            // 初始化为凝视-对地模式
            ARIDLL_EOArithInitWithMode(pTracker,imgW,imgH,GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_GRAY_Y8,
                                       GLB_SYS_MODE::GLB_SYS_STARE,GLB_SCEN_MODE::GLB_SCEN_GROUND);

        }else{
            // Start timer
            double t = (double)cv::getTickCount();

            //1仿真下发锁定指令
            if (frn == 3) {
                int SelectX = otRes.nX;
                int SelectY = otRes.nY;
                int setLockBoxW = otRes.nObjW;
                int setLockBoxH = otRes.nObjH;
                ARIDLL_LockCommand(pTracker,SelectX, SelectY, setLockBoxW, setLockBoxH);
            }

            t1 = steady_clock::now();

            cv::TickMeter tm;
            tm.start();
            // 调用TLD流程
            //ARIDLL_RunTLDTracker(pTracker, img);
            ARIDLL_RunController(pTracker, imgY8, stInputPara, &stOutput);
            t2 = steady_clock::now();
            time_span_ = duration_cast < duration < double >> (t2 - t1);

            tm.stop();


            // Calculate Frames per second (FPS)
            double fps = cv::getTickFrequency() / ((double)cv::getTickCount() - t);

            auto trackerOut = stOutput.stTrackers[0];
            cv::Rect outRect;
            outRect.width = MAX(15,(int)trackerOut.nObjW);
            outRect.height= MAX(15, (int)trackerOut.nObjW);
            outRect.x = (int)trackerOut.nX-outRect.width/2;
            outRect.y = (int)trackerOut.nY-outRect.height/2;

            //****************** kcf_tld results **********************************
            int x1 = outRect.x;
            int y1 = outRect.y;
            int w  = outRect.width;
            int h  = outRect.height;
            float score = trackerOut.fConf;
            if (frn>3){
                printf("#F:[%d] KCF_trackbbox:[%d %d %d %d] prob:[%.2f] time:[%.2f] (ms)\n",frn,x1,y1,w,h,score,tm.getTimeMilli());
                // draw kcf_tld track results
                cv::putText(frame,"KCF", cv::Point(outRect.x, outRect.y-8),
                            cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 0, 255));
                cv::rectangle(frame, outRect , cv::Scalar(0, 0,255));
                cv::putText(frame,std::to_string(score), cv::Point(outRect.x+outRect.width, outRect.y+outRect.height),
                            cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 0, 255),1);

            }

            if (1)
            {
                cv::Rect rect;
                rect.width  = otRes.nObjW;
                rect.height = otRes.nObjH;
                rect.x = otRes.nX - otRes.nObjW/2;
                rect.y = otRes.nY - otRes.nObjH/2;
                float tkscore = otRes.fProb;

                cv::putText(frame,"AI", cv::Point(rect.x, rect.y-8),
                            cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0));
                cv::rectangle(frame, rect , cv::Scalar(0, 255,0));
                cv::putText(frame,std::to_string(tkscore), cv::Point(rect.x+rect.width, rect.y+rect.height),
                            cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0),1);


                bool track_status_smooth = UpdateTrackScoreDeque(ot_scores, tkscore, dq_size, ac_score, &avgTScore);
                printf ("#F:[%d],currectScore:[%.4f] smooth-score:[%.4f]\n",frn,tkscore,avgTScore);

            }

            float tS = 1000 * time_span_.count();
            if (tS>40){
                outTn+=1;
                printf ("over time:[%d]\n",outTn);
            }
            avgT += tS*1.0;


            // Display FPS
            std::cout << "FPS: " << fps << std::endl;
            std::cout << std::endl;
            cv::putText(frame,"FPS "+ std::to_string( fps), cv::Point(128, 20),
                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 0, 255),1);

            cv::putText(frame,"Frn: "+ std::to_string( frn), cv::Point(28, 20),
                        cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 0, 255),1);


            if (i >= vecImgs.size())
            {
                break;
            }

        }
#if (D_DEBUG_SAVE_VIDEO == 1)
        cv::resize(frame, show_frame, size);
        writer.write(show_frame);
#endif
    }
    printf ("{%d}/{%d} ,averageTime:%f (ms),rate:[%f]\n",outTn,vecImgs.size(),avgT/frn,outTn*1.0/vecImgs.size());
    writer.release();
#if (D_USE_TRACKER_API == 1)
    GUD_ALG_AI_TRACK_Release(nTracker);
    printf ("GUD_ALG_AI_TRACK_Release Sucess!\n");
#endif

#if (D_USE_DETECTER_API == 1)
    ret = Uninit_AI();
    if (maskdata!=NULL)
        free(maskdata);
    if (maskImg!=NULL)
        free(maskImg);
    printf ("Uninit_AI DETECTER Sucess!\n");
#endif

}
#endif

int main(int argc, char** argv)
{
    if (argc < 3) {
        Usage(argv[0]);
        return -1;
    }

#if (1 == D_CPU_DEBUG)
    cpu_set_t mask;           //CPU核的集合
	cpu_set_t get;            //获取在集合中的CPU

	int num= sysconf(_SC_NPROCESSORS_CONF);
	int j = 0;

	CPU_ZERO(&mask);    /* 初始化set集，将set置为空*/
                      /*将本进程绑定到CPU0上*/
	CPU_SET(3, &mask);
	if (sched_setaffinity(0, sizeof(mask), &mask) == -1)
	{
		printf("Set CPU affinity failue, ERROR:%s\n", strerror(errno));
		return -1;
	}
#endif
    //const char* video_path = "../../datasets/car2";

    switch (*argv[1]) {
        case '0': {
            DETECT_MODEL_TYPE = 0;
            IMAGEWIDTH        = 1280;
            IMAGEHEIGHT       = 1024;
        }
            break;
        case '1': {
            DETECT_MODEL_TYPE = 1;
            IMAGEWIDTH        = 640;
            IMAGEHEIGHT       = 512;
        }
            break;
        default:{
            DETECT_MODEL_TYPE = 1;
            IMAGEWIDTH        = 640;
            IMAGEHEIGHT       = 512;
        }
            break;
    }

    const char* video_path = argv[2];
    std::string JsPath = "../models/BM1684x/gd_ai_nanotrack_v3_240905.json";
#if (D_AI_PARALLEL == 0)
    track_detection_demo_serial(JsPath.c_str(),video_path);
#else
    track_detection_demo_parallel(JsPath.c_str(),video_path);
#endif

    return 0;
}