#include "S732.h"
#include <iostream>
#include "API_UnderStitch.h"
#include "PlatformDefine.h"
#include <string.h>
#include "opencv2/opencv.hpp"
#include <random>
#include <string>


using namespace cv;
using namespace std;

void ProcessVL(string filePath)
{
    auto stitcher = API_UnderStitch::Create("D:/cache");
    stitcher->SetOutput("BT", "google_tiles");


    GD_VIDEO_FRAME_S frame = { 0 };//输入帧
    GD_VIDEO_FRAME_S pan = { 0 };//输出全景

    cv::Mat mat_pan;//全景显示

    FILE* file = fopen(filePath.c_str(), "rb");

    cv::VideoWriter output;


    int i = 0;


    SINT32 nVLFrameSize = 1.5 * (S732_VL_W * S732_VL_H + S732_VL_W * PARA_VL_LINE_S732);
    unsigned char* pFrameVL = new unsigned char[nVLFrameSize];


    bool stitchInit = false;

    std::deque<FrameInfo> infoBuffer;
    const int delayFrames = 5; // 假设延迟3帧

    std::deque<cv::Mat> MatBuffer;
    const int matdelayFrames = 2; // 假设延迟3帧

    fseek(file, nVLFrameSize * 80, SEEK_SET);

    while (!feof(file))
    {
        fread(pFrameVL, 1, nVLFrameSize, file);

        STD_DTArith_Record732 Paras_VL = { 0 };
        memcpy(&Paras_VL, (unsigned char*)(pFrameVL + int(1.5 * S732_VL_W * S732_VL_H)), sizeof(STD_DTArith_Record732));

        FrameInfo info_rev = { 0 };
        info_rev.nFrmID = i;

        info_rev.camInfo.fPixelSize = Paras_VL.trackInputPara.stCameraInfo.fPixelSize;

        info_rev.camInfo.nFocus = Paras_VL.trackInputPara.stCameraInfo.nFocus;

        info_rev.craft.stAtt.fYaw = Paras_VL.trackInputPara.stAirCraftInfo.stAtt.fYaw;
        info_rev.craft.stAtt.fPitch = Paras_VL.trackInputPara.stAirCraftInfo.stAtt.fPitch;
        info_rev.craft.stAtt.fRoll = Paras_VL.trackInputPara.stAirCraftInfo.stAtt.fRoll;

        info_rev.craft.stPos.B = Paras_VL.trackInputPara.stAirCraftInfo.stPos.B;
        info_rev.craft.stPos.L = Paras_VL.trackInputPara.stAirCraftInfo.stPos.L;
        info_rev.craft.stPos.H = Paras_VL.trackInputPara.stAirCraftInfo.stPos.H;

        info_rev.nEvHeight = info_rev.craft.stPos.H - 1360;

        info_rev.servoInfo.fServoAz = Paras_VL.trackInputPara.stServoInfo.fServoAz;
        info_rev.servoInfo.fServoPt = Paras_VL.trackInputPara.stServoInfo.fServoPt;

        info_rev.nWidth = S732_VL_W;
        info_rev.nHeight = S732_VL_H;

        FrameInfo info = info_rev;



        cv::Mat mat_src(S732_VL_H * 1.5, S732_VL_W, CV_8UC1, pFrameVL);
        cv::Mat IMG;
        cv::cvtColor(mat_src, IMG, cv::COLOR_YUV2GRAY_NV21);
        

        cv::Mat f;
        IMG.convertTo(f, CV_32F, 1.0 / 255.0);
        cv::pow(f, 0.8, f);    // gamma < 1 => 变亮
        cv::Mat bright;
        f.convertTo(bright, CV_8U, 255.0);

        cv::cvtColor(bright, bright, cv::COLOR_GRAY2BGR);


        cv::namedWindow("IMG_show", cv::WINDOW_NORMAL);
        imshow("IMG_show", IMG);



        MatBuffer.push_back(bright.clone());

        if (MatBuffer.size() < matdelayFrames)
        {
            continue;
        }

        cv::Mat matuse = MatBuffer.front();

        MatBuffer.pop_front();


        frame.enPixelFormat = GD_PIXEL_FORMAT_RGB_PACKED;
        frame.u32Width = S732_VL_W;
        frame.u32Height = S732_VL_H;
        frame.u64VirAddr[0] = matuse.data;

        //imwrite("D:/imgVL_u.jpg", IMG);


        if (!stitchInit)
        {
            stitchInit = true;
            stitcher->Init(info);

            UPanConfig cfg = { 0 };
            cfg.bOutGoogleTile = 1;
            cfg.bOutFrameTile = 0;
            cfg.bUseBA = 1;
            stitcher->SetConfig(cfg);

            stitcher->SetOutput("baotou","F:/google_tiles");


            pan = stitcher->ExportPanAddr();

            mat_pan = cv::Mat(pan.u32Height, pan.u32Width, CV_8UC4, pan.u64VirAddr[0]);

            output.open("output.mp4", VideoWriter::fourcc('M', 'P', '4', 'V'), 50, Size(pan.u32Width / 8, pan.u32Height / 8), true);
            if (!output.isOpened())
            {
                cout << "打开视频失败" << endl;
                return;
            }
        }
        else
        {
            std::cout << info.nFrmID << "   " <<info.craft.stPos.B << " " << info.craft.stPos.L << " " << info.craft.stPos.H << " "
                << info.craft.stAtt.fYaw << " " << info.craft.stAtt.fPitch << " " << info.craft.stAtt.fRoll << " "
                << info.servoInfo.fServoAz << " " << info.servoInfo.fServoPt + 90
                << std::endl;

            cv::TickMeter tm;
            tm.start();
            // 基于外参的快拼
            stitcher->Run(frame, info);

            tm.stop();

            cout << "time:" << tm.getTimeMilli() << endl;
        }

        Mat pan_ds; 
        Mat pan_rgb_ds(cv::Size(mat_pan.cols / 8, mat_pan.rows / 8),CV_8UC3);
        cv::resize(mat_pan, pan_ds, cv::Size(mat_pan.cols / 8, mat_pan.rows / 8));

        cv::cvtColor(pan_ds, pan_rgb_ds, cv::COLOR_BGRA2BGR);



        output.write(pan_rgb_ds);

        cv::namedWindow("pan_opt",0);
        imshow("pan_opt", pan_rgb_ds);


        if (cv::waitKey(1) == 27)
        {
            break;
        }

        i = i + 1;


    }

    //cv::TickMeter tm;
    //tm.start();
    //// 处理帧
    //stitcher->OptAndOutCurrPan();

    //tm.stop();

    //cout << "time opt:" << tm.getTimeMilli() << endl;





    waitKey(1);


    output.release();
}


int main()
{
    ProcessVL("F:/包头/20251101/20251101_091550742_2.yuv");
	return 0;
}