#include "Arith_BATask.h"
#include "ceres/ceres.h"
using namespace ceres;

struct CostFunctor {
    template <typename T>
    bool operator()(const T* const x, T* residual) const {
        residual[0] = 10.0 - x[0];
        return true;
    }
};



BA_Task::BA_Task(GeoStitcher * pGeoTrans)
{
    _GeoStitcher = pGeoTrans;
    _FeaMatcher = new FeatureMatcher(DetectorType::ORB, MatcherType::BF);
    _imgVec.reserve(100);
}

BA_Task::~BA_Task()
{
}

SINT32 BA_Task::addFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
{
    cv::Mat frame = Mat(img.u32Height, img.u32Width, CV_8UC1, (void*)img.u64VirAddr[0]);

    // 缓存图像帧
    if (img.enPixelFormat == GD_PIXEL_FORMAT_E::GD_PIXEL_FORMAT_GRAY_Y8)
    {
        _imgVec.emplace_back(frame.clone());
    }
    // 缓存外参
    _paraVec.push_back(para);

    // 缓存初始H
    Proj t_Proj = _GeoStitcher->AnlayseTform(para);
    auto H = _GeoStitcher->findHomography(t_Proj,_panPara);
    _origMatrix.push_back(H);
    _currMatrix.push_back(H);


    // 提取特征点
    std::vector<cv::KeyPoint> keypoints;
    cv::Mat descriptors;
    _FeaMatcher->extractFeatures(frame,keypoints,descriptors);
    _FeaPtVec.push_back(keypoints);
    _FeaDespVec.push_back(descriptors);

    // 缓存包围多边形
    _polygon.push_back(warpRectWithH(H,cv::Size(img.u32Width,img.u32Height)));




    return _imgVec.size();
}

void BA_Task::setPanPara(PanInfo info)
{
    _panPara = info;
}






void BA_Task::Test()
{
    // google::InitGoogleLogging("ceres");

    // // The variable to solve for with its initial value. It will be
    // // mutated in place by the solver.
    // double x = 0.5;
    // const double initial_x = x;

    // // Build the problem.
    // ceres::Problem problem;

    // // Set up the only cost function (also known as residual). This uses
    // // auto-differentiation to obtain the derivative (jacobian).
    // ceres::CostFunction* cost_function =
    //     new ceres::AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
    // problem.AddResidualBlock(cost_function, nullptr, &x);

    // // Run the solver!
    // ceres::Solver::Options options;
    // options.minimizer_progress_to_stdout = true;
    // ceres::Solver::Summary summary;
    // ceres::Solve(options, &problem, &summary);

    // std::cout << summary.BriefReport() << "\n";
    // std::cout << "x : " << initial_x << " -> " << x << "\n";

    
}

void BA_Task::optimizeBA()
{
    // 计算匹配性矩阵
    CalMatchMat(0.3);


}

SINT32 BA_Task::CalMatchMat(float fiou_thre)
{
    _IOUMat = cv::Mat::zeros(_polygon.size(),_polygon.size(),CV_32FC1);
    // 先计算IOU矩阵
    for (size_t i = 0; i < _polygon.size(); i++)
    {
        vector<cv::Point2f> poly_i = _polygon[i];

        for (size_t j = 0; j < _polygon.size(); j++)
        {
            if (i == j)
            {
                _IOUMat.at<float>(i,j) = 1; 
                continue;
            }
            vector<cv::Point2f> poly_j = _polygon[j];

            float fiou = computeQuadrilateralIOU(poly_i,poly_j);
            _IOUMat.at<float>(i,j) = fiou;
            _IOUMat.at<float>(j,i) = fiou;//是对称矩阵
            
        }
    }

    // IOU大于0.3以上才进行特征点匹配
    for (size_t i = 0; i < _polygon.size(); i++)
    {
        for (size_t j = 0; j < _polygon.size(); j++)
        {
            if (i == j)
            {
                continue;
            }

            if (_IOUMat.at<float>(i,j) < fiou_thre)
            {
                continue;
            }


            // IOU满足条件才匹配特征点
            cv::Mat image(1000, 1000, CV_8UC3, cv::Scalar(255, 255, 255));
            vector<vector<cv::Point2f>> tmpPoly;
            tmpPoly.push_back(_polygon[i]);
            tmpPoly.push_back(_polygon[j]);
            drawPolygons(image, tmpPoly);

            // 显示绘制结果
            cv::imshow("Polygons", image);
            cv::waitKey(1);
            std::vector<cv::DMatch> matches;
            _FeaMatcher->matchFeatures(_FeaDespVec[i],_FeaDespVec[j],matches);

            // 可视化匹配结果
            cv::Mat img_matches;
            cv::drawMatches(
                _imgVec[i], _FeaPtVec[i], // 第一幅图像及其特征点
                _imgVec[j], _FeaPtVec[j], // 第二幅图像及其特征点
                matches,          // 匹配结果
                img_matches,      // 输出图像
                cv::Scalar::all(-1), // 匹配线颜色（默认随机颜色）
                cv::Scalar::all(-1), // 未匹配点颜色（默认不绘制）
                std::vector<char>(), // 掩码（可选，用于筛选匹配）
                cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS // 不绘制未匹配的点
            );

            cv::resize(img_matches,img_matches,cv::Size(1280,512));

            // 显示匹配结果
            cv::imshow("Feature Matches", img_matches);
            cv::waitKey(0);

        }
    }
    



    return 0;
}




// 函数：绘制多边形
void drawPolygons(cv::Mat& image, const std::vector<std::vector<cv::Point2f>>& polygons) 
{
    // 定义颜色列表
    std::vector<cv::Scalar> colors = {
        cv::Scalar(255, 0, 0),   // 蓝色
        cv::Scalar(0, 255, 0),   // 绿色
        cv::Scalar(0, 0, 255),   // 红色
        cv::Scalar(255, 255, 0), // 青色
        cv::Scalar(255, 0, 255), // 品红
        cv::Scalar(0, 255, 255)  // 黄色
    };

    // 绘制每个多边形
    for (size_t i = 0; i < polygons.size(); ++i) {
        const auto& polygon = polygons[i];
        cv::Scalar color = colors[i % colors.size()]; // 循环使用颜色

        // 将多边形点转换为整数类型
        std::vector<cv::Point> intPolygon;
        for (const auto& pt : polygon) {
            intPolygon.push_back(cv::Point(static_cast<int>(pt.x), static_cast<int>(pt.y)));
        }

        // 绘制多边形
        cv::polylines(image, intPolygon, true, color, 2);
    }
}