// 单目标对地跟踪流程测试:将TLD从算法中剥离到外部，导致API调用形式调整int argc,char *argv[]
// 读取avi视频进行测试


#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;
using std::string;

using namespace cv;

void convertRGBToNV12(const cv::Mat& rgb, cv::Mat& yuv) {
    if (rgb.empty()) {
        std::cerr << "Input image is empty." << std::endl;
        return;
    }

    // 将RGB转换为YUV格式（YUV444）
    cv::Mat yuv444;
    cv::cvtColor(rgb, yuv444, cv::COLOR_BGR2YUV);

    // 准备Y和UV平面
    int width = rgb.cols;
    int height = rgb.rows;

    // NV12格式需要的尺寸
    yuv.create(height + height / 2, width, CV_8UC1);

    // 提取Y通道
    for (int i = 0; i < height; ++i) {
        for (int j = 0; j < width; ++j) {
            yuv.at<uchar>(i, j) = yuv444.at<cv::Vec3b>(i, j)[0];
        }
    }

    // 提取UV平面（交替存储）
    for (int i = 0; i < height / 2; ++i) {
        for (int j = 0; j < width / 2; ++j) {
            int u = 0, v = 0;
            for (int k = 0; k < 2; ++k) {
                for (int l = 0; l < 2; ++l) {
                    u += yuv444.at<cv::Vec3b>(2 * i + k, 2 * j + l)[1];
                    v += yuv444.at<cv::Vec3b>(2 * i + k, 2 * j + l)[2];
                }
            }
            // 平均化UV值并存储
            yuv.at<uchar>(height + i, 2 * j) = static_cast<uchar>(u / 4);
            yuv.at<uchar>(height + i, 2 * j + 1) = static_cast<uchar>(v / 4);
        }
    }
}

int main(int argc,char *argv[])
{
	cv::VideoCapture capture(argv[1]);
	printf("argv[0]:%s\n",argv[1]);
	int nWidth = int(capture.get(cv::CAP_PROP_FRAME_WIDTH));
	int nHeight = int(capture.get(cv::CAP_PROP_FRAME_HEIGHT));

    // 创建算法句柄
	ArithHandle pTracker = STD_CreatEOArithHandle();

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

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

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

    


    // 模拟算法执行流程
	int nTrackSuc = 0;
	cv::Mat frame;
    cv::Mat framenv12;
	while(1)
	{
		stInputPara.unFrmId++;

		capture.read(frame);

        convertRGBToNV12(frame,framenv12);


        // cv::Mat bgr;
        // cv::cvtColor(framenv12, bgr, cv::COLOR_YUV2BGR_NV12);

        // imshow("bgr",bgr);
        // waitKey(1);

        if(frame.empty())
            break;

        // 下发面锁定指令
		if (stInputPara.unFrmId == 10)
		{
			ARIDLL_LockCommand(pTracker, 228*2,279*2,40,25);
		}

        // 构建图像类型
		GD_VIDEO_FRAME_S img = { 0 };
		img.enPixelFormat = GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_NV12;
		img.u32Width = nWidth;
		img.u32Height = nHeight;
		img.u32Stride[0] = img.u32Width;
		img.u64VirAddr[0] = framenv12.data;


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


        cv::Mat bgr;
        cv::cvtColor(framenv12, bgr, cv::COLOR_YUV2BGR_NV12);

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

        printf("%f,%f,%f,%f\n",trackerOut.nX,trackerOut.nY,trackerOut.nObjW,trackerOut.nObjH);

		//cv::rectangle(bgr,outRect,cv::Scalar(0,0,255));


		//cv::resize(bgr,bgr,cv::Size(nWidth/2,nHeight/2));
		//imshow("res",bgr);
		//cv::waitKey(2);

	}

	if(fabs(stOutput.stTrackers[0].nX - 1508) < 20 && 
	fabs(stOutput.stTrackers[0].nY - 750) < 20)
	{
		cout << "pass" << endl;
	}
	

	return 0;
}