#include <iostream>
#include <fstream>
#include "S729.h"
#include "S732.h"
#include <string.h>
#include "opencv2/opencv.hpp"
#include <random>
//#include <direct.h>

//typedef unsigned char BYTE;
//typedef unsigned int SINT32;

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

unsigned short pFrameIR_732[S732_IR_W * (S732_IR_H + PARA_IR_LINE_S732)] = { 0 };
unsigned char pImageIR_732[S732_IR_W * (S732_IR_H)] = { 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 };

unsigned short pFrameVL_732[S732_VL_W * (S732_VL_H + PARA_VL_LINE_S732)] = { 0 };
unsigned char pImageVL_732[S732_VL_W * (S732_VL_H)] = { 0 };

/*
 * 将Y16数据转换成Y8数据
 */
void	Map16BitTo8Bit_u(unsigned short* psh16BitData, int nDataLen, unsigned char* pby8BitData, int nBrightness, int nContrast, float lowP, float highP)
{
    //int nBrightness = 100;
    //int nContrast = 100;

    if (pby8BitData == NULL || psh16BitData == NULL || nDataLen <= 0)
    {
        return;
    }

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

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

    //设置阈值大小为: AreaSigma*图象大小/100
    int nSigmaLow = (int)(lowP * nDataLen);    // 大动态范围
    int nSigmaHigh = (int)(highP * nDataLen);    // 大动态范围
    int nSum = 0;
    int nMin = 0;
    int nMax = 0;

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

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

    nBrightness = (nBrightness > 100) ? 100 : nBrightness;
    nBrightness = (nBrightness < 0) ? 0 : nBrightness;

    nContrast = (nContrast > 100) ? 100 : nContrast;
    nContrast = (nContrast < 0) ? 0 : nContrast;

    //计算对比度亮度
    float K = (float)(256.0 / (nMax - nMin + 1)) * float(2 * nContrast / 100.0);

    if (K > 1.0)
    {
        K = 1.0;
    }

    float C = (float)(128 * float(2 * nBrightness / 100.0) - K * (nMax + nMin) / 2);


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



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;
}
// 转换S729 raw到jpg和txt
void convertS729_IR(string inputFile, string outputPath) 
{
    std::cout << "Converting S729...\n";

    FILE* file = fopen(inputFile.c_str(), "rb");
    if (!file) {
        std::cerr << "Error opening file: " << inputFile << std::endl;
        return;
    }

    // 打开txt文件用于存储文件名以及对应的参数
    std::ofstream txtFile(outputPath + "list.txt");


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


        Map16BitTo8Bit(pFrameIR, IMAGE_WIDTH_IR * IMAGE_HEIGHT_IR, pImageIR);

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

        // 保存jpg图像
        std::string jpgFile = outputPath + "/" + "frame_" + std::to_string(i) + ".jpg";
        cv::imwrite(jpgFile, mat_src);

        // 写入txt文件
        txtFile << std::fixed << std::setprecision(6);
        txtFile << "Frame " << i << "," 
        << Paras_IR.Paras_IR.airCraftInfo.B<< ","
        <<Paras_IR.Paras_IR.airCraftInfo.L<< ","
        <<Paras_IR.Paras_IR.airCraftInfo.H<< ","
        <<Paras_IR.Paras_IR.airCraftInfo.H<< ","
        <<Paras_IR.Paras_IR.servoInfo.fAz<< ","
        <<Paras_IR.Paras_IR.servoInfo.fPz<< ","
        <<Paras_IR.Paras_IR.caminfo.nFocal<< ","
        <<Paras_IR.Paras_IR.caminfo.nPixleSize
        << "\n";

        i++;
    }

    // 关闭文件
    fclose(file);
    txtFile.close();
    std::cout << "Conversion completed. Output saved to " << outputPath << "list.txt\n";


    // Conversion logic for S729 goes here
}


