#include "API_UnderStitch.h"
#include "Arith_UnderStitch.h"
#include "Arith_Utils.h"
#include "Arith_CoordModule.h"
#include "Arith_FeaMatch.h"
#include "Arith_GeoSolver.h"
#include <opencv2/opencv.hpp>
#include <omp.h>
#include "utils/Arith_ThreadPool.h"

#ifdef _WIN32
#include <direct.h>
#include <io.h>
#define MKDIR(dir) _mkdir(dir)
#define ACCESS _access
#else
#include <sys/stat.h>
#include <unistd.h>
#define MKDIR(dir) mkdir(dir, 0777)
#define ACCESS access
#endif


#include "Logger.h"



using namespace std;
using namespace cv;

API_UnderStitch* API_UnderStitch::Create(std::string cachedir)
{
    return new UnderStitch(cachedir);
}


void API_UnderStitch::Destroy(API_UnderStitch* obj)
{
    delete obj;
}


UnderStitch::UnderStitch(std::string cachedir)
{
	// 设置日志立即刷新
	spdlog::flush_on(spdlog::level::info);
	spdlog::flush_every(std::chrono::seconds(0));

	_cacheDir = cachedir;
	
	// 创建缓存对象
	_cache = new FileCache<FrameCache>(500, cachedir);

	_GeoSolver = new GeoSolver();

	_FeaMatcher = new FeatureMatcher(DetectorType::SIFT, MatcherType::FLANN);

	_BATask = new BA_Task(_cache);

	_BlendTask = new MapBlend(_cache);

	_panPara = { 0 };

	_totalFrameCnt = 0;

	_lastFrameKey = 0;

	LOG_INFO("UnderStitch init success");
}

UnderStitch::~UnderStitch()
{
	Stop();
	delete _GeoSolver;
	delete _cache;
}




UPanInfo UnderStitch::InitMap(FrameInfo info)
{

	cv::Mat H0 = _GeoSolver->findHomography(info);

	// 计算视场中心的地理坐标
	cv::Point2f ct_geo = warpPointWithH(H0, cv::Point2f(info.nWidth / 2, info.nHeight / 2));

	// 使用载体偏航角判断飞行方向
	double yaw_angle = info.craft.stAtt.fYaw;
	
	// 根据飞行方向调整全景图尺寸
	int base_size = MIN(info.nWidth * 5, 8000);
	UPanInfo panPara = { 0 };
	
	// 根据偏航角计算宽高比
	// 将偏航角归一化到0-360度
	yaw_angle = fmod(yaw_angle + 360.0, 360.0);
	
	// 计算宽高比系数
	// 当偏航角为0或180度时，宽高比最大（1:0.6）
	// 当偏航角为90或270度时，宽高比最小（0.6:1）
	// 使用余弦函数实现平滑过渡
	double cos_angle = cos(yaw_angle * CV_PI / 180.0);
	double aspect_ratio = 0.6 + 0.4 * (1.0 + cos_angle) / 2.0;
	
	// 根据飞行方向设置宽高
	if (std::abs(cos_angle) > 0.7071) // cos(45°) ≈ 0.7071
	{
		// 接近水平方向
		panPara.m_pan_width = base_size;
		panPara.m_pan_height = base_size * aspect_ratio;
	}
	else
	{
		// 接近垂直方向
		panPara.m_pan_width = base_size * aspect_ratio;
		panPara.m_pan_height = base_size;
	}

	

	// 计算单帧投影后的地理尺寸
	auto geo_pro_rect = warpRectWithH_2Rect(H0,cv::Size(info.nWidth,info.nHeight));
	
	// 计算目标比例（单帧投影后占全景图的0.25）
	float target_ratio = 0.25;
	float current_ratio = MAX(geo_pro_rect.width / (target_ratio*panPara.m_pan_width), geo_pro_rect.height / (target_ratio*panPara.m_pan_height));
	
	// 调整scale参数
	panPara.scale =  1.0f/current_ratio;

	// 重新计算投影矩阵
	cv::Mat mapH0 = getAffineFromGeo2Pan(panPara);
	auto cur = warpPointWithH(mapH0, ct_geo);

	// 计算平移到全景图固定点的平移量
	int planX = panPara.m_pan_width/2;
	int planY = panPara.m_pan_height/2;

	panPara.map_shiftX += planX - (cur.x);
	panPara.map_shiftY += planY - (cur.y);

	// 获取全景图的投影矩阵
	_H_pan = getAffineFromGeo2Pan(panPara);

	// 计算全景图瓦片相关
	auto P1 = getBLHFromPan(cv::Point2f(0, 0), _H_pan);
	auto P2 = getBLHFromPan(cv::Point2f(panPara.m_pan_width, 0), _H_pan);
	auto P3 = getBLHFromPan(cv::Point2f(panPara.m_pan_width, panPara.m_pan_height), _H_pan);
	auto P4 = getBLHFromPan(cv::Point2f(0, panPara.m_pan_height), _H_pan);

	auto minL = min(min(min(P1.L, P2.L), P3.L), P4.L);
	auto maxL = max(max(max(P1.L, P2.L), P3.L), P4.L);
	auto minB = min(min(min(P1.B, P2.B), P3.B), P4.B);
	auto maxB = max(max(max(P1.B, P2.B), P3.B), P4.B);

	_panTile.boxLatLon.north = maxB;
	_panTile.boxLatLon.south = minB;
	_panTile.boxLatLon.west = minL;
	_panTile.boxLatLon.east = maxL;
	_panTile.ind.z = _googleProduct.ZoomLevel(panPara.scale);

	// 生成瓦片划分
	_taskTilesVec = _googleProduct.CalcTileOverlayVec(_panTile, panPara.m_pan_width, panPara.m_pan_height);

	LOG_INFO("InitMap success, yaw angle: {:.2f}, aspect ratio: {:.2f}, scale: {:.2f}", 
		yaw_angle, aspect_ratio, panPara.scale);

	return panPara;
}

