#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <iostream>
#include "API_FeaStitch.h"
#include "API_FrontStitch.h"
#include "API_UnderStitch.h"
#include "PlatformDefine.h"
#include <string.h>
#include "opencv2/opencv.hpp"
#include <random>
#include <string>
#include "utils.h"
using namespace std;


// 前视拼接
void Run_FrontStitch(std::string picFloder)
{
    Utils reader;
    int FrmCnt = reader.open(picFloder);

    auto pair0 = reader.readInfoAndMat();
    FrameInfo para = pair0.first;
    cv::Mat src = pair0.second;


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

    auto stitcher = API_FrontStitch::Create();

    stitcher->Init(para, ScanRange{-30,30}, ScanRange{-15,-1});

    cv::VideoWriter writer("pan.mp4", cv::VideoWriter::fourcc('M', 'P', '4', 'V'), 5, cv::Size(pan.u32Width, pan.u32Height));

    pan = stitcher->ExportPanAddr();

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

    frame.enPixelFormat = GD_PIXEL_FORMAT_RGB_PACKED;
    frame.u32Width = src.cols;
    frame.u32Height = src.rows;

    //POINT32F ppt = {200,100 };
    //auto blh = stitcher->getBLHFromPanUV(ppt);
    //auto pt = stitcher->getPanUVFromBLH(blh);
    //printf("b:%f,l:%f,h:%f\n", blh.B, blh.L, blh.H);

    for (size_t i = 0; i < FrmCnt - 1; i++)
    {
        auto pair = reader.readInfoAndMat();
        FrameInfo para = pair.first;
        cv::Mat src = pair.second;


        imshow("src", src);
        cv::waitKey(1);

        frame.u64VirAddr[0] = src.data;

        para.nEvHeight = 1316;

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

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

        cout << "Az:" << para.servoInfo.fServoAz << "Pt:" << para.servoInfo.fServoPt << endl;
        cout << "Roll:" << para.craft.stAtt.fRoll << "Pitch:" << para.craft.stAtt.fPitch << "Yaw:" << para.craft.stAtt.fYaw << endl;
        cout << "H:" << para.craft.stPos.H << "B:" << para.craft.stPos.B << "L:" << para.craft.stPos.L << endl;
        cout << "Focus:" << para.camInfo.nFocus << "PixelSize:" << para.camInfo.fPixelSize << "AglReso:" << para.camInfo.fAglReso << endl;
        // 显示全景图
        cv::Mat res;
        cv::resize(mat_pan, res, cv::Size(pan.u32Width / 2, pan.u32Height / 2));
        imshow("pan", res);
        cv::waitKey(1);

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

        cv::Mat rgb(pan.u32Height, pan.u32Width, CV_8UC3);
        writer.write(rgb);
    }
    writer.release();
}



// 下视拼接
void Run_UnderStitch(std::string picFloder)
{
    Utils reader;
    int FrmCnt = reader.open(picFloder);

    auto pair0 = reader.readInfoAndMat();
    FrameInfo para = pair0.first;
    cv::Mat src = pair0.second;


    //// 4K输入需要降采样
    //cv::resize(src, src, cv::Size(src.cols / 2, src.rows / 2));
    //para.camInfo.fPixelSize *= 2;

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

    auto stitcher = API_UnderStitch::Create();

    stitcher->SetOutput("DJI", picFloder + "/google_tiles");

    stitcher->Init(para);

    pan = stitcher->ExportPanAddr();

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

    frame.enPixelFormat = GD_PIXEL_FORMAT_RGB_PACKED;
    frame.u32Width = src.cols;
    frame.u32Height = src.rows;

    for (size_t i = 0; i < FrmCnt - 1; i++)
    {
        auto pair = reader.readInfoAndMat();
        FrameInfo para = pair.first;
        cv::Mat src = pair.second;
        // 4K输入需要降采样
        //cv::resize(src, src, cv::Size(src.cols / 2, src.rows / 2));
        //para.camInfo.fPixelSize *= 2;

        para.nEvHeight = para.craft.stPos.H - 1356;

        frame.u64VirAddr[0] = src.data;

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

        //cout << "time:" << tm.getTimeMilli() << endl;
        cout << "Az:" << para.servoInfo.fServoAz << "Pt:" << para.servoInfo.fServoPt << endl;
        cout << "Roll:" << para.craft.stAtt.fRoll << "Pitch:" << para.craft.stAtt.fPitch << "Yaw:" << para.craft.stAtt.fYaw << endl;
        cout << "H:" << para.nEvHeight << "B:" << para.craft.stPos.B << "L:" << para.craft.stPos.L << endl;
        cout << "Focus:" << para.camInfo.nFocus << "PixelSize:" << para.camInfo.fPixelSize << "AglReso:" << para.camInfo.fAglReso << 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 / 4, pan_rgb.rows /4));

        imshow("", pan_rgb_ds);

        cv::waitKey(1);
    }

    cv::TickMeter tm;
    tm.start();
    // 强制优化，生成最终拼接结果
    stitcher->OptAndOutCurrPan();

    stitcher->Stop();

    tm.stop();

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


}


int main(int argc, char** argv)
{
    printf("Hello World General\n");
    //Run_FrontStitch("F:/S729/1_output");
    Run_FrontStitch("F:/20250928_145957066_9前视扫描_output");

    //Run_UnderStitch("F:/20250928_145957066_9前视扫描_output");

    //Run_UnderStitch("F:/20250928_155516413_28房子_output");
    return 0;
}