void convertS729_VL(string inputFile, string outputPath) 
{
    std::cout << "convertS729_VL\n";

    FILE* file = fopen(inputFile.c_str(), "rb");
    if (!file) {
        std::cerr << "Error opening file: " << inputFile << std::endl;
        return;
    }

    // 打开txt文件用于存储文件名以及对应的参数
    std::ofstream txtFile(outputPath + "/list.txt");

    int i = 0;
    while (!feof(file))
    {
        printf("%d\n",i);
        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));

        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("",IMG);
        //waitKey(1);

        // 保存jpg图像
        std::string jpgFile = outputPath + "/" + "frame_" + std::to_string(i) + ".jpg";
        cv::imwrite(jpgFile, IMG);

        // 写入txt文件
        txtFile << std::fixed << std::setprecision(6);
        txtFile << "Frame " << i << "," 
        << Paras_VL.Paras_VL.airCraftInfo.B<< ","
        <<Paras_VL.Paras_VL.airCraftInfo.L<< ","
        <<Paras_VL.Paras_VL.airCraftInfo.H<< ","
        <<Paras_VL.Paras_VL.airCraftInfo.H<< ","
        <<Paras_VL.Paras_VL.airCraftInfo.fYaw<< ","
        <<Paras_VL.Paras_VL.airCraftInfo.fPitch<< ","
        <<Paras_VL.Paras_VL.airCraftInfo.fRoll<< ","
        <<Paras_VL.Paras_VL.servoInfo.fAz<< ","
        <<Paras_VL.Paras_VL.servoInfo.fPz<< ","
        <<Paras_VL.Paras_VL.caminfo.nFocal<< ","
        <<Paras_VL.Paras_VL.caminfo.nPixleSize
        << "\n";

        i++;
    }

    // 关闭文件
    fclose(file);
    txtFile.close();
    std::cout << "Conversion completed. Output saved to " << outputPath << "list.txt\n";

}

// 解析DJI SRT文件
struct FrameData 
{
    int frameNumber;
    std::string timeRange;
    int frameCnt;
    std::string timestamp;
    float focal_len = 0;
    float dzoom_ratio = 0;
    double latitude = 0;
    double longitude = 0;
    float rel_alt = 0;
    float abs_alt = 0;
    float gb_yaw = 0;
    float gb_pitch = 0;
    float gb_roll = 0;

    // 衍生参数
    float real_focal_mm = 0;
    float pixel_size_um = 0;
};

float extractValueAfter(const std::string& line, const std::string& key)
{
    size_t pos = line.find(key);
    if (pos == std::string::npos) return 0;
    
    pos += key.length();
    size_t end = line.find(' ', pos);
    if (end == std::string::npos) end = line.length();
    
    return std::stof(line.substr(pos, end - pos));
}

double extractDoubleAfter(const std::string& line, const std::string& key) 
{
    size_t pos = line.find(key);
    if (pos == std::string::npos) return 0;
    pos += key.length();
    size_t end = line.find(' ', pos);
    if (end == std::string::npos) end = line.length();
    return std::stod(line.substr(pos, end - pos));
}

// 获取真实焦距和像元尺寸
void inferCameraParams_H20(FrameData& frame, const std::string& filename) 
{
    const float full_frame_diag = 43.3f;
    float sensor_diag = 0.0f;

    if (filename.find("_W") != std::string::npos) {
        sensor_diag = 9.4f;
        frame.pixel_size_um = 1.98f;
    }
    else if (filename.find("_Z") != std::string::npos) {
        sensor_diag = 7.7f;
        frame.pixel_size_um = 2.4f;
    }
    else if (filename.find("_T") != std::string::npos) {
        sensor_diag = 10.9f;
        frame.pixel_size_um = 12.0f;
    }
    else {
        sensor_diag = 9.4f;
        frame.pixel_size_um = 1.98f;
    }

    frame.real_focal_mm = frame.focal_len * (sensor_diag / full_frame_diag);
}
// 获取真实焦距和像元尺寸
void inferCameraParams_H30(FrameData& frame, const std::string& filename) 
{
    const float full_frame_diag = 43.3f;
    float sensor_diag = 0.0f;

    if (filename.find("_W") != std::string::npos) {
    frame.real_focal_mm = 6.72;
    frame.pixel_size_um = 2.4;
    }
    else if (filename.find("_Z") != std::string::npos) {

    }
    else if (filename.find("_T") != std::string::npos) {
        frame.real_focal_mm = 24;
        frame.pixel_size_um = 12;
    }
    else {

    }

    //frame.real_focal_mm = frame.focal_len * (sensor_diag / full_frame_diag);
}
std::vector<FrameData> parseDJISRT(const std::string& filename) {
    std::vector<FrameData> frames;
    std::ifstream file(filename);
    
    if (!file.is_open()) {
        std::cerr << "Error: Could not open file " << filename << std::endl;
        return frames;
    }

    std::string lines[5];
    while (std::getline(file, lines[0])) {  // Frame number line
        // Read next 4 lines
        for (int i = 1; i < 5; ++i) {
            if (!std::getline(file, lines[i])) break;
        }

        FrameData frame;
        
        // Line 1: Frame number
        frame.frameNumber = std::stoi(lines[0]);
        
        // Line 2: Time range (00:00:00,000 --> 00:00:00,033)
        frame.timeRange = lines[1];
        
        // Line 3: FrameCnt and timestamp
        std::istringstream iss(lines[2]);
        std::string temp;
        iss >> temp >> frame.frameCnt >> frame.timestamp;
        
        // Line 4: Metadata (direct field extraction)
        const std::string& meta = lines[3];
        frame.focal_len = extractValueAfter(meta, "focal_len: ");
        frame.dzoom_ratio = extractValueAfter(meta, "dzoom_ratio: ");
        frame.latitude = extractDoubleAfter(meta, "latitude: ");
        frame.longitude = extractDoubleAfter(meta, "longitude: ");
        frame.rel_alt = extractValueAfter(meta, "rel_alt: ");
        frame.abs_alt = extractValueAfter(meta, "abs_alt: ");
        frame.gb_yaw = extractValueAfter(meta, "gb_yaw: ");
        frame.gb_pitch = extractValueAfter(meta, "gb_pitch: ");
        frame.gb_roll = extractValueAfter(meta, "gb_roll: ");

        inferCameraParams_H30(frame, filename);

        frames.push_back(frame);
        
        // Skip the empty line (line 5)
    }
    
    file.close();
    return frames;
}

