#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "planeLocalization_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>

//version 1.0.0 : base version release to customer
//version 1.1.0 : 优化了机鼻点提取（迭代），增加了没有飞机的输出
//version 1.2.0 : 修正了回归测试中发现的问题
//version 1.3.0 : 修正了取发动机点特征点的一个Bug
//version 1.3.1 : 改进了取发动机点特征点算法
std::string	m_strVersion = " PlaneLocalization 1.3.1";
const char* wd_PlaneLocalizationVersion(void)
{
	return m_strVersion.c_str();
}

//使用RANSAC方法云计算地面参数。
SSG_planeCalibPara wd_getGroundCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	//设置初始结果
	double initCalib[9] = {
		1.0, 0.0, 0.0,
		0.0, 1.0, 0.0,
		0.0, 0.0, 1.0 };
	SSG_planeCalibPara groundParam;
	for (int i = 0; i < 9; i++)
	{
		groundParam.planeCalib[i] = initCalib[i];
		groundParam.invRMatrix[i] = initCalib[i];
	}
	groundParam.planeHeight = -1.0;

	std::vector<cv::Point3d> points;
	for (int line = 0; line < (int)scanLines.size(); line++)
	{
		for (int j = 0, j_max = (int)scanLines[line].size(); j < j_max; j++)
		{
			if ( (scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z <200000)) //200米内的数据
			{
				cv::Point3d a_pt = cv::Point3d(scanLines[line][j].pt3D.x, scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.z);
				points.push_back(a_pt);
			}
		}
	}

	double distTh = 1.0;
	std::vector<cv::Point3d> in_inliers;
	Plane groundPlane = ransacFitPlane(points, in_inliers, distTh);
	if (in_inliers.size() < 100)
	{
		return groundParam;
	}

	SVzNL3DPoint vec_1;
	if (groundPlane.B > 0)
		vec_1 = { groundPlane.A, groundPlane.B, groundPlane.C };
	else
		vec_1 = { -groundPlane.A, -groundPlane.B, -groundPlane.C };

	SVzNL3DPoint vec_z = { 0, 1.0, 0.0 };
	groundParam = wd_computeRTMatrix(vec_1, vec_z);

	//计算地面高度
	std::vector<cv::Point3d> groundPoints;
	for (int i = 0; i < (int)in_inliers.size(); i++)
	{
		cv::Point3d rPt;
		rPt.x = in_inliers[i].x * groundParam.planeCalib[0] + in_inliers[i].y * groundParam.planeCalib[1] + in_inliers[i].z * groundParam.planeCalib[2];
		rPt.y = in_inliers[i].x * groundParam.planeCalib[3] + in_inliers[i].y * groundParam.planeCalib[4] + in_inliers[i].z * groundParam.planeCalib[5];
		rPt.z = in_inliers[i].x * groundParam.planeCalib[6] + in_inliers[i].y * groundParam.planeCalib[7] + in_inliers[i].z * groundParam.planeCalib[8];
		groundPoints.push_back(rPt);
	}
	double groundY = 0;
	for (int i = 0; i < (int)groundPoints.size(); i++)
		groundY += groundPoints[i].y;
	groundY = groundY / (int)groundPoints.size();
	groundParam.planeHeight = groundY;
	return groundParam;
}

int _checkAdjacency(
	std::vector< SVzNL3DPosition>& cluster_1, 
	const int cluster1_idx,  const int cluster2_idx,
	std::vector<std::vector<int>>& clusterMask)
{
	int lineNum = (int)clusterMask.size();
	int linePtNum = (int)clusterMask[0].size();
	int cluster1_size = (int)cluster_1.size();

	int adjacentNum = 0;
	for (int i = 0; i < cluster1_size; i++)
	{
		SVzNL3DPosition& a_pt = cluster_1[i];
		//检查相邻
		int lineIdx = a_pt.nPointIdx >> 16;
		int ptIdx = a_pt.nPointIdx & 0xFFFF;
		if (clusterMask[lineIdx][ptIdx] != cluster1_idx)
			continue;

		int leftCol = lineIdx - 1;
		int rightCol = lineIdx + 1;
		int topRow = ptIdx - 1;
		int btmRow = ptIdx + 1;
		if ((leftCol < 0) || (rightCol >= lineNum) || (topRow < 0) || (btmRow >= linePtNum))
			continue;

		if (clusterMask[leftCol][ptIdx] == cluster2_idx)
			adjacentNum++;
		if (clusterMask[rightCol][ptIdx] == cluster2_idx)
			adjacentNum++;
		if (clusterMask[lineIdx][topRow] == cluster2_idx)
			adjacentNum++;
		if (clusterMask[lineIdx][btmRow] == cluster2_idx)
			adjacentNum++;
	}
	return adjacentNum;
}