UPanInfo UnderStitch::ReLocateMap(FrameInfo info)
{
    cv::Mat H0 = _GeoSolver->findHomography(info);

    // 计算视场中心的地理坐标
    cv::Point2f ct_geo = warpPointWithH(H0, cv::Point2f(info.nWidth / 2, info.nHeight / 2));

    // 计算当前帧中心点在全景图中的位置
    auto cur = warpPointWithH(_H_pan, ct_geo);

    // 计算平移到全景图固定点的平移量
    int planX = _panPara.m_pan_width/2;
    int planY = _panPara.m_pan_height/2;

    // 计算平移量，注意：包含原有的偏移量
    float shift_x = (planX - cur.x);
    float shift_y = (planY - cur.y);

    _panPara.map_shiftX += shift_x;
    _panPara.map_shiftY += shift_y;

    // 获取全景图的投影矩阵
    _H_pan = getAffineFromGeo2Pan(_panPara);

    // 计算全景图瓦片相关
    auto P1 = getBLHFromPan(cv::Point2f(0, 0), _H_pan);
    auto P2 = getBLHFromPan(cv::Point2f(_panPara.m_pan_width, 0), _H_pan);
    auto P3 = getBLHFromPan(cv::Point2f(_panPara.m_pan_width, _panPara.m_pan_height), _H_pan);
    auto P4 = getBLHFromPan(cv::Point2f(0, _panPara.m_pan_height), _H_pan);

    auto minL = min(min(min(P1.L, P2.L), P3.L), P4.L);
    auto maxL = max(max(max(P1.L, P2.L), P3.L), P4.L);
    auto minB = min(min(min(P1.B, P2.B), P3.B), P4.B);
    auto maxB = max(max(max(P1.B, P2.B), P3.B), P4.B);

    _panTile.boxLatLon.north = maxB;
    _panTile.boxLatLon.south = minB;
    _panTile.boxLatLon.west = minL;
    _panTile.boxLatLon.east = maxL;
    _panTile.ind.z = _googleProduct.ZoomLevel(_panPara.scale);

    // 生成瓦片划分
    _taskTilesVec = _googleProduct.CalcTileOverlayVec(_panTile, _panPara.m_pan_width, _panPara.m_pan_height);

    LOG_INFO("ReInitMap success, shift_x: {}, shift_y: {}", shift_x, shift_y);

    return _panPara;
}


