#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>

#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

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)
{
	// 创建缓存对象
	_cache = new FileCache<FrameCache>(50, cachedir);

	_GeoSolver = new GeoSolver();

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

	_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));// 视场中心的地理坐标

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

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


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

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


	panPara.map_shiftX = planX - (cur.x);//平移X
	panPara.map_shiftY = planY - (cur.y);//平移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);


	return panPara;
}


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

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

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

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

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

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

SINT32 UnderStitch::Run(GD_VIDEO_FRAME_S img, FrameInfo para)
{
	// 快拼
	GeoStitch(img, para);


	if (_config.bUseBA)
	{
		// 预处理+缓存
		ReceiveFrame(img, para);

		// 按照需要执行优化
		//if (0)
		//{
		//	ProcessFrame(_recvFrameKey);
		//}

	}

	// todo
	// 推扫出视场，重新初始化全景图
	return 0;
}

SINT32 UnderStitch::OptAndOutCurrPan()
{
	if (_config.bUseBA)
	{
		// 优化所有帧
		ProcessFrame(_recvFrameKey);
	}

	// 输出当前所有瓦片
	_googleProduct.ExportTileSet(_panImage, _taskTilesVec, _outDir, _filename);

	return _recvFrameKey.size();
}



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

SINT32  UnderStitch::GeoStitch(GD_VIDEO_FRAME_S img, FrameInfo para)
{
	// 获取RGBA图像
	cv::Mat src = getRGBAMatFromGDFrame(img,img.u64VirAddr[0]);

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

	// 结合全景图投影H
	cv::Mat H = _H_pan * H_geo;

	TileInfo info = { 0 };
	cv::Mat Orth_Image = getOrthImage(src, para, info);

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

	//src = src.colRange(200, src.cols);

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


	cv::Mat mask = cv::Mat::ones(src.size(), CV_8UC1);

	cv::Mat warped_mask;
	cv::warpPerspective(mask, warped_mask, H, imagetmp.size(), cv::INTER_LINEAR);
	imagetmp.copyTo(_panImage, warped_mask);

	// 覆盖区域掩码更新
	_panMask = _panMask | warped_mask;


	return 0;
}

// 废弃，改为输出单帧正射后的瓦片
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);

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

	_totalFrameCnt++;

	return _totalFrameCnt;
}

SINT32 UnderStitch::ProcessFrame(vector<KeyType> keys)
{
	if (keys.size() == 0)
	{
		return -1;
	}

	// 优化所有帧
	//_BATask->OptFrame(keys, _H_pan);

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

	return 0;
}



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