void convertDJ(string videoPath, string outputPath)
{
    cv::VideoCapture cap(videoPath);

    // Get video properties
    double fps = cap.get(cv::CAP_PROP_FPS);
    int width = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_WIDTH));
    int height = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_HEIGHT));
    int frame_count = static_cast<int>(cap.get(cv::CAP_PROP_FRAME_COUNT));

    // 解析srt文件
    string srtPath = videoPath.substr(0, videoPath.find_last_of('.')) + ".srt";
    std::vector<FrameData> srt = parseDJISRT(srtPath);

    printf("解析srt文件 done!\n");

    // 打开txt文件用于存储文件名以及对应的参数
    std::ofstream txtFile(outputPath + "/list.txt");


    int frmID = 0;
    while (true)
    {

        printf("%d\n",frmID);
        cv::Mat mat;

        // Read a new frame
        cap >> mat;



        // Check if frame is empty (end of video)
        if (mat.empty()) 
        {
            std::cout << "End of video\n";
            cap.release();
            break;
        }

        frmID++;

        if(frmID%25 !=0)
        {
            continue;
        }


        // 保存jpg图像
        std::string jpgFile = outputPath + "/" + "frame_" + std::to_string(frmID) + ".jpg";
        cv::imwrite(jpgFile, mat);

        // 写入txt文件
        txtFile << std::fixed << std::setprecision(6);
        txtFile << "Frame " << frmID << "," 
        << srt[frmID].latitude<< ","
        <<srt[frmID].longitude<< ","
        <<srt[frmID].abs_alt<< ","
        <<srt[frmID].rel_alt<< ","
        <<srt[frmID].gb_yaw<< ","
        <<srt[frmID].gb_pitch + 90<< "," 
        <<srt[frmID].gb_roll<< ","
        <<0<< ","
        <<-90<< ","
        <<srt[frmID].real_focal_mm<< ","
        <<srt[frmID].pixel_size_um
        << "\n";

    }

}