UPanInfo UnderStitch::Init(FrameInfo info)
{
	// 设置拼接原点：地理计算的基准点
	_GeoSolver->SetOriginPoint(info);

	// 初始化全景图
	_panPara = InitMap(info);

	_panImage = cv::Mat(_panPara.m_pan_height, _panPara.m_pan_width, CV_8UC4, cv::Scalar(0,0,0,0));

	_panMask = cv::Mat::zeros(_panPara.m_pan_height, _panPara.m_pan_width, CV_8UC1);

	_lastFrameKey = 0;

	_totalFrameCnt = 0;

	// 默认配置参数
	_config.bOutGoogleTile = true;
	_config.bUseBA = true;
	_config.bOutFrameTile = true;

	// 重置全景图
	_panImage.setTo(0);
	_panMask.setTo(0);

	LOG_INFO("Init success");
	return _panPara;
}

void UnderStitch::SetOutput(std::string filename, std::string outdir)
{
	_filename = filename;
	_outDir = outdir;
	_kmlPath = _outDir + "/" + _filename + ".kml";

	// 创建输出目录
	if (ACCESS(_outDir.c_str(), 0) != 0)
	{
		MKDIR(_outDir.c_str());
	}

	LOG_INFO("SetOutput {}",_outDir);
}

void UnderStitch::SetConfig(UPanConfig config)
{
	_config = config;
}

bool UnderStitch::isFrameInPanView(const cv::Size& frameSize, const cv::Mat& H, const cv::Size& panSize)
{
    // 使用warpRectWithH_2Rect计算投影后的矩形
    cv::Rect2f warped_rect = warpRectWithH_2Rect(H, frameSize);
    
    // 检查是否完全在panSize范围内
    // 如果完全在范围内，返回true
    if (warped_rect.x > 0 && 
        warped_rect.y > 0 && 
        warped_rect.x + warped_rect.width < panSize.width && 
        warped_rect.y + warped_rect.height < panSize.height)
    {
        return true;  // 完全在内部，是有效的帧
    }
    
    // 如果接触到边缘或超出范围，返回false
    return false;  // 需要触发处理
}

SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S frame, FrameInfo para)
{
    // 视频帧筛选
    if (!FilterFrame(para))
    {
        return -1;
    }

    LOG_INFO("Run frame {} ,_totalFrameCnt {}",para.nFrmID,_totalFrameCnt);

    // 获取地理投影H
    cv::Mat H_geo = _GeoSolver->findHomography(para);

    // 判断帧是否在全景图视场内
    if (!isFrameInPanView(cv::Size(para.nWidth, para.nHeight), _H_pan * H_geo, 
        cv::Size(_panPara.m_pan_width, _panPara.m_pan_height)))
    {
        LOG_INFO("Frame {} is out of pan view, reinitializing projection parameters", para.nFrmID);
        
		// 输出当前所有瓦片，再不输出就会丢失当前全景中的瓦片
		LOG_INFO("ExportTileSet");
		_googleProduct.ExportTileSet(_panImage, _panMask, _taskTilesVec, _outDir, _filename + std::to_string(_lastFrameKey));

        // 重新初始化投影参数
        _panPara = ReLocateMap(para);
			_panImage.setTo(0);
			_panMask.setTo(0);
			_BlendTask->DirectMap(_recvFrameKey, _H_pan, _panImage,_panMask);


		// 提交线程池
		// _threadPool.commit(
		// 	[this]() {
		// 	// 重投影所有帧到全景
		// 	_panImage.setTo(0);
		// 	_panMask.setTo(0);
		// 	_BlendTask->DirectMap(_recvFrameKey, _H_pan, _panImage,_panMask);



		// 	}, 
		// 	"remap"
		// );

        return 0;
    }

    // 获取RGBA图像
    cv::Mat src = getRGBAMatFromGDFrame(frame,frame.u64VirAddr[0]);

    // 地理纠正
    TileInfo info = { 0 };
    cv::Mat Orth_Image = getOrthImage(src, para, info);

    // 输出单帧谷歌瓦片
    if (_config.bOutFrameTile)
    {
        _googleProduct.ExportGeoPng(Orth_Image, info, _outDir + "/FrameTile");
    }

    // 投影帧到全景
    _BlendTask->MapFrame(src, H_geo, _H_pan, _panImage,_panMask);

    // 预处理+缓存
    ReceiveFrame(frame, para);

	_config.bUseBA = 0;

    if (_config.bUseBA && TriggerBA(para))
    {
        // 异步执行BA优化
        _threadPool.commit(
            [this]() {
                OptAndOutCurrPan();
            }, 
            "BA_Optimization"
        );
    }		

    return 0;
}

