#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()
{
    auto stitcher = API_UnderStitch::Create(IMAGE_WIDTH_IR, IMAGE_HEIGHT_IR);
    //stitcher->Test();

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

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


    FILE* file = fopen("F:/S729/20241219152917_4.xraw", "rb");
    GaussianRandom gr(0.0, 1, 0.0);


    for (size_t i = 0; i < 600; i++)
    {

        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 % 30 != 0)
            {
                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->GeoStitch(frame, info);

            // 接收帧
           auto a = stitcher->ReceiveFrame(frame, info);

        }

        imshow("pan", mat_pan);
        waitKey(1);

    }

    // 处理帧
    stitcher->ProcessFrame();

    imshow("pan_opt", mat_pan);
    waitKey(0);

}

void ProcessVL(string filePath,string outname)
{
    auto stitcher = API_UnderStitch::Create(IMAGE_WIDTH_VL, IMAGE_HEIGHT_VL);

    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))
    {
        
        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;

        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 % 50 != 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->GeoStitch(frame, info);

            tm.stop();

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

            // 接收帧
            auto a = stitcher->ReceiveFrame(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;
    }

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

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


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

    waitKey(1);


    // 输出谷歌png
    stitcher->ExportGeoPng("D:/google_tiles", outname);

    // 输出谷歌tile
    //stitcher->ExportGoogleTile("D:/google_tiles", outname);
}


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

    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;

    _fseeki64(file, nVLFrameSize * 2200, SEEK_SET);

    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);

        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,180,180);

            pan = stitcher->ExportPanAddr();

            mat_pan = cv::Mat(pan.u32Height, pan.u32Width, CV_8UC3, pan.u64VirAddr[0]);
        }
        else
        {
            if (i % 30 != 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->GeoStitch(frame, info);

            tm.stop();

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


        }

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

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


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

    waitKey(0);


}


int main(int, char**)
{
    //ProcessIR();
    //ProcessVL("H:/vl_1920_1080_para40_y8/22.video","22");
    //ProcessVL("H:/vl_1920_1080_para40_y8/20241219152643_1.video", "20241219152643_1");
    //ProcessVL("H:/vl_1920_1080_para40_y8/20241219152917_4.video", "20241219152917_4");
    //ProcessVL("H:/vl_1920_1080_para40_y8/20241219153515_10.video", "20241219153515_10");
    //
    //ProcessVL("H:/vl_1920_1080_para40_y8/1.video", "1");

    ProcessFrontVL("H:/vl_1920_1080_para40_y8/1.video");
}