// 单目标对空跟踪流程测试，关注跟踪器FPS

#include "NeoArithStandardDll.h"
#include "utils.h"
#include <iostream>
#include <memory>
#include <string.h>
#include <algorithm>
#include <thread>
#include "opencv2/opencv.hpp"
using std::cout;
using std::endl;
int main()
{
    // 产生一个仿真Y16数据
	int nWidth = 640;
	int nHeight = 512;
	SimTargetImage_Y16 factory(nWidth, nHeight);
	factory.setBackGround(5000, 10);
    // 叠加一个初始目标
	float moveX = 100;
	float moveY = 100;
	float size = 3;
	float spdx = 1;
	float spdy = 1;
	factory.addTarget(moveX, moveY, size, size, 6500);

    // 创建算法句柄
	ArithHandle pTracker = STD_CreatEOArithHandle();
    // 初始化为凝视-对空模式
	ARIDLL_EOArithInitWithMode(pTracker,nWidth,nHeight,GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_GRAY_Y16,
	GLB_SYS_MODE::GLB_SYS_STARE,GLB_SCEN_MODE::GLB_SCEN_SKY);

    // 算法输入部分
	ARIDLL_INPUTPARA stInputPara = { 0 };
	stInputPara.unFrmId++;
	stInputPara.stCameraInfo.fPixelSize = 15;
	stInputPara.stCameraInfo.nFocus = 300;

    // 算法输出部分
	ARIDLL_OUTPUT stOutput = { 0 };


    // 模拟算法执行流程
	int nTrackSuc = 0;
	for (int i = 0; i < 300; i++)
	{
		stInputPara.unFrmId++;

        // 目标开始运动
		factory.setBackGround(5000, 10);
		moveX += spdx;
		moveY += spdy;


		size = 3 + i/20;
		factory.addTarget((int)moveX, (int)moveY, (int)size, (int)size, 6500);

		// 3个波门内目标
		factory.addTarget((int)moveX - 20, (int)moveY, 3, 3, 6500);
		factory.addTarget((int)moveX, (int)moveY + 20 , 3, 3, 6500);
		factory.addTarget((int)moveX - 20 , (int)moveY +20, 3, 3, 6500);

        // 下发点锁定指令
		if (i == 30)
		{
			ARIDLL_LockCommand(pTracker, (int)moveX, (int)moveY,0,0);
		}

        // 构建图像类型
		GD_VIDEO_FRAME_S img = { 0 };
		img.enPixelFormat = GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_GRAY_Y16;
		img.u32Width = nWidth;
		img.u32Height = nHeight;
		img.u32Stride[0] = img.u32Width * 2;
		img.u64VirAddr[0] = (UBYTE8*)factory.getImageData();

		


		// 红外目标检测API调用
		int targetNum = 0;

		// 目标搜索仅在搜索状态执行，保持与经典对空算法一致，用于与上一版本耗时对比
		// 新跟踪器中搜索在单独线程中不间断执行，本demo中仅展示用法，不做并行示范。
		if (stOutput.nStatus == GLB_STATUS_SEARCH)
		{		
			targetNum = ARIDLL_SearchFrameTargets(pTracker, img);
		}


        // 运行算法主控逻辑API
		ARIDLL_RunController(pTracker, img, stInputPara, &stOutput);


		// 绘制跟踪结果
		cv::Mat src = factory.getMatRGB();

		auto trackerOut = stOutput.stTrackers[0];
		cv::Rect outRect;
		outRect.width = MAX(15,int(trackerOut.nObjW));
		outRect.height= MAX(15,int(trackerOut.nObjH));
		outRect.x = trackerOut.nX-outRect.width/2.0;
		outRect.y = trackerOut.nX-outRect.height/2.0;
		cv::rectangle(src,outRect,cv::Scalar(0,0,255));
		char str[100];
		sprintf(str,"%d-%d,%d,%d",i,int(stOutput.stTrackers[0].nX - moveX),int(stOutput.stTrackers[0].nY - moveY),stOutput.stTrackers->nPipeLostCnt);
		cv::putText(src,cv::String(str),cv::Point(outRect.x - 10,outRect.y),1,2,cv::Scalar(255,255,0));
		//imshow("res",src);
		//cv::waitKey(2);
		
		if ((stOutput.nStatus == GLB_STATUS_TRACK || stOutput.nStatus == GLB_STATUS_MOTRACK) && stOutput.nTrackObjCnts == 1)
		{
			if (abs(stOutput.stTrackers[0].nX - moveX) < 5 &&
				abs(stOutput.stTrackers[0].nY - moveY) < 5)
			{
				nTrackSuc++;
			}
		}

	}

	if (nTrackSuc > 270 * 0.99)
	{
		cout << "pass" << endl;
	}



	return 0;
}