void UnderStitch::Stop()
{
	_recvFrameKey.clear();
	_baWindowKey.clear();
	_cache->clear();
}

SINT32 UnderStitch::OptAndOutCurrPan()
{
	LOG_INFO("BA_Optimization start{}-{}",_baWindowKey[0],_baWindowKey[_baWindowKey.size()-1]);
	if (_config.bUseBA)
	{
		// 优化滑动窗口内的帧
		_BATask->OptFrame(_baWindowKey, _H_pan);
		LOG_INFO("BA_Optimization finish");
	}

	// 重投影所有帧到全景
	_BlendTask->DirectMap(_baWindowKey, _H_pan, _panImage,_panMask);

	// 输出当前所有瓦片
	LOG_INFO("ExportTileSet");
	_googleProduct.ExportTileSet(_panImage, _panMask, _taskTilesVec, _outDir, _filename + std::to_string(_lastFrameKey));

	return _baWindowKey.size();
}


bool UnderStitch::FilterFrame(FrameInfo para)
{
	// 如果是第一帧，直接返回true
	if (_lastFrameKey == 0)
	{
		// 更新处理的上一帧信息
		_lastFrameKey = para.nFrmID;
		_lastFrameInfo = para;
		return true;
	}

	// 扫描角过大，跳过
	if (ABS(para.servoInfo.fServoPt) > 45)
	{
		return false;
	}

	// 获取当前帧的Geo覆盖
	cv::Mat H = _GeoSolver->findHomography(para);
	std::vector<cv::Point2f> currCorners = warpRectWithH(H, cv::Size(para.nWidth, para.nHeight));
	
	// 获取上一帧的Geo覆盖
	cv::Mat lastH = _GeoSolver->findHomography(_lastFrameInfo);
	std::vector<cv::Point2f> lastCorners = warpRectWithH(lastH, cv::Size(_lastFrameInfo.nWidth, _lastFrameInfo.nHeight));

	// 计算IOU
	double iou = computeQuadrilateralIOU(currCorners, lastCorners);
	
	// 计算旋转角
	double currAngle = std::atan2(H.at<double>(1,0), H.at<double>(0,0)) * 180.0 / CV_PI;
	double lastAngle = std::atan2(lastH.at<double>(1,0), lastH.at<double>(0,0)) * 180.0 / CV_PI;
	double angleDiff = std::abs(currAngle - lastAngle);
	if (angleDiff > 180.0) angleDiff = 360.0 - angleDiff;


	// 判断条件：IOU < 0.8 或 旋转角差 > 30°
	bool bFilter = (iou < 0.8) || (angleDiff > 30.0);

	if(bFilter)
	{
		// 更新处理的上一帧信息
		_lastFrameKey = para.nFrmID;
		_lastFrameInfo = para;

		LOG_INFO("FilterFrame iou:{:.2f} angleDiff:{:.2f}", iou, angleDiff);
	}

	return bFilter;
}

