#include "Arith_GeoSolver.h"
#include "Arith_UnderStitch.h"
#include "Arith_Utils.h"
#include "Arith_CoordModule.h"
#include "Arith_SysStruct.h"
using namespace cv;

GeoSolver::GeoSolver()
{
}

GeoSolver::~GeoSolver()
{
}



//cv::Point2f GeoSolver::project(cv::Point2f pos_pan, Proj m_Proj, PanInfo pan)
//{
//	cv::Point2f pos_geo = Trans_pan2Geo(pos_pan, pan);
//	cv::Point2f pos_frame = Trans_Geo2uv(pos_geo, m_Proj.tf_g2p);
//	return pos_frame;
//}
//
//cv::Point2f GeoSolver::back_project(cv::Point2f pos_frame, Proj m_Proj, PanInfo pan)
//{
//	cv::Point2f pos_geo = Trans_uv2Geo(pos_frame, m_Proj.tf_p2g);
//
//	cv::Point2f pos_pan = Trans_Geo2pan(pos_geo, pan);
//
//	return pos_pan;
//}

void GeoSolver::SetOriginPoint(FrameInfo info)
{
	originPoint = getXYZFromBLH(info.craft.stPos);
	origininfo = info;
	return;
}


PointBLH GeoSolver::getBLHFromFrame(cv::Mat H, cv::Point2f pt)
{
	cv::Point2f pt_Geo = warpPointWithH(H, pt);
	return getBLHFromGeo(pt_Geo);
}


PointBLH GeoSolver::getBLHFromGeo(cv::Point2f ptInGeo)
{
	PointXYZ ptNUE = { 0 };
	// 本模块使用的地理系是东(x)-北(y)-地(z),需要转换一下
	ptNUE.X = ptInGeo.y;
	ptNUE.Y = -origininfo.nEvHeight;
	ptNUE.Z = ptInGeo.x;
	PointBLH res = getBLHFromXYZ(getCGCSXYZFromNUEXYZ(ptNUE, originPoint));
	return res;
}

cv::Point2f GeoSolver::getGeoFromBLH(PointBLH ptPos)
{
	cv::Point2f ptInGeo = { 0 };
	PointXYZ ptNUE = { 0 }; 
	ptNUE = getNUEXYZFromCGCSXYZ(getXYZFromBLH(ptPos), originPoint);
	// 本模块使用的地理系是东(x)-北(y)-地(z),需要转换一下
	ptInGeo.x = ptNUE.Z;
	ptInGeo.y = ptNUE.X;

	return ptInGeo;
}

cv::Point2f GeoSolver::Trans_uv2Geo(cv::Point2f pos_frame, TForm form)
{
	Mat point = (Mat_<double>(3, 1) << pos_frame.x, pos_frame.y, 1);
	Mat result = form.R * point;
	// 转局部地理系
	double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
	double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);
	// 平移到原点地理系
	warpedX += form.T.at<double>(0, 0);
	warpedY += form.T.at<double>(1, 0);

	return cv::Point2f(warpedX, warpedY);
}

cv::Point2f GeoSolver::Trans_Geo2uv(cv::Point2f pos_geo, TForm form_inv)
{
	// 先平移到当前相机位置
	cv::Point2f pos_cam = pos_geo;
	pos_cam.x = pos_geo.x + form_inv.T.at<double>(0, 0);
	pos_cam.y = pos_geo.y + form_inv.T.at<double>(1, 0);

	Mat point = (Mat_<double>(3, 1) << pos_cam.x, pos_cam.y, 1);
	Mat result = form_inv.R * point;

	// 转像方
	double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
	double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);

	return cv::Point2f(warpedX, warpedY);
}





Mat GeoSolver::Mat_TransENGMove(FrameInfo info)
{
	PointXYZ ptCurr = getXYZFromBLH(info.craft.stPos);
	PointXYZ diff = getNUEXYZFromCGCSXYZ(ptCurr, originPoint);

	Mat move = Mat::zeros(3, 1, CV_64F);

	move.at<double>(0, 0) = diff.Z;
	move.at<double>(1, 0) = diff.X;
	move.at<double>(2, 0) = -diff.Y;
	return move;
}

Mat GeoSolver::Mat_TransENG2uv(FrameInfo info)
{
	//从地理坐标系转像素坐标

	//[u,v,1]'=Z*M*[X,Y,DH]' = Z*M*[1,0,0;0,1,0;0,0,DH]'*[X,Y,1]'
	//[u,v,1]'=Z*M(内参)*M(alaph)*M(beta)*M(roll)*M(pitch)*M(yaw)*[X,Y,DH]


	// 深度矩阵
	Mat M_het = (Mat_<double>(3, 3) << 1, 0, 0,
		0, 1, 0,
		0, 0, info.nEvHeight
		);



	float yaw = info.craft.stAtt.fYaw;
	Mat M_yaw = (Mat_<double>(3, 3) << cosd(yaw), -sind(yaw), 0,
		sind(yaw), cosd(yaw), 0,
		0, 0, 1
		);

	float pit = info.craft.stAtt.fPitch;
	Mat M_pitch = (Mat_<double>(3, 3) << 1, 0, 0,
		0, cosd(pit), -sind(pit),
		0, sind(pit), cosd(pit)
		);

	/* 1    0      0
	0    cos    sin
	0   -sin    cos
	*/

	float roll = info.craft.stAtt.fRoll;
	Mat M_roll = (Mat_<double>(3, 3) << cosd(roll), 0, sind(roll),
		0, 1, 0,
		-sind(roll), 0, cosd(roll)
		);


	float beta = info.servoInfo.fServoAz;

	Mat M_beta = (Mat_<double>(3, 3) << cosd(beta), -sind(beta), 0,
		sind(beta), cosd(beta), 0,
		0, 0, 1
		);


	float alaph = info.servoInfo.fServoPt;

	Mat M_alaph = (Mat_<double>(3, 3) << 1, 0, 0,
		0, cosd(alaph), -sind(alaph),
		0, sind(alaph), cosd(alaph)

		);

	// 内参
	FLOAT32 fd = info.camInfo.nFocus / info.camInfo.fPixelSize * 1000;

	Mat M_cam = (Mat_<double>(3, 3) << fd, 0, info.nWidth / 2,
		0, -fd, info.nHeight / 2,
		0, 0, 1
		);

	Mat M = M_cam * M_alaph * M_beta * M_roll * M_pitch * M_yaw * M_het;
	//cout << M_cam * M_alaph * M_beta * M_roll * M_pitch * M_yaw * M_het;
	return M;
}