void convertS732_IR(string videoPath, string outputPath)
{
    std::cout << "convertS732_IR\n";

    FILE* file = fopen(videoPath.c_str(), "rb");
    if (!file) {
        std::cerr << "Error opening file: " << videoPath << std::endl;
        return;
    }

    // 打开txt文件用于存储文件名以及对应的参数
    std::ofstream txtFile(outputPath + "/list.txt");

    int i = 0;
    while (!feof(file))
    {
        printf("%d\n", i);

        fread(pFrameIR_732, 2, S732_IR_W * (S732_IR_H + PARA_IR_LINE_S732), file);


        STD_DTArith_Record732 Paras_ir_s732 = { 0 };
        memcpy(&Paras_ir_s732, (unsigned char*)(pFrameIR_732 + S732_IR_W * S732_IR_H), sizeof(STD_DTArith_Record732));


        Map16BitTo8Bit_u(pFrameIR_732, S732_IR_W * S732_IR_H, pImageIR,40,40,0.02, 0.02);
        cv::Mat mat_src(S732_IR_H, S732_IR_W, CV_8UC1, pImageIR);


        imshow("",mat_src);
        waitKey(1);

        // 保存jpg图像
        std::string jpgFile = outputPath + "/" + "frame_" + std::to_string(i) + ".jpg";
        cv::imwrite(jpgFile, mat_src);

        // 写入txt文件
        txtFile << std::fixed << std::setprecision(6);
        txtFile << "Frame " << i << ","
            << Paras_ir_s732.trackInputPara.stAirCraftInfo.stPos.B << ","
            << Paras_ir_s732.trackInputPara.stAirCraftInfo.stPos.L << ","
            << Paras_ir_s732.trackInputPara.stAirCraftInfo.stPos.H << ","
            << Paras_ir_s732.trackInputPara.stAirCraftInfo.stPos.H << ","
            << Paras_ir_s732.trackInputPara.stAirCraftInfo.stAtt.fYaw << ","
            << Paras_ir_s732.trackInputPara.stAirCraftInfo.stAtt.fPitch << ","
            << Paras_ir_s732.trackInputPara.stAirCraftInfo.stAtt.fRoll << ","
            << Paras_ir_s732.trackInputPara.stServoInfo.fServoAz << ","
            << Paras_ir_s732.trackInputPara.stServoInfo.fServoPt << ","
            << Paras_ir_s732.trackInputPara.stCameraInfo.nFocus << ","
            << Paras_ir_s732.trackInputPara.stCameraInfo.fPixelSize
            << "\n";

        i++;
    }

    // 关闭文件
    fclose(file);
    txtFile.close();
    std::cout << "Conversion completed. Output saved to " << outputPath << "list.txt\n";
}



void convertS732_VL(string inputFile, string outputPath)
{
    std::cout << "convertS732_VL\n";

    FILE* file = fopen(inputFile.c_str(), "rb");
    if (!file) {
        std::cerr << "Error opening file: " << inputFile << std::endl;
        return;
    }

    // 打开txt文件用于存储文件名以及对应的参数
    std::ofstream txtFile(outputPath + "/list.txt");

    int i = 0;
    while (!feof(file))
    {
        printf("%d\n", i);
        SINT32 nVLFrameSize = 1.5 * S732_VL_W * S732_VL_H + 1.5 * S732_VL_W * PARA_VL_LINE_S732;
        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));

        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_YUV2BGR_NV12);
        imshow("",IMG);
        waitKey(1);

        // 保存jpg图像
        std::string jpgFile = outputPath + "/" + "frame_" + std::to_string(i) + ".jpg";
        cv::imwrite(jpgFile, IMG);

        // 写入txt文件
        txtFile << std::fixed << std::setprecision(6);
        txtFile << "Frame " << i << ","
            << Paras_VL.trackInputPara.stAirCraftInfo.stPos.B << ","
            << Paras_VL.trackInputPara.stAirCraftInfo.stPos.L << ","
            << Paras_VL.trackInputPara.stAirCraftInfo.stPos.H << ","
            << Paras_VL.trackInputPara.stAirCraftInfo.stPos.H << ","
            << Paras_VL.trackInputPara.stAirCraftInfo.stAtt.fYaw << ","
            << Paras_VL.trackInputPara.stAirCraftInfo.stAtt.fPitch << ","
            << Paras_VL.trackInputPara.stAirCraftInfo.stAtt.fRoll << ","
            << Paras_VL.trackInputPara.stServoInfo.fServoAz << ","
            << Paras_VL.trackInputPara.stServoInfo.fServoPt << ","
            << Paras_VL.trackInputPara.stCameraInfo.nFocus << ","
            << Paras_VL.trackInputPara.stCameraInfo.fPixelSize
            << "\n";

        i++;
    }

    // 关闭文件
    fclose(file);
    txtFile.close();
    std::cout << "Conversion completed. Output saved to " << outputPath << "list.txt\n";

}


int main(int argc, char** argv)
{
    std::cout << "Hello, from convert!\n";

    
    //if (argc < 2) {
    //    std::cerr << "Usage: " << argv[0] << " <input_file> <output_path>" << std::endl;
    //    return 1;
    //}

    string inputFilePath = "G:/202509baotou/20250928_155516413_28房子.yuv";

    // 去除后缀，在同级目录建立output文件夹
    string inputFileName = inputFilePath.substr(0, inputFilePath.find_last_of('.'));
    string outputPath = inputFileName + "_output";
    // if (_mkdir(outputPath.c_str()) == -1) 
    // {
    //     std::cerr << "Error: Could not create output directory " << outputPath << std::endl;
    //     return 1;
    // }
    
    //convertS729_VL(inputFilePath, outputPath); // Replace with actual input and output file names

   // convertS732_IR(inputFilePath, outputPath); // Replace with actual input and output file names

    convertS732_VL(inputFilePath, outputPath);

    return 0;
}