bool UnderStitch::TriggerBA(FrameInfo para)
{
    // 1. 固定帧间隔触发
    const int FIXED_FRAME_INTERVAL = 5;
    if (_totalFrameCnt % FIXED_FRAME_INTERVAL == 0)
    {
        LOG_INFO("TriggerBA: Fixed frame interval reached");
        return true;
    }

    // 2. 形态变化触发
    if (_lastFrameKey != 0)
    {
        cv::Mat currH = _GeoSolver->findHomography(para);
        cv::Mat lastH = _GeoSolver->findHomography(_lastFrameInfo);
        
        // 计算旋转角变化
        double currAngle = std::atan2(currH.at<double>(1,0), currH.at<double>(0,0)) * 180.0 / CV_PI;
        double lastAngle = std::atan2(lastH.at<double>(1,0), lastH.at<double>(0,0)) * 180.0 / CV_PI;
        double angleDiff = std::abs(currAngle - lastAngle);
        if (angleDiff > 180.0) angleDiff = 360.0 - angleDiff;

        // 计算平移量变化
        double txDiff = std::abs(currH.at<double>(0,2) - lastH.at<double>(0,2));
        double tyDiff = std::abs(currH.at<double>(1,2) - lastH.at<double>(1,2));
        
        if (angleDiff > 15.0 || txDiff > 100.0 || tyDiff > 100.0)
        {
            LOG_INFO("TriggerBA: Significant transformation change detected");
            return true;
        }
    }

    // 3. 特征点匹配质量触发
    if (_baWindowKey.size() >= 2)
    {
        std::shared_ptr<FrameCache> lastFrame;
        std::shared_ptr<FrameCache> currFrame;
        
        if (_cache->get(_baWindowKey.back(), lastFrame) && 
            _cache->get(para.nFrmID, currFrame))
        {
            // 如果特征点数量显著减少
            if (currFrame->ptNum < lastFrame->ptNum * 0.5)
            {
                LOG_INFO("TriggerBA: Significant feature point reduction detected");
                return true;
            }
        }
    }

    // 4. 累积误差触发
    if (_baWindowKey.size() >= 10)
    {
        // 计算最近10帧的累积变换误差
        double totalError = 0.0;
        size_t startIdx = _baWindowKey.size() - 10;  // 从倒数第10帧开始
        
        for (size_t i = startIdx + 1; i < _baWindowKey.size(); i++)
        {
            std::shared_ptr<FrameCache> prevFrame;
            std::shared_ptr<FrameCache> currFrame;
            
            if (_cache->get(_baWindowKey[i-1], prevFrame) && 
                _cache->get(_baWindowKey[i], currFrame))
            {
                cv::Mat H1(3, 3, CV_64F, prevFrame->H);
                cv::Mat H2(3, 3, CV_64F, currFrame->H);
                cv::Mat diff = H1 - H2;
                totalError += cv::norm(diff);
            }
        }
        
        if (totalError > 1000.0)  // 设置一个合适的阈值
        {
            LOG_INFO("TriggerBA: Accumulated error threshold reached");
            return true;
        }
    }

    return false;
}

// 废弃，改为输出单帧正射后的瓦片
void UnderStitch::CutTileRealTime()
{
	// 在4倍降采样图上计算瓦片覆盖情况
	int downSampleR = 4;

	cv::Mat panMasksmall;
	cv::resize(_panMask, panMasksmall, cv::Size(_panMask.cols / downSampleR, _panMask.rows / downSampleR));

	// 计算覆盖图的积分图
	cv::Mat integralMask;
	cv::integral(panMasksmall, integralMask, CV_32S);

	// 遍历已经在初始化阶段得到的瓦片划分
	auto tiles = _taskTilesVec;

	int cnt = 0;
	for (size_t i = 0; i < tiles.size(); i++)
	{
		TileInfo* ptile = &_taskTilesVec[i];
		RECT32S box = ptile->boxInPan;

		// 跳过已输出块
		if (ptile->nStatus > 0)
		{
			//cnt++;
			continue;
		}

		SINT32 x1 = box.x/ downSampleR;
		SINT32 y1 = box.y/ downSampleR;
		SINT32 x2 = box.x/ downSampleR + box.w/ downSampleR;
		SINT32 y2 = box.y/ downSampleR + box.h/ downSampleR;

		// 使用积分图快速计算区域和
		float maskSum = integralMask.at<int>(y2, x2)
			- integralMask.at<int>(y1, x2)
			- integralMask.at<int>(y2, x1)
			+ integralMask.at<int>(y1, x1);

		// 计算瓦片影像覆盖率
		float Coverage = maskSum * downSampleR * downSampleR / (box.w * box.h);

		if (Coverage > 0.95)
		{
			// 输出并标记
			ptile->nStatus = 1;
			_googleProduct.ExportOneTile(_panImage, *ptile, _outDir, _filename);
		}

	}

	
}


