#include <iostream>
#include "API_UnderStitch.h"
#include "API_FrontStitch.h"
#include "S729.h"
#include "PlatformDefine.h"
#include <string.h>
#include "opencv2/opencv.hpp"
#include <random>
using namespace std;
using namespace cv;
class GaussianRandom {
public:
    GaussianRandom(double mean = 0.0, double stddev = 1.0, double amplitude = 1.0)
        : generator(std::random_device{}()),
          distribution(mean, stddev),
          amplitude(amplitude) {}

    double generate() {
        return amplitude * distribution(generator);
    }

private:
    std::mt19937 generator;
    std::normal_distribution<double> distribution;
    double amplitude;
};


void Map16BitTo8Bit(unsigned short *psh16BitData, long lDataLen, BYTE *pby8BitData)
{
    if (psh16BitData == NULL || pby8BitData == NULL || lDataLen <= 0)
    {
        return;
    }

    //指向直方图的数据指针
    int *pHist = new int[65536];
    memset(pHist, 0, 65536 * sizeof(int));

    int i = 0;
    for(i = 0; i < lDataLen; i+=10)
    {
        pHist[psh16BitData[i]]++;
    }

    //设置阈值大小为: AreaSigma*图象大小/100
    int nSigma = 0.02*lDataLen;
    int nSum = 0;
    int nMin = 0;
    int nMax = 0;

    //求映射的最大最小值
    for(i = 0; i < 65536; i++)
    {
        nSum += pHist[i];
        if(nSum >= nSigma)
        {
            nMin = i;
            break;
        }
    }

    nSum = 0;

    for(i = 65535; i >= 0; i--)
    {
        nSum += pHist[i];
        if(nSum >= nSigma)
        {
            nMax = i;
            break;
        }
    }

    //计算对比度亮度
    float K = (float)(120.0/(nMax - nMin + 1));
    float C = (float)(-K * nMin);

    //图像映射
    for (i = 0; i < lDataLen; i++)
    {
        int nValue = (int)(K * psh16BitData[i] + C);
        if (nValue < 0)
        {
            pby8BitData[i] = 0;
        }
        else if (nValue > 255)
        {
            pby8BitData[i] = 255;
        }
        else
        {
            pby8BitData[i] = nValue;
        }
    }

    delete[] pHist;
}


// 红外帧
unsigned short pFrameIR[IMAGE_WIDTH_IR*(IMAGE_HEIGHT_IR + PARA_IR_LINE)] = {0};
unsigned char pImageIR[IMAGE_WIDTH_IR*(IMAGE_HEIGHT_IR)] = {0};


// 电视帧
unsigned char pFrameVL[IMAGE_WIDTH_VL * (IMAGE_HEIGHT_VL + PARA_IR_LINE) * 2] = { 0 };
unsigned char pImageVL[IMAGE_WIDTH_VL * (IMAGE_HEIGHT_VL) * 2] = { 0 };