Proj GeoSolver::AnlayseTform(FrameInfo info)
{
	Proj projection;
	// 从像方->地理
	projection.tf_p2g.R = Mat_TransENG2uv(info).inv();
	projection.tf_p2g.T = Mat_TransENGMove(info);

	// 从地理->像方
	projection.tf_g2p.R = Mat_TransENG2uv(info);
	projection.tf_g2p.T = -projection.tf_p2g.T;

	return projection;
}


// 检验H的计算
cv::Mat GeoSolver::findHomography2(FrameInfo info)
{
	Proj _proj = AnlayseTform(info);

	std::vector<cv::Point2f> srcPoints;
	srcPoints.push_back(cv::Point2f(0, 0));
	srcPoints.push_back(cv::Point2f(1000, 0));
	srcPoints.push_back(cv::Point2f(1000, 1000));
	srcPoints.push_back(cv::Point2f(0, 1000));

	// 同名点计算，从像方到全景
	cv::Point2f leftTop_map = Trans_uv2Geo(srcPoints[0], _proj.tf_p2g);
	cv::Point2f rightTop_map = Trans_uv2Geo(srcPoints[1], _proj.tf_p2g);
	cv::Point2f rightBottom_map = Trans_uv2Geo(srcPoints[2], _proj.tf_p2g);
	cv::Point2f leftBottom_map = Trans_uv2Geo(srcPoints[3], _proj.tf_p2g);

	// 目标图像（全景图）的四个顶点坐标
	std::vector<cv::Point2f> dstPoints;
	dstPoints.push_back(leftTop_map); // 左
	dstPoints.push_back(rightTop_map); // 右上
	dstPoints.push_back(rightBottom_map); // 右下
	dstPoints.push_back(leftBottom_map); // 左下

	// 计算单应性矩阵 H
	cv::Mat H = cv::findHomography(srcPoints, dstPoints);
	return H;
}


// 根据R和t，解析H
cv::Mat GeoSolver::findHomography(FrameInfo info)
{
	Proj _proj = AnlayseTform(info);
	TForm tform = _proj.tf_p2g;

	double R[9] = { 0 };
	double T[3] = { 0 };

	memcpy(R, tform.R.data, sizeof(double) * 9);
	memcpy(T, tform.T.data, sizeof(double) * 3);



	Mat H = (Mat_<double>(3, 3) << (R[0] + T[0] * R[6]), (R[1] + T[0] * R[7]), (R[2] + T[0] * R[8]),
		(R[3] + T[1] * R[6]), (R[4] + T[1] * R[7]), (R[5] + T[1] * R[8]),
		R[6], R[7], R[8]);

	// 归一化
	H = H / H.at<double>(2, 2);


	return H;
}


// 计算多边形的面积
double polygonArea(const vector<cv::Point2f>& points)
{
	double area = 0.0;
	int n = points.size();
	for (int i = 0; i < n; i++) {
		int j = (i + 1) % n;
		area += points[i].x * points[j].y - points[j].x * points[i].y;
	}
	return abs(area) / 2.0;
}

// 计算两个四边形的 IOU
double computeQuadrilateralIOU(const vector<cv::Point2f>& quad1, const vector<cv::Point2f>& quad2)
{
	// 将四边形转换为多边形
	vector<Point2f> poly1 = quad1;
	vector<Point2f> poly2 = quad2;

	// 计算交集多边形
	vector<Point2f> intersectionPolygon;
	intersectConvexConvex(poly1, poly2, intersectionPolygon);

	// 计算面积
	double area1 = polygonArea(poly1);
	double area2 = polygonArea(poly2);
	double intersectionArea = polygonArea(intersectionPolygon);
	double unionArea = area1 + area2 - intersectionArea;

	// 计算 IOU
	if (unionArea == 0) return 0.0; // 避免除零错误
	return intersectionArea / unionArea;
}

cv::Point2f warpPointWithH(cv::Mat H, cv::Point2f srcPt)
{
	Mat point = (Mat_<double>(3, 1) << srcPt.x, srcPt.y, 1);
	Mat result = H * point;
	// 转局部地理系
	double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
	double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);

	return cv::Point2f(warpedX,warpedY);
}

vector<cv::Point2f> warpRectWithH(cv::Mat H,cv::Size size)
{
	vector<cv::Point2f> _res;
	_res.push_back(warpPointWithH(H, cv::Point2f(0,0)));
	_res.push_back(warpPointWithH(H, cv::Point2f(size.width,0)));
	_res.push_back(warpPointWithH(H, cv::Point2f(size.width,size.height)));
	_res.push_back(warpPointWithH(H, cv::Point2f(0,size.height)));
	return _res;
}