SINT32 UnderStitch::ReceiveFrame(GD_VIDEO_FRAME_S img, FrameInfo para)
{	
	// 构造单帧缓存
	auto _t_frame_cache = std::make_shared<FrameCache>();

	_t_frame_cache->_para = para;
	_t_frame_cache->_frame_info = img;

	int imgSize = 0;

	if(img.enPixelFormat == GD_PIXEL_FORMAT_GRAY_Y8)
	{
		imgSize = img.u32Height * img.u32Width;
	}
	if(img.enPixelFormat == GD_PIXEL_FORMAT_RGB_PACKED)
	{
		imgSize = img.u32Height * img.u32Width * 3;
	}	
	if (img.enPixelFormat == GD_PIXEL_FORMAT_NV12)
	{
		imgSize = img.u32Height * img.u32Width * 1.5;
	}

	// 深拷贝图像数据
	memcpy(_t_frame_cache->_data,img.u64VirAddr[0],imgSize);

	// 提取特征点
	std::vector<cv::KeyPoint> keypoints;
	cv::Mat descriptors;

	cv::Mat src = getRGBAMatFromGDFrame(img, img.u64VirAddr[0]);

	_FeaMatcher->extractFeatures(src, keypoints, descriptors);
	
	size_t keyNum = MIN(keypoints.size(), FEA_NUM_MAX);

	_t_frame_cache->ptNum = keyNum;

	// 保存特征点
	memcpy(_t_frame_cache->_pt, keypoints.data(), sizeof(cv::KeyPoint) * keyNum);

	// 保存描述子
	memcpy(_t_frame_cache->_desp, descriptors.data, sizeof(float) * keyNum * FEA_DES_SIZE);

	cv::Mat_<double> H0 = _GeoSolver->findHomography(para);

	// 保存初始H
	memcpy(_t_frame_cache->H, H0.data, sizeof(double) * 9);

	// 预处理结果加入文件缓存
	_cache->set(para.nFrmID,_t_frame_cache);

	_recvFrameKey.push_back(para.nFrmID);
	_baWindowKey.push_back(para.nFrmID);

	// 保持滑动窗口大小
	const size_t WINDOW_SIZE = 30;  // 滑动窗口大小
	if (_baWindowKey.size() > WINDOW_SIZE)
	{
		_baWindowKey.erase(_baWindowKey.begin());
	}

	// 优化表初始化
	_recvFrameOptFlag[para.nFrmID] = 0;

	_totalFrameCnt++;

	return _totalFrameCnt;
}





bool UnderStitch::ExportGeoPng()
{
	// 计算全景图的坐标范围
	auto P1 = getBLHFromPan(cv::Point2f(0, 0), _H_pan);
	auto P2 = getBLHFromPan(cv::Point2f(_panImage.cols, 0), _H_pan);

	auto P3 = getBLHFromPan(cv::Point2f(_panImage.cols, _panImage.rows), _H_pan);
	auto P4 = getBLHFromPan(cv::Point2f(0, _panImage.rows), _H_pan);

	auto minL = min(min(min(P1.L, P2.L), P3.L), P4.L);
	auto maxL = max(max(max(P1.L, P2.L), P3.L), P4.L);

	auto minB = min(min(min(P1.B, P2.B), P3.B), P4.B);
	auto maxB = max(max(max(P1.B, P2.B), P3.B), P4.B);

	TileInfo info = { 0 };

	info.boxLatLon.north = maxB;
	info.boxLatLon.south = minB;
	info.boxLatLon.west = minL;
	info.boxLatLon.east = maxL;
	info.tileName = _filename;
	info.href = _filename + ".png";

	// 输出谷歌地图产品
	_googleProduct.ExportGeoPng(_panImage, info, _outDir);

	return 0;
}


