#include <iostream>
#include "API_VideoStitch.h"
#include "S729.h"
#include "PlatformDefine.h"
#include <string.h>
#include "opencv2/opencv.hpp"
using namespace std;
using namespace cv;

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

int main(int, char**)
{
    auto stitcher = API_VideoStitch::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;//全景显示
    //return 0;
    FILE* file = fopen("../20241219152917_4.xraw","rb");

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

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

        if(i % 2 != 0)
        {
            continue;
        }

        S729paras_IR Paras_IR = { 0 };
	    memcpy(&Paras_IR, (unsigned char*)(pFrameIR + IMAGE_WIDTH_IR*IMAGE_HEIGHT_IR), sizeof(S729paras_IR));
        
        FrameInfo info = {0};
        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;

        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;

        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_8UC1, pan.u64VirAddr[0]);
        }
        else
        {
            //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
            //<< endl;


            stitcher->Updata(frame,info);


        }
        //imshow("src", mat_src);
        imshow("pan", mat_pan);
        waitKey(1);

    }
    

    waitKey(0);

}