void _searchSuperCluster(
	std::vector<int>& a_superCluster, 
	std::vector<std::vector<int>>& adjacencyScoreTable, 
	std::vector<int>& clusterFlags, 
	std::vector<SVzNL3DRangeD>& clusterROIs,
	const int adjacencyScoreTh, 
	const double adjacencyZDistTh)
{
	int clusterNum = adjacencyScoreTable.size();

	int searchIdx = 0;
	int seedIdx = a_superCluster[0];
	clusterFlags[seedIdx] = 1;
	while (searchIdx < a_superCluster.size())
	{
		int clusterIdx = a_superCluster[searchIdx];
		SVzNL3DRangeD& seedROI = clusterROIs[clusterIdx];
		for (int i = 0; i < clusterNum; i++)
		{
			if ((clusterFlags[i] > 0) || (i == seedIdx))
				continue;

			int score = adjacencyScoreTable[clusterIdx][i];
			if (score > adjacencyScoreTh)
			{
				a_superCluster.push_back(i);
				clusterFlags[i] = 1;
			}
			else //if (score > 0)
			{
				//计算距离:使用Z距离 
				SVzNL3DRangeD& chkROI = clusterROIs[i];
				double zDist;
				if (seedROI.zRange.min > chkROI.zRange.max)
					zDist = seedROI.zRange.min - chkROI.zRange.max;
				else if (chkROI.zRange.min > seedROI.zRange.max)
					zDist = chkROI.zRange.min - seedROI.zRange.max;
				else
					zDist = 0; //重叠
				
				if (zDist < adjacencyZDistTh)
				{
					a_superCluster.push_back(i);
					clusterFlags[i] = 1;
				}
			}
		}
		searchIdx++;
	}
}

void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt)
{
	if (a_pt.z > 1E-4)
	{
		if (roi.zRange.max < 0)
		{
			roi.xRange.min = a_pt.x;
			roi.xRange.max = a_pt.x;
			roi.yRange.min = a_pt.y;
			roi.yRange.max = a_pt.y;
			roi.zRange.min = a_pt.z;
			roi.zRange.max = a_pt.z;
		}
		else
		{
			if (roi.xRange.min > a_pt.x)
				roi.xRange.min = a_pt.x;
			if (roi.xRange.max < a_pt.x)
				roi.xRange.max = a_pt.x;
			if (roi.yRange.min > a_pt.y)
				roi.yRange.min = a_pt.y;
			if (roi.yRange.max < a_pt.y)
				roi.yRange.max = a_pt.y;
			if (roi.zRange.min > a_pt.z)
				roi.zRange.min = a_pt.z;
			if (roi.zRange.max < a_pt.z)
				roi.zRange.max = a_pt.z;
		}
	}
	return;
}

SVzNL3DRangeD _getSupcluster(std::vector<int>& superClusterIndice, std::vector<SVzNL3DRangeD>& objClustersROIs)
{
	int idx_0 = superClusterIndice[0];
	SVzNL3DRangeD mergeROI = objClustersROIs[idx_0];
	for (int i = 1; i < (int)superClusterIndice.size(); i++)
	{
		int idx = superClusterIndice[i];
		SVzNL3DRangeD& a_roi = objClustersROIs[idx];
		mergeROI.xRange.min = mergeROI.xRange.min > a_roi.xRange.min ? a_roi.xRange.min : mergeROI.xRange.min;
		mergeROI.xRange.max = mergeROI.xRange.max < a_roi.xRange.max ? a_roi.xRange.max : mergeROI.xRange.max;
		mergeROI.yRange.min = mergeROI.yRange.min > a_roi.yRange.min ? a_roi.yRange.min : mergeROI.yRange.min;
		mergeROI.yRange.max = mergeROI.yRange.max < a_roi.yRange.max ? a_roi.yRange.max : mergeROI.yRange.max;
		mergeROI.zRange.min = mergeROI.zRange.min > a_roi.zRange.min ? a_roi.zRange.min : mergeROI.zRange.min;
		mergeROI.zRange.max = mergeROI.zRange.max < a_roi.zRange.max ? a_roi.zRange.max : mergeROI.zRange.max;
	}
	return mergeROI;
}

