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

using namespace std;
using namespace cv;

API_UnderStitch* API_UnderStitch::Create(SINT32 nWidth, SINT32 nHeight)
{
    return new UnderStitch(nWidth,nHeight);
}


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


UnderStitch::UnderStitch(SINT32 nWidth, SINT32 nHeight)
{
	_GeoSolver = new GeoSolver();

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

	_cache = new FileCache<FrameCache>(50,"./cache");

	_BATask = new BA_Task(_cache);

	_BlendTask = new MapBlend(_cache);

	_panPara = { 0 };

	_totalFrameCnt = 0;
}

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




UPanInfo UnderStitch::InitMap(FrameInfo info)
{
	// 设置拼接原点
	_GeoSolver->SetOriginPoint(info);

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

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

	// 中心原始空间分辨率 P/m
	double gsd =  (info.nEvHeight * info.camInfo.fPixelSize) / (info.camInfo.nFocus * 1000);

	// 全景图初始化
	UPanInfo panPara = { 0 };
	panPara.m_pan_width = MIN(info.nWidth * 5,5000);//全景宽
	panPara.m_pan_height = MIN(info.nWidth * 5,5000);//全景高
	panPara.scale = gsd / 1;//比例尺,1m = ?pix

	int zoom = _googleProduct.getZoomLevel(panPara.scale);

	// 直接无平移解算
	cv::Mat H_0 = getAffineFromGeo2Pan(panPara);
	auto cur = warpPointWithH(H_0, ct_geo);

	// 计算平移到全景图固定点的平移量，从此处开始拼接
	int planX = panPara.m_pan_width/2;
	int planY = panPara.m_pan_height/2;

	panPara.map_shiftX = planX - (cur.x);//平移X
	panPara.map_shiftY = planY - (cur.y);//平移Y

	// 获取全景图的投影矩阵，全景图配置更新前保持不变
	_H_pan = getAffineFromGeo2Pan(panPara);


	return panPara;
}




UPanInfo UnderStitch::Init(FrameInfo info)
{
	_panPara = InitMap(info);

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

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


	return _panPara;
}

//BYTE8 UnderStitch::GeoStitch(GD_VIDEO_FRAME_S img, FrameInfo para)
//{
//	Proj t_Proj = _GeoStitcher->AnlayseTform(para);
//
//	// 计算帧的map四至
//	cv::Point2f leftTop_map = _GeoStitcher->back_project(cv::Point2f(0,0), t_Proj, _panPara);
//	cv::Point2f rightTop_map = _GeoStitcher->back_project(cv::Point2f(img.u32Width,0), t_Proj, _panPara);
//	cv::Point2f rightBottom_map = _GeoStitcher->back_project(cv::Point2f(img.u32Width,img.u32Height), t_Proj, _panPara);
//	cv::Point2f leftBottom_map = _GeoStitcher->back_project(cv::Point2f(0,img.u32Height), t_Proj, _panPara);
//
//
//	auto H = _GeoStitcher->findHomography(t_Proj,_panPara);
//	leftTop_map = warpPointWithH(H,cv::Point2f(0,0));
//	rightTop_map = warpPointWithH(H,cv::Point2f(img.u32Width,0));
//	rightBottom_map = warpPointWithH(H,cv::Point2f(img.u32Width,img.u32Height));
//	leftBottom_map = warpPointWithH(H,cv::Point2f(0,img.u32Height));
//
//	// 计算全景图的范围
//	int right = max(max(max(leftTop_map.x, leftBottom_map.x), rightTop_map.x), rightBottom_map.x);
//	int left = min(min(min(leftTop_map.x, leftBottom_map.x), rightTop_map.x), rightBottom_map.x);
//	int top = min(min(min(leftTop_map.y, leftBottom_map.y), rightTop_map.y), rightBottom_map.y);
//	int bottom = max(max(max(leftTop_map.y, leftBottom_map.y), rightTop_map.y), rightBottom_map.y);
//
//
//	int xRange = right - left;
//	int yRnage = bottom - top;
//
//	//反映射到像素坐标
//	int valid_top = std::max(0, top);
//	int valid_bottom = std::min(_pan.cols, bottom);
//	int valid_left = std::max(0, left);
//	int valid_right = std::min(_pan.rows, right);
//
//#pragma omp parallel for
//	for (int i = valid_top; i < valid_bottom; i++)
//	{
//		for (int j = valid_left; j < valid_right; j++)
//		{
//			//转换为pixel坐标
//			cv::Point2f p_img = _GeoStitcher->project(Point2f(j, i), t_Proj, _panPara);
//
//			p_img = warpPointWithH(H.inv(),Point2f(j, i));
//			if (p_img.x >= 0 && p_img.y >= 0 && p_img.x < img.u32Width && p_img.y < img.u32Height)
//			{
//				_pan.data[i * _pan.rows + j] =
//					FourPointInterpolation(img.u64VirAddr[0], img.u32Width, img.u32Height, p_img.x, p_img.y);
//			}
//		}
//	}
//
//
//    return 0;
//}

BYTE8 UnderStitch::GeoStitch(GD_VIDEO_FRAME_S img, FrameInfo para)
{
	cv::Mat src = getRGBAMatFromGDFrame(img,img.u64VirAddr[0]);

	// uv2Geo H
	cv::Mat H1 = _GeoSolver->findHomography(para);

	cv::Mat H = _H_pan * H1;

	// 利用H投影当前帧到全景
	cv::Mat imagetmp(_panImage.size(), CV_8UC3, cv::Scalar(0, 0, 0));

	cv::warpPerspective(src, imagetmp, H, imagetmp.size(), cv::INTER_LINEAR, cv::BORDER_TRANSPARENT);

	cv::Mat mask = cv::Mat::ones(src.size(), CV_8UC1) * 255;
	cv::Mat warped_mask;
	cv::warpPerspective(mask, warped_mask, H, imagetmp.size(), cv::INTER_LINEAR);
	imagetmp.copyTo(_panImage, warped_mask);
	return 0;
}






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

	_totalFrameCnt++;



	return _totalFrameCnt;
}

SINT32 UnderStitch::ProcessFrame()
{
	// 优化所有帧
	_BATask->OptFrame(_recvFrameKey, _H_pan);

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




	return 0;
}



bool UnderStitch::ExportGeoPng(std::string dir, std::string name)
{
	// 计算全景图的坐标范围
	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.box.north = maxB;
	info.box.south = minB;
	info.box.west = minL;
	info.box.east = maxL;
	info.tileName = name;
	info.href = name + "_pan.png";

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

	return 0;
}



bool UnderStitch::ExportGoogleTile(std::string dir, std::string name)
{
	// 计算全景图的坐标范围
	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.box.north = maxB;
	info.box.south = minB;
	info.box.west = minL;
	info.box.east = maxL;
	info.ind.z = _googleProduct.getZoomLevel(_panPara.scale);

	_googleProduct.ExportTile(_panImage, info, dir, name);
	return 0;
}



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

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