void ProcessIR(string filePath, string outname)
{
    auto stitcher = API_UnderStitch::Create();

    stitcher->SetOutput(outname, "D:/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");
    GaussianRandom gr(0.0, 1, 0.0);

    int i = 0;

    while (!feof(file))
    {

        fread(pFrameIR, 2, IMAGE_WIDTH_IR * (IMAGE_HEIGHT_IR + PARA_IR_LINE), file);

        S729paras_IR Paras_IR = { 0 };
        memcpy(&Paras_IR, (unsigned char*)(pFrameIR + IMAGE_WIDTH_IR * IMAGE_HEIGHT_IR), sizeof(S729paras_IR));


        FrameInfo info = { 0 };
        info.nFrmID = i;
        info.camInfo.nFocus = Paras_IR.Paras_IR.caminfo.nFocal;
        info.camInfo.fPixelSize = Paras_IR.Paras_IR.caminfo.nPixleSize;

        info.craft.stAtt.fYaw = Paras_IR.Paras_IR.airCraftInfo.fYaw;
        info.craft.stAtt.fPitch = Paras_IR.Paras_IR.airCraftInfo.fPitch;
        info.craft.stAtt.fRoll = Paras_IR.Paras_IR.airCraftInfo.fRoll;

        info.craft.stPos.B = Paras_IR.Paras_IR.airCraftInfo.B;
        info.craft.stPos.L = Paras_IR.Paras_IR.airCraftInfo.L;
        info.craft.stPos.H = Paras_IR.Paras_IR.airCraftInfo.H;


        info.nEvHeight = Paras_IR.Paras_IR.airCraftInfo.H - 25;

        info.servoInfo.fServoAz = Paras_IR.Paras_IR.servoInfo.fAz;
        info.servoInfo.fServoPt = Paras_IR.Paras_IR.servoInfo.fPz + 90;

        info.nWidth = IMAGE_WIDTH_IR;
        info.nHeight = IMAGE_HEIGHT_IR;


        info.craft.stAtt.fYaw += gr.generate();
        info.craft.stAtt.fPitch += gr.generate();
        info.craft.stAtt.fRoll += gr.generate();


        Map16BitTo8Bit(pFrameIR, IMAGE_WIDTH_IR * IMAGE_HEIGHT_IR, pImageIR);

        cv::Mat mat_src(IMAGE_HEIGHT_IR, IMAGE_WIDTH_IR, CV_8UC1, pImageIR);


        frame.enPixelFormat = GD_PIXEL_FORMAT_GRAY_Y8;
        frame.u32Width = IMAGE_WIDTH_IR;
        frame.u32Height = IMAGE_HEIGHT_IR;
        frame.u64VirAddr[0] = pImageIR;





        if (i == 0)
        {
            stitcher->Init(info);

            pan = stitcher->ExportPanAddr();

            mat_pan = cv::Mat(pan.u32Height, pan.u32Width, CV_8UC4, pan.u64VirAddr[0]);
        }
        else
        {
            if (i % 15 != 0)
            {
                i = i + 1;
                continue;
            }
            std::cout << 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
                << std::endl;

            // 基于外参的快拼
           stitcher->Run(frame, info);


           cv::Mat res;
           cv::resize(mat_pan, res, cv::Size(pan.u32Width / 4, pan.u32Height / 4));
           imshow("pan_opt", res);

           waitKey(1);

        }

        i = i + 1;
    }
    stitcher->OptAndOutCurrPan();
    cv::Mat res;
    cv::resize(mat_pan, res, cv::Size(pan.u32Width / 4, pan.u32Height / 4));
    imshow("pan_opt", res);
    waitKey(0);

}