cv::Mat UnderStitch::getOrthImage(cv::Mat src, FrameInfo para,TileInfo& tile)
{
	cv::Mat H = _GeoSolver->findHomography(para);

	cv::Rect2f roi = warpRectWithH_2Rect(H, src.size());

	float scale = src.cols / MAX(roi.width, roi.height);
	float shiftX = -roi.x * scale;
	float shiftY = (roi.y  + roi.height)* scale;

	Mat H_proj = (Mat_<double>(3, 3) << scale, 0, shiftX,
		0, -scale, shiftY,
		0, 0, 1
		);
	cv::Rect2f roi2 = warpRectWithH_2Rect(H_proj * H, src.size());

	cv::Mat dst;
	warpPerspective(src, dst, H_proj * H, cv::Size(roi2.width, roi2.height));


	// 计算全景图的坐标范围
	auto P1 = _GeoSolver->getBLHFromFrame(H,cv::Point2f(0, 0));
	auto P2 = _GeoSolver->getBLHFromFrame(H, cv::Point2f(src.cols, 0));

	auto P3 = _GeoSolver->getBLHFromFrame(H, cv::Point2f(src.cols, src.rows));
	auto P4 = _GeoSolver->getBLHFromFrame(H, cv::Point2f(0, src.rows));

	auto minL = min(min(min(P1.L, P2.L), P3.L), P4.L);
	auto maxL = max(max(max(P1.L, P2.L), P3.L), P4.L);

	auto minB = min(min(min(P1.B, P2.B), P3.B), P4.B);
	auto maxB = max(max(max(P1.B, P2.B), P3.B), P4.B);

	

	tile.boxLatLon.north = maxB;
	tile.boxLatLon.south = minB;
	tile.boxLatLon.west = minL;
	tile.boxLatLon.east = maxL;
	tile.tileName = _filename + std::to_string(para.nFrmID);
	tile.href = _filename + std::to_string(para.nFrmID) + ".png";



	return dst;
}

GD_VIDEO_FRAME_S UnderStitch::ExportPanAddr()
{
	GD_VIDEO_FRAME_S pan_out;

	pan_out.enPixelFormat = GD_PIXEL_FORMAT_RGB_PACKED;
	pan_out.u32Width = _panPara.m_pan_width;
	pan_out.u32Height = _panPara.m_pan_height;
	pan_out.u64VirAddr[0] = _panImage.data;

	return pan_out;
}

cv::Mat UnderStitch::ExportPanMat()
{
	return _panImage;
}

PointBLH UnderStitch::getBLHFromPan(cv::Point2f ptInPan, cv::Mat _H_panPara)
{
	cv::Mat H_inv = _H_panPara.inv();

	// 解算到局部地理系
	cv::Point2f ptInGeo = warpPointWithH(H_inv,ptInPan);

	return _GeoSolver->getBLHFromGeo(ptInGeo);
}

cv::Point2f UnderStitch::getPanXYFromBLH(PointBLH ptInBLH, cv::Mat _H_panPara)
{
	// 经纬度转局部地理系
	cv::Point2f ptGeo = _GeoSolver->getGeoFromBLH(ptInBLH);

	// 投影到全景图
	return warpPointWithH(_H_panPara, ptGeo);;
}



cv::Mat UnderStitch::getAffineFromGeo2Pan(UPanInfo _pan)
{
	Mat H = (Mat_<double>(3, 3) << _pan.scale, 0, _pan.map_shiftX,
		0, -_pan.scale, _pan.m_pan_height + _pan.map_shiftY,
		0, 0, 1
		);
	return H;
}