//判断聚类的最近点是否可能是机鼻点。方法：以最近点搜索在距离范围内的点（指向机身），计算机身高度
bool _checkNosePoint(
	std::vector<SVzNL3DPosition>& a_cluster, 
	SVzNL3DPosition& nearestPoint, 
	SVzNLRangeD& distChkRange, 
	const double bodyHeight)
{
	nearestPoint.nPointIdx = 0;
	nearestPoint.pt3D = { 0.0, 0.0, 0.0 };
	for (int i = 0; i < (int)a_cluster.size(); i++)
	{
		if (nearestPoint.pt3D.z < 1e-4)
			nearestPoint = a_cluster[i];
		else if (nearestPoint.pt3D.z > a_cluster[i].pt3D.z)
			nearestPoint = a_cluster[i];
	}

	//判断是否是机鼻点
	std::vector<SVzNL3DPoint> chkPoints;
	for (int i = 0; i < (int)a_cluster.size(); i++)
	{
		double dist = sqrt(pow(a_cluster[i].pt3D.x - nearestPoint.pt3D.x, 2) + pow(a_cluster[i].pt3D.z - nearestPoint.pt3D.z, 2));
		if ((dist >= distChkRange.min) && (dist <= distChkRange.max))
			chkPoints.push_back(a_cluster[i].pt3D);
	}
	if (chkPoints.size() < 2)
		return false;

	SVzNLRangeD yRange = { chkPoints[0].y, chkPoints[0].y };
	for (int i = 1; i < (int)chkPoints.size(); i++)
	{
		yRange.min = yRange.min > chkPoints[i].y ? chkPoints[i].y : yRange.min;
		yRange.max = yRange.max < chkPoints[i].y ? chkPoints[i].y : yRange.max;
	}
	
	double height = yRange.max - yRange.min;
	if (height > bodyHeight)
		return true;
	return false;
}

SSX_planeInfo wd_planeLocalization(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_planeCalibPara groundCalibParam,
	const SSX_planeParkingParam parkingParam,
	const SSG_treeGrowParam growParam,
#if _OUTPUT_DEBUG_DATA
	std::vector< std::vector<SVzNL3DPosition>>& debugData,
#endif
	int* errCode)
{
	*errCode = 0;
	//内部参数
	double planeMinHeight = 4000; //最小高度4米
	double planeMinWidth = 2500; //最小宽度2.5米
	double nearFarTh = 80000.0; //远近分界
	double groundHOffset = 200; //去除地面参数
	double bodyHeadJudge_lenTh = 20000.0; //机头与机身判断的长度门限。20米
	SVzNLRangeD bodyRangeToNose = { -2000.0, 1000.0 }; //以机鼻高度为基准，定义机身的高度范围
	SVzNLRangeD sameRBodyRange = { 5000.0, 28000.0 }; //机身圆桶段，以距离机鼻距离为基准
	SVzNLRangeD engineToNoseDistRange = { 12000.0, 16000.0 };
	SVzNLRangeD bodyYRange = { groundCalibParam.planeHeight - 4500.0, groundCalibParam.planeHeight - 1500 }; //机身的Y范围
	SVzNLRangeD nearestPointNostChkDistRange = { 3500.0, 4500.0 };
	double minBodyHeight = 2000.0;

	SSX_planeInfo planePoseInfo;
	memset(&planePoseInfo, 0, sizeof(SSX_planeInfo));
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return planePoseInfo;
	}

	int linePtNum = (int)scanLines[0].size();
	//判断数据格式是否为grid。算法只能处理grid数据格式
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		if (linePtNum != (int)scanLines[line].size())
		{
			isGridData = false;
			break;
		}
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return planePoseInfo;
	}

	//将引导线指向Y轴
	SVzNL3DPoint guideLineVec = { parkingParam.guideLinePoint.x - parkingParam.parkingPoint.x , 0, parkingParam.guideLinePoint.z - parkingParam.parkingPoint.z};
	if (guideLineVec.z < 0)
		guideLineVec = {-guideLineVec.x, -guideLineVec.y, -guideLineVec.z};
	guideLineVec = vec3_normalize(guideLineVec);
	SVzNL3DPoint targetVec = { 0, 0, 1 };
	SSG_planeCalibPara guideLineRotatePara = wd_computeRTMatrix(guideLineVec, targetVec);

	//对正引导线
	SVzNL3DPoint rotateParkingPoint = wd_ptRotate(parkingParam.parkingPoint, guideLineRotatePara.planeCalib);
	SVzNL3DPoint chkPoint = wd_ptRotate(parkingParam.guideLinePoint, guideLineRotatePara.planeCalib);
	//计算目标范围
	SVzNLRangeD ROI_x_near = { rotateParkingPoint.x - parkingParam.parkingRange, rotateParkingPoint.x + parkingParam.parkingRange };
	SVzNLRangeD ROI_x_far = { rotateParkingPoint.x - parkingParam.parkingRange * 3, rotateParkingPoint.x + parkingParam.parkingRange* 3 };  //大于100m外，需要放宽，因为飞机正在进引导线
	SVzNLRangeD ROI_z = { rotateParkingPoint.z - parkingParam.distFromNoseToWheel - 2000.0, rotateParkingPoint.z + parkingParam.guidingRange };
	//旋转， ROI过滤

	std::vector< std::vector<SVzNL3DPosition>> rotateROIData;
	rotateROIData.resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		rotateROIData[line].resize(linePtNum);
		for (int j = 0; j < linePtNum; j++)
		{
			SVzNL3DPosition a_pt;
			a_pt.nPointIdx = 0;
			a_pt.pt3D = wd_ptRotate(scanLines[line][j].pt3D, groundCalibParam.planeCalib);
			if (a_pt.pt3D.y >= (groundCalibParam.planeHeight - groundHOffset))
				a_pt.pt3D = { 0.0, 0.0, 0.0 };

			//再次旋转，将引导线指向Y轴
			a_pt.pt3D = wd_ptRotate(a_pt.pt3D, guideLineRotatePara.planeCalib);
			rotateROIData[line][j] = a_pt;
		}
	}

	std::vector< std::vector<SWD_poloarScan2D>> polarScanData; //扫描数据：扫描水平角、垂直角和距离,
	polarScanData.resize(lineNum);
	std::vector<SVzNL3DPosition> validPoints;
	for (int line = 0; line < lineNum; line++)
	{
		polarScanData[line].resize(rotateROIData[line].size());
		for (int j = 0, j_max = (int)rotateROIData[line].size(); j < j_max; j++)
		{
			if (rotateROIData[line][j].pt3D.z > 1e-4)  //去除地面后的数据
			{
				SVzNL3DPosition a_pt;
				a_pt.nPointIdx = (line << 16) | (j & 0xffff);
				a_pt.pt3D = rotateROIData[line][j].pt3D;
				validPoints.push_back(a_pt);

				//计算扫描角度：使用旋转前数据
				a_pt = scanLines[line][j];
				SWD_poloarScan2D a_polarData;
				a_polarData.range_mm = sqrt(pow(a_pt.pt3D.x, 2) + pow(a_pt.pt3D.y, 2) + pow(a_pt.pt3D.z, 2));
				//水平角：
				double yaw_rad = atan2(-a_pt.pt3D.x, a_pt.pt3D.z);
				a_polarData.yaw_deg = yaw_rad * 180 / PI;
				//垂直角：
				double pitch_rad = asin(-a_pt.pt3D.y / a_polarData.range_mm);
				a_polarData.pitch_deg = pitch_rad * 180 / PI;
				polarScanData[line][j] = a_polarData;
			}
			else
			{
				polarScanData[line][j].range_mm = 0;
				polarScanData[line][j].yaw_deg = 0;
				polarScanData[line][j].pitch_deg = 0;
			}
			scanLines[line][j].nPointIdx = 0;
		}
	}