void ProcessVL(string filePath,string outname)
{
    auto stitcher = API_UnderStitch::Create();
    stitcher->SetOutput(outname, "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;
    GaussianRandom gr(0.0, 1, 0.0);

    int i = 0;

    while (!feof(file))
    {
        
        SINT32 nVLFrameSize = 1.5 * IMAGE_WIDTH_VL * IMAGE_HEIGHT_VL + IMAGE_WIDTH_VL * PARA_IR_LINE;
        fread(pFrameVL, 1, nVLFrameSize, file);

        S729paras_VL Paras_VL = { 0 };
        memcpy(&Paras_VL, (unsigned char*)(pFrameVL + int(1.5 * IMAGE_WIDTH_VL * IMAGE_HEIGHT_VL)), sizeof(S729paras_VL));

        FrameInfo info = { 0 };
        info.nFrmID = i;
        info.camInfo.nFocus = Paras_VL.Paras_VL.caminfo.nFocal;
        info.camInfo.fPixelSize = Paras_VL.Paras_VL.caminfo.nPixleSize;

        info.craft.stAtt.fYaw = Paras_VL.Paras_VL.airCraftInfo.fYaw;
        info.craft.stAtt.fPitch = Paras_VL.Paras_VL.airCraftInfo.fPitch;
        info.craft.stAtt.fRoll = Paras_VL.Paras_VL.airCraftInfo.fRoll;

        info.craft.stPos.B = Paras_VL.Paras_VL.airCraftInfo.B;
        info.craft.stPos.L = Paras_VL.Paras_VL.airCraftInfo.L;
        info.craft.stPos.H = Paras_VL.Paras_VL.airCraftInfo.H;


        info.nEvHeight = Paras_VL.Paras_VL.airCraftInfo.H - 25;

        info.servoInfo.fServoAz = Paras_VL.Paras_VL.servoInfo.fAz;
        info.servoInfo.fServoPt = Paras_VL.Paras_VL.servoInfo.fPz + 90;

        info.nWidth = IMAGE_WIDTH_VL;
        info.nHeight = IMAGE_HEIGHT_VL;


        //info.craft.stAtt.fYaw += gr.generate();
        //info.craft.stAtt.fPitch += gr.generate();
        //info.craft.stAtt.fRoll += gr.generate();

        cv::Mat mat_src(IMAGE_HEIGHT_VL * 1.5, IMAGE_WIDTH_VL, CV_8UC1, pFrameVL);
        cv::Mat IMG;
        cv::cvtColor(mat_src, IMG, cv::COLOR_YUV2BGR_NV12);

        frame.enPixelFormat = GD_PIXEL_FORMAT_NV12;
        frame.u32Width = IMAGE_WIDTH_VL;
        frame.u32Height = IMAGE_HEIGHT_VL;
        frame.u64VirAddr[0] = pFrameVL;

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


        if (i == 0 /*|| i == 200*/)
        {
            stitcher->Init(info);

            pan = stitcher->ExportPanAddr();

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

            output.open("D:/output.mp4", VideoWriter::fourcc('H', '2', '6', '4'), 5, Size(pan.u32Width/8, pan.u32Height/8), true);
            if (!output.isOpened()) 
            {
                cout << "打开视频失败" << endl;
                return;
            }
        }
        else
        {
            if (i % 20 != 0)
            {
                i = i + 1;
                continue;
            }
            std::cout << 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
                << std::endl;

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

            tm.stop();

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



        }

        Mat pan_rgb, pan_rgb_ds;
        cv::cvtColor(mat_pan, pan_rgb, cv::COLOR_BGRA2BGR);

        cv::resize(pan_rgb, pan_rgb_ds, cv::Size(pan_rgb.cols / 8, pan_rgb.rows / 8));

        output.write(pan_rgb_ds);

        imshow("pan_opt", pan_rgb_ds);


        waitKey(1);

        i = i + 1;
    }

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

    tm.stop();

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


    Mat pan_rgb, pan_rgb_ds;
    cv::cvtColor(mat_pan, pan_rgb, cv::COLOR_BGRA2BGR);

    cv::resize(pan_rgb, pan_rgb_ds, cv::Size(pan_rgb.cols / 8, pan_rgb.rows / 8));

    for (int i = 0; i < 25; i++)
    {
        cv::putText(pan_rgb_ds, "BA opt", cv::Point(100, 40), 0, 1, cv::Scalar(255, 0, 0));
        output.write(pan_rgb_ds);
    }


    waitKey(1);


    output.release();
}


// 处理前视可见光扫描
void ProcessFrontVL(string filePath)
{
    auto stitcher = API_FrontStitch::Create();

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

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

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


    GaussianRandom gr(0.0, 1, 0.0);

    SINT32 nVLFrameSize = 1.5 * IMAGE_WIDTH_VL * IMAGE_HEIGHT_VL + IMAGE_WIDTH_VL * PARA_IR_LINE;

    cv::VideoWriter output;

    int i = 0;
    while (!feof(file))
    {
        fread(pFrameVL, 1, nVLFrameSize, file);

        S729paras_VL Paras_VL = { 0 };
        memcpy(&Paras_VL, (unsigned char*)(pFrameVL + int(1.5 * IMAGE_WIDTH_VL * IMAGE_HEIGHT_VL)), sizeof(S729paras_VL));

        FrameInfo info = { 0 };
        info.nFrmID = i;
        info.camInfo.nFocus = Paras_VL.Paras_VL.caminfo.nFocal;
        info.camInfo.fPixelSize = Paras_VL.Paras_VL.caminfo.nPixleSize;

        info.craft.stAtt.fYaw = Paras_VL.Paras_VL.airCraftInfo.fYaw;
        info.craft.stAtt.fPitch = Paras_VL.Paras_VL.airCraftInfo.fPitch;
        info.craft.stAtt.fRoll = Paras_VL.Paras_VL.airCraftInfo.fRoll;

        info.craft.stPos.B = Paras_VL.Paras_VL.airCraftInfo.B;
        info.craft.stPos.L = Paras_VL.Paras_VL.airCraftInfo.L;
        info.craft.stPos.H = Paras_VL.Paras_VL.airCraftInfo.H;


        info.nEvHeight = Paras_VL.Paras_VL.airCraftInfo.H - 25;

        info.servoInfo.fServoAz = Paras_VL.Paras_VL.servoInfo.fAz;
        info.servoInfo.fServoPt = Paras_VL.Paras_VL.servoInfo.fPz + 90;

        info.nWidth = IMAGE_WIDTH_VL;
        info.nHeight = IMAGE_HEIGHT_VL;


        info.craft.stAtt.fYaw += gr.generate();
        info.craft.stAtt.fPitch += gr.generate();
        info.craft.stAtt.fRoll += gr.generate();


        cv::Mat mat_src(IMAGE_HEIGHT_VL * 1.5, IMAGE_WIDTH_VL, CV_8UC1, pFrameVL);
        cv::Mat IMG;
        cv::cvtColor(mat_src, IMG, cv::COLOR_YUV2BGR_NV12);
        imwrite("D:/imgVL_f.jpg", IMG);
        //imshow("src", IMG);
        //waitKey(1);

        frame.enPixelFormat = GD_PIXEL_FORMAT_NV12;
        frame.u32Width = IMAGE_WIDTH_VL;
        frame.u32Height = IMAGE_HEIGHT_VL;
        frame.u64VirAddr[0] = pFrameVL;

        if (i == 0)
        {
            stitcher->Init(info, ScanRange{-10,100}, ScanRange{ 56,90 });

            pan = stitcher->ExportPanAddr();

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

            
            //output.open("F:/output.mp4", VideoWriter::fourcc('H', '2', '6', '4'), 5, Size(pan.u32Width/4, pan.u32Height/4), true);
            //if (!output.isOpened()) 
            //{
            //    cout << "打开视频失败" << endl;
            //    return;
            //}
        }
        else
        {
            if (i % 20 != 0)
            {
                i = i + 1;
                continue;
            }

            std::cout << 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
                << std::endl;

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


            tm.stop();

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

        }


        cv::Mat res;
        cv::resize(mat_pan, res, cv::Size(pan.u32Width / 4, pan.u32Height / 4));
        imshow("pan_opt", res);

        //output << res;



        waitKey(1);
        i = i + 1;
    }

    stitcher->OptAndOutCurrPan();

    cv::Mat res;
    cv::resize(mat_pan, res, cv::Size(pan.u32Width / 4, pan.u32Height / 4));
    imshow("pan_opt", res);

    //output.release();

    waitKey(0);


}


int main(int, char**)
{
    //ProcessIR("F:/S729/22.xraw", "22");
    //ProcessVL("Z:/729dataset/raw/vl_1920_1080_para40_y8/20241219153557_11.video", "20241219152643_1");
    ProcessVL("F:/S729/20241219152917_4.video", "20241219152917_4");
    //ProcessVL("F:/S729/20241219153515_10.video", "20241219153515_10");
    //
    //ProcessVL("F:/S729/20241219153557_11.video", "20241219153557_11");

    //ProcessVL()

    //ProcessFrontVL("F:/S729/1.video");
}