#if _OUTPUT_DEBUG_DATA
	//输出投影数据
	debugData.clear();
	debugData.resize(scanLines.size());
	for (int line = 0; line < lineNum; line++)
	{
		debugData[line].resize(scanLines[line].size());
		for (int j = 0, j_max = (int)scanLines[line].size(); j < j_max; j++)
		{
			debugData[line][j].nPointIdx = 0;
			debugData[line][j].pt3D = rotateROIData[line][j].pt3D;
		}
	}
#endif

	//聚类
	//使用SVzNL3DPosition的nPointIdx表示2D信息（高16位Line， 低16位ptIdx）
	//搜索时搜索邻域以加速
	int clusterCheckWin = 5;
	double clusterDist = sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2));
	int distType = 1;  //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> allClusters;
	wd_pointClustering_speedUp(
		validPoints,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType, //0 - 2d distance; 1- 3d distance 
		allClusters  //result
	);

	//统计cluster的ROI
	std::vector<SVzNL3DRangeD> allClusterROIs;
	allClusterROIs.resize(allClusters.size());
	for (int m = 0; m < (int)allClusters.size(); m++)
	{
		SVzNL3DRangeD a_roi3D;
		memset(&a_roi3D, 0, sizeof(SVzNL3DRangeD));
		a_roi3D.zRange.max = -1;
		for (int i = 0; i < (int)allClusters[m].size(); i++)
			_updateRoi3D(a_roi3D, allClusters[m][i].pt3D);
		allClusterROIs[m] = a_roi3D;
	}

	//ROI过滤
	std::vector<std::vector< SVzNL3DPosition>> objClusters;
	std::vector<SVzNL3DRangeD> objClustersROIs;
	for (int m = 0; m < (int)allClusters.size(); m++)
	{
		SVzNL3DRangeD& a_roi = allClusterROIs[m];
		double height = groundCalibParam.planeHeight - a_roi.yRange.min;
		if (height >= 2000.00)  //2米高度门限， 跳过一般人身高
		{
			if ((a_roi.zRange.min - rotateParkingPoint.z) < nearFarTh)
			{
				if ((a_roi.xRange.min >= ROI_x_near.min) && (a_roi.xRange.max <= ROI_x_near.max) &&
					(a_roi.zRange.min >= ROI_z.min) && (a_roi.zRange.max <= ROI_z.max))
				{
					objClusters.push_back(allClusters[m]);
					objClustersROIs.push_back(a_roi);
				}
			}
			else
			{
				if ((a_roi.xRange.min >= ROI_x_far.min) && (a_roi.xRange.max <= ROI_x_far.max) &&
					(a_roi.zRange.min >= ROI_z.min) && (a_roi.zRange.max <= ROI_z.max))
				{
					objClusters.push_back(allClusters[m]);
					objClustersROIs.push_back(a_roi);
				}
			}
		}
	}

	//分析聚类之间的关系
	//建立聚类Mask
	int clusterNum = (int)objClusters.size();
	std::vector<std::vector<int>> clusterMask;
	clusterMask.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		clusterMask[i].resize(linePtNum);
		std::fill(clusterMask[i].begin(), clusterMask[i].end(), -1);
	}
	for (int i = 0; i < clusterNum; i++)
	{
		std::vector< SVzNL3DPosition>& a_cluster = objClusters[i];
		int ptSize = (int)a_cluster.size();
		for (int j = 0; j < ptSize; j++)
		{
			SVzNL3DPosition& a_pt = a_cluster[j];
			int lineIdx = a_pt.nPointIdx >> 16;
			int ptIdx = a_pt.nPointIdx & 0xffff;
			clusterMask[lineIdx][ptIdx] = i;
		}
	}
	//构建聚类的相邻关系表
	std::vector<std::vector<int>> adjacencyScoreTable;
	adjacencyScoreTable.resize(clusterNum);
	for (int i = 0; i < clusterNum; i++)
		adjacencyScoreTable[i].resize(clusterNum);

	for (int i = 0; i < clusterNum; i++)
	{
		std::vector< SVzNL3DPosition>& cluster_1 = objClusters[i];
		for (int j = i + 1; j < clusterNum; j++)
		{
			int score = _checkAdjacency(cluster_1, i, j, clusterMask);
			adjacencyScoreTable[i][j] = score;
			adjacencyScoreTable[j][i] = score;  
		}
	}

	//二次聚类成超聚类
	int adjacencyScoreTh = 10;
	double adjacencyZDistTh = 18000.0;//18米长
	std::vector<std::vector<int>> superClusers;  //超级聚类，使用聚类的ID
	std::vector<int> clusterFlags;
	clusterFlags.resize(objClusters.size());
	std::fill(clusterFlags.begin(), clusterFlags.end(), -1);
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		if (clusterFlags[i] >= 0)
			continue;

		std::vector<int> a_superCluster;
		a_superCluster.push_back(i);
		//递归搜索
		_searchSuperCluster(a_superCluster, adjacencyScoreTable, clusterFlags, objClustersROIs, adjacencyScoreTh, adjacencyZDistTh);
		
		//计算ROI
		SVzNL3DRangeD superROI = _getSupcluster(a_superCluster, objClustersROIs);
		double h = abs(superROI.yRange.min - groundCalibParam.planeHeight);
		double w = superROI.xRange.max - superROI.xRange.min;
		double len = superROI.zRange.max - superROI.zRange.min;

		if( (h > planeMinHeight) && (w > planeMinWidth))
			superClusers.push_back(a_superCluster);
	}

#if _OUTPUT_DEBUG_DATA
	//标注
	for (int i = 0; i < (int)superClusers.size(); i++)
	{
		std::vector<int>& a_superCluster = superClusers[i];
		int memberSize = (int)a_superCluster.size();
		for (int m = 0; m < memberSize; m++)
		{
			int clusterIdx = a_superCluster[m];
			std::vector< SVzNL3DPosition>& a_cluster = objClusters[clusterIdx];
			int ptSize = (int)a_cluster.size();
			for (int j = 0; j < ptSize; j++)
			{
				SVzNL3DPosition& a_pt = a_cluster[j];
				int lineIdx = a_pt.nPointIdx >> 16;
				int ptIdx = a_pt.nPointIdx & 0xffff;
				debugData[lineIdx][ptIdx].nPointIdx += (i + 1);
			}
		}
	}
#endif

	if (superClusers.size() == 0)
	{
		*errCode = SX_ERR_NO_PLANE_FOUND;
		return planePoseInfo;
	}

	//挑选出点数最多的超聚类作为飞机
	std::vector<int> superCluserPointSizes;
	superCluserPointSizes.resize(superClusers.size());
	for (int i = 0; i < (int)superClusers.size(); i++)
	{
		std::vector<int>& a_superCluster = superClusers[i];
		int superSize = 0;
		int memberSize = (int)a_superCluster.size();
		for (int m = 0; m < memberSize; m++)
		{
			int clusterIdx = a_superCluster[m];
			std::vector< SVzNL3DPosition>& a_cluster = objClusters[clusterIdx];
			superSize += (int)a_cluster.size();
		}
		superCluserPointSizes[i] = superSize;
	}

	int bestId = 0;
	for (int i = 1; i < (int)superCluserPointSizes.size(); i++)
	{
		if (superCluserPointSizes[bestId] < superCluserPointSizes[i])
			bestId = i;
	}

	std::vector<int>& planeSuperCluster = superClusers[bestId];
	int planeClusterSize = (int)planeSuperCluster.size();
	//寻找机头位置: ROI最靠前（Z最小）
	int noseClusterId = -1;
	SVzNL3DPosition nosePoint;
	nosePoint.nPointIdx = 0;
	nosePoint.pt3D = { 0.0, 0.0, 0.0 };
	for (int i = 0; i < planeClusterSize; i++)
	{
		int clusterId = planeSuperCluster[i];
		SVzNL3DRangeD& a_roi = objClustersROIs[clusterId];
		if ((a_roi.yRange.min < bodyYRange.min) && (a_roi.yRange.max > bodyYRange.max))  //处于机身的Y范围内
		{
			SVzNL3DPosition nearestPoint;
			bool validNoseCluster = _checkNosePoint(
				objClusters[clusterId],
				nearestPoint,
				nearestPointNostChkDistRange,
				minBodyHeight);
			if (true == validNoseCluster)
			{
				if (noseClusterId < 0)
				{
					noseClusterId = clusterId;
					nosePoint = nearestPoint;
				}
				else if (nosePoint.pt3D.z > nearestPoint.pt3D.z)
				{
					noseClusterId = clusterId;
					nosePoint = nearestPoint;
				}
			}
		}
	}
	if (noseClusterId < 0)
	{
		*errCode = SX_ERR_NOSEPOINT_FAIL;
		return planePoseInfo;
	}

	//计算姿态
	//判断机鼻所在的长度：以机鼻高度为基准，取高度范围内的点，计算长度
	SVzNLRangeD bodyHRange = { nosePoint.pt3D.y + bodyRangeToNose.min, nosePoint.pt3D.y + bodyRangeToNose.max };
	std::vector<SVzNL3DPosition> bodyData;
	for (int i = 0; i < (int)objClusters[noseClusterId].size(); i++)
	{
		if ((objClusters[noseClusterId][i].pt3D.y >= bodyHRange.min) && (objClusters[noseClusterId][i].pt3D.y <= bodyHRange.max))
			bodyData.push_back(objClusters[noseClusterId][i]);
	}
	//计算到机鼻最长距离
	double maxDistanceToNose = 0;
	std::vector<SVzNL2DPointD> XOZBodayProjectionData;
	std::vector<SVzNL3DPosition> XOZBodayData;
	for (int i = 0; i < (int)bodyData.size(); i++)
	{
		double dist = sqrt(pow(nosePoint.pt3D.x - bodyData[i].pt3D.x, 2) + pow(nosePoint.pt3D.z - bodyData[i].pt3D.z, 2));
		if (maxDistanceToNose < dist)
			maxDistanceToNose = dist;

		if ((dist >= sameRBodyRange.min) && (dist <= sameRBodyRange.max))
		{
			XOZBodayData.push_back(bodyData[i]);

			SVzNL2DPointD a_projection = { bodyData[i].pt3D.x, bodyData[i].pt3D.z };
			XOZBodayProjectionData.push_back(a_projection);
		}
	}
	if (maxDistanceToNose > bodyHeadJudge_lenTh)  //机身
	{
		//取机身数据，PCA方法计算姿态
		// 二维PCA，求平面内条纹方向
		SVzNL2DPointD axis, centroid;
		pca2D(XOZBodayProjectionData, axis, centroid);
		if (axis.y < 0)
			axis = { -axis.x, -axis.y };

		//以centrod为基准，计算到centroid最远的点为机鼻点
		double maxDistance = 0;
		SVzNL3DPosition bestPoint; 
		for (int m = 0; m < (int)objClusters[noseClusterId].size(); m++)
		{
			if (objClusters[noseClusterId][m].pt3D.z < centroid.y)
			{
				double dist = sqrt(pow(objClusters[noseClusterId][m].pt3D.x - centroid.x, 2) + pow(objClusters[noseClusterId][m].pt3D.z - centroid.y, 2));
				if (maxDistance < dist)
				{
					maxDistance = dist;
					bestPoint = objClusters[noseClusterId][m];
				}
			}
		}
		if(maxDistance > 1e-4)  //迭代计算机鼻点
			nosePoint = bestPoint;

		double dirAngle = atan(axis.x / axis.y) * 180.0 / PI;
		planePoseInfo.nosePoint = nosePoint.pt3D;
		planePoseInfo.axis = { axis.x, 0, axis.y };
		planePoseInfo.axis = vec3_normalize(planePoseInfo.axis);
		planePoseInfo.distance = nosePoint.pt3D.z - rotateParkingPoint.z + parkingParam.distFromNoseToWheel;
		planePoseInfo.deviation = nosePoint.pt3D.x - rotateParkingPoint.x;  //偏离引导线
		planePoseInfo.dirAngle_deg = dirAngle; //方向角度

#if _OUTPUT_DEBUG_DATA
		{
			int nose_lineIdx = nosePoint.nPointIdx >> 16;
			int nose_ptIdx = nosePoint.nPointIdx & 0xffff;
			debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x10000;  //机鼻点
		}
		for(int m = 0; m <(int)XOZBodayData.size(); m ++)
		{
			int nose_lineIdx = XOZBodayData[m].nPointIdx >> 16;
			int nose_ptIdx = XOZBodayData[m].nPointIdx & 0xffff;
			if( (debugData[nose_lineIdx][nose_ptIdx].nPointIdx & 0xffff0000) == 0)
				debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x20000;  //机身点
		}
#endif
	}
	else  //机头
	{
		//取左右发动机数据
		std::vector<std::vector<SVzNL3DPosition>> distanceValidData;
		distanceValidData.resize(planeClusterSize);
		for (int idx = 0; idx < planeClusterSize; idx++)
		{
			int clusterIdx = planeSuperCluster[idx];
			if (clusterIdx == noseClusterId)
				continue;

			for (int i = 0; i < (int)objClusters[clusterIdx].size(); i++)
			{
				if (objClusters[clusterIdx][i].pt3D.z > nosePoint.pt3D.z)
				{
					double dist = sqrt(pow(nosePoint.pt3D.x - objClusters[clusterIdx][i].pt3D.x, 2) + pow(nosePoint.pt3D.z - objClusters[clusterIdx][i].pt3D.z, 2));
					if ((dist >= engineToNoseDistRange.min) && (dist <= engineToNoseDistRange.max))
						distanceValidData[idx].push_back(objClusters[clusterIdx][i]);
				}
			}
		}
		//计算ROI
		std::vector<SVzNLRangeD> dataROIs;
		std::vector<int> validFlags;
		dataROIs.resize(planeClusterSize);
		validFlags.resize(planeClusterSize);
		int validNum = 0;
		for (int idx = 0; idx < planeClusterSize; idx++)
		{
			if (distanceValidData[idx].size() == 0)
			{
				validFlags[idx] = 0;
				dataROIs[idx] = { 0.0, 0.0 };
			}
			else
			{
				validFlags[idx] = 1;
				validNum++;
				SVzNLRangeD xRng = { distanceValidData[idx][0].pt3D.x,distanceValidData[idx][0].pt3D.x };
				for (int i = 1; i < (int)distanceValidData[idx].size(); i++)
				{
					xRng.min = xRng.min > distanceValidData[idx][i].pt3D.x ? distanceValidData[idx][i].pt3D.x : xRng.min;
					xRng.max = xRng.max < distanceValidData[idx][i].pt3D.x ? distanceValidData[idx][i].pt3D.x : xRng.max;
				}
				dataROIs[idx] = xRng;
			}
		}
		if (validNum < 2)
		{
			*errCode = SX_ERR_ENDINE_FAIL;
			return planePoseInfo;
		}

		//取左右Engine
		int leftEngineIdx = -1;
		int rightEngineIdx = -1;
		for (int idx = 0; idx < planeClusterSize; idx++)
		{
			if (validFlags[idx] == 0)
				continue;

			if (leftEngineIdx < 0)
				leftEngineIdx = idx;
			else if (dataROIs[leftEngineIdx].min > dataROIs[idx].min)
				leftEngineIdx = idx;

			if (rightEngineIdx < 0)
				rightEngineIdx = idx;
			else if (dataROIs[rightEngineIdx].max < dataROIs[idx].max)
				rightEngineIdx = idx;
		}

		if (leftEngineIdx == rightEngineIdx)
		{
			*errCode = SX_ERR_ENDINE_FAIL;
			return planePoseInfo;
		}

		//取左右点集的最低点，作为左右发动机的参考点


		SVzNL3DPosition leftNearestPoint = distanceValidData[leftEngineIdx][0];
		for (int i = 1; i < (int)distanceValidData[leftEngineIdx].size(); i++)
		{
			if (leftNearestPoint.pt3D.z > distanceValidData[leftEngineIdx][i].pt3D.z)
				leftNearestPoint = distanceValidData[leftEngineIdx][i];
		}
		std::vector<SVzNL3DPosition> leftEengineData;

		for (int i = 0; i < (int)distanceValidData[leftEngineIdx].size(); i++)
		{
			double zDiff = distanceValidData[leftEngineIdx][i].pt3D.z - leftNearestPoint.pt3D.z;
			if (zDiff < 500.0) //0.5米范围内
				leftEengineData.push_back(distanceValidData[leftEngineIdx][i]);
		}
		SVzNL3DPosition leftEnginePoint = leftEengineData[0];
		SVzNL3DPosition leftCentroid = { 0, {0, 0, 0} };
		for (int i = 1; i < (int)leftEengineData.size(); i++)
		{
			if (leftEnginePoint.pt3D.y < leftEengineData[i].pt3D.y)
				leftEnginePoint = leftEengineData[i];
			leftCentroid.pt3D.x += leftEengineData[i].pt3D.x;
			leftCentroid.pt3D.y += leftEengineData[i].pt3D.y;
			leftCentroid.pt3D.z += leftEengineData[i].pt3D.z;
		}
		int leftEngineDataSize = (int)leftEengineData.size();
		leftCentroid.pt3D.x = leftCentroid.pt3D.x / leftEngineDataSize;
		leftCentroid.pt3D.y = leftCentroid.pt3D.y / leftEngineDataSize;
		leftCentroid.pt3D.z = leftCentroid.pt3D.z / leftEngineDataSize;

		SVzNL3DPosition rightNearestPoint = distanceValidData[rightEngineIdx][0];
		for (int i = 1; i < (int)distanceValidData[rightEngineIdx].size(); i++)
		{
			if (rightNearestPoint.pt3D.z > distanceValidData[rightEngineIdx][i].pt3D.z)
				rightNearestPoint = distanceValidData[rightEngineIdx][i];
		}
		std::vector<SVzNL3DPosition> rightEngineData;
		for (int i = 0; i < (int)distanceValidData[rightEngineIdx].size(); i++)
		{
			double zDiff = distanceValidData[rightEngineIdx][i].pt3D.z - rightNearestPoint.pt3D.z;
			if (zDiff < 500.0) //0.5米范围内
				rightEngineData.push_back(distanceValidData[rightEngineIdx][i]);
		}

		SVzNL3DPosition rightEnginePoint = rightEngineData[0];
		SVzNL3DPosition rightCentroid = { 0, {0, 0, 0} };
		for (int i = 1; i < (int)rightEngineData.size(); i++)
		{
			if (rightEnginePoint.pt3D.y < rightEngineData[i].pt3D.y)
				rightEnginePoint = rightEngineData[i];

			rightCentroid.pt3D.x += rightEngineData[i].pt3D.x;
			rightCentroid.pt3D.y += rightEngineData[i].pt3D.y;
			rightCentroid.pt3D.z += rightEngineData[i].pt3D.z;
		}
		int rightEngineDataSize = (int)rightEngineData.size();
		rightCentroid.pt3D.x = rightCentroid.pt3D.x / rightEngineDataSize;
		rightCentroid.pt3D.y = rightCentroid.pt3D.y / rightEngineDataSize;
		rightCentroid.pt3D.z = rightCentroid.pt3D.z / rightEngineDataSize;
#if _OUTPUT_DEBUG_DATA
		{
			int nose_lineIdx = nosePoint.nPointIdx >> 16;
			int nose_ptIdx = nosePoint.nPointIdx & 0xffff;
			debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x10000;  //机鼻点
		}
		{
			int nose_lineIdx = leftEnginePoint.nPointIdx >> 16;
			int nose_ptIdx = leftEnginePoint.nPointIdx & 0xffff;
			debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x40000;  //机鼻点

			nose_lineIdx = rightEnginePoint.nPointIdx >> 16;
			nose_ptIdx = rightEnginePoint.nPointIdx & 0xffff;
			debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x40000;  //机鼻点
		}
#endif
		leftEnginePoint.pt3D = leftCentroid.pt3D;
		rightEnginePoint.pt3D = rightCentroid.pt3D;

		//计算姿态
		//axis与左右发动机连续垂直, 采用(-y,, x)形式
		SVzNL2DPointD axis = { -(rightEnginePoint.pt3D.z - leftEnginePoint.pt3D.z), rightEnginePoint.pt3D.x - leftEnginePoint.pt3D.x };
		if (axis.y < 0)
			axis = { -axis.x, -axis.y};

		double dirAngle = atan(axis.x / axis.y) * 180.0 / PI;
		planePoseInfo.nosePoint = nosePoint.pt3D;
		planePoseInfo.axis = { axis.x, 0, axis.y };
		planePoseInfo.axis = vec3_normalize(planePoseInfo.axis);
		planePoseInfo.distance = nosePoint.pt3D.z - rotateParkingPoint.z + parkingParam.distFromNoseToWheel;
		planePoseInfo.deviation = nosePoint.pt3D.x - rotateParkingPoint.x;  //偏离引导线
		planePoseInfo.dirAngle_deg = dirAngle; //方向角度
	}
	
	return planePoseInfo;
}