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

//version 1.0.0 : base version release to customer
//version 1.1.0 : 添加了标定Mark检测，添加了相对于标定柱的坐标输出，添加了相机调平功能
//version 1.1.1 : 优化了标定柱检出。标定柱同时为扫描线端点时能够检出
std::string	m_strVersion = "1.1.1";
const char* wd_bagThreadPositioningVersion(void)
{
	return m_strVersion.c_str();
}

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara wd_bagThread_getBaseCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_getPlaneCalibPara2(scanLines);
}

//相机姿态调平，并去除地面
void wd_bagThread_lineDataR(
	std::vector< SVzNL3DPosition>& a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT_vector(a_line, camPoseR, groundH);
}

#if 1

SVzNL3DPosition _findClosestPoint(std::vector<SVzNL3DPosition>& lineData, double a, double  b, double c, double* distMin)
{
	SVzNL3DPosition bestPt = { -1, { 0, 0, -1 } };

	double mod = sqrt(a * a + b * b);
	a = a / mod;
	b = b / mod;
	c = c / mod;
	double minDist = DBL_MAX;
	for (int i = 0; i < (int)lineData.size(); i++)
	{
		if (lineData[i].pt3D.z > 1e-4)
		{
			double dist = abs(a * lineData[i].pt3D.x + b * lineData[i].pt3D.y + c);
			if (minDist > dist)
			{
				minDist = dist;
				bestPt = lineData[i];
				bestPt.nPointIdx = i;
			}
		}
	}
	*distMin = minDist;
	return bestPt;
}

//聚类
void cloutPointsClustering(
	std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
	std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
	SSG_treeGrowParam growParam,
	std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
	std::vector<SWD_clustersInfo>& clustersInfo)
{
	int lineNum = (int)feature3DInfo.size();
	if (lineNum == 0)
		return;
	int linePtNum = (int)feature3DInfo[0].size();
	//采用迭代思想，回归思路进行高效聚类
	int clusterID = 1;
	int clusterCheckWin = 5;
	for (int y = 0; y < linePtNum; y++)
	{
		for (int x = 0; x < lineNum; x++)
		{
			SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
			if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
				continue;

			SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
			SVzNL3DRangeD a_clusterRoi;
			a_clusterRoi.xRange.min = a_feature3DValue.x;
			a_clusterRoi.xRange.max = a_feature3DValue.x;
			a_clusterRoi.yRange.min = a_feature3DValue.y;
			a_clusterRoi.yRange.max = a_feature3DValue.y;
			a_clusterRoi.zRange.min = a_feature3DValue.z;
			a_clusterRoi.zRange.max = a_feature3DValue.z;

			SVzNL2DPoint a_seedPos = { x, y };
			std::vector< SVzNL2DPoint> a_cluster;
			a_cluster.push_back(a_seedPos);
			wd_gridPointClustering(
				featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
				feature3DInfo,//double,记录坐标信息
				clusterCheckWin, //搜索窗口
				growParam,//聚类条件
				clusterID, //当前Cluster的ID
				a_cluster,  //result
				a_clusterRoi
			);
			clusters.push_back(a_cluster);
			SWD_clustersInfo a_info;
			a_info.clusterIdx = clusterID;
			a_info.ptSize = (int)a_cluster.size();
			a_info.roi3D = a_clusterRoi;
			clustersInfo.push_back(a_info);
			clusterID++;
		}
	}
}

//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0 
SVzNL3DPoint rotateXoY(SVzNL3DPoint& pt, double sinTheta, double cosTheta)
{
	return (SVzNL3DPoint{ (pt.x * cosTheta - pt.y * sinTheta), (pt.x * sinTheta + pt.y * cosTheta), pt.z });
}

//线头位置检测定位
void wd_bagThreadPositioning(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSX_ScanInfo scanInfo,  //true:激光线平行槽道；false:激光线垂直槽道
	const SSG_planeCalibPara groundCalibPara,
	const SSG_outlierFilterParam filterParam, //噪点过滤参数
	const SSG_cornerParam cornerPara, //V型特征参数
	const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数
	const SSG_treeGrowParam growParam, //特征生长参数
	std::vector<SSX_bagThreadInfo>& bagThreadInfo,
	std::vector<SSX_bagThreadInfo>& bagThreadInfo_relative, //相对于Mark的输出
	std::vector<SVzNL3DPoint>& output_markCenter,  //输出Mark位置信息
	int* errCode)
{
	*errCode = 0;
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}
	if (true == scanInfo.isHorizonScan)
	{
		*errCode = SG_ERR_LASER_DIR_NOT_SUPPORTED;
		return;
	}
	if (false == scanInfo.scanFromThreadHead)
	{
		*errCode = SG_ERR_SCAN_DIR_NOT_SUPPORTED;
		return;
	}

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

	//调平。
	for (int i = 0; i < lineNum; i++)
	{																					 //行处理
		//调平，去除地面
		wd_bagThread_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
	}

	//生成水平扫描数据
	//统计平均线间隔和点间隔，用于算法在计算前向角和后向角时加速
	double ptInterval = 0;
	int ptIntevalNum = 0;
	std::vector< std::vector<SVzNL3DPosition>> data_lines_h;  //水平扫描数据
	data_lines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		data_lines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			//scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			data_lines_h[j][line] = scanLines[line][j];
			data_lines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			data_lines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
			if (j > 0)
			{
				if ((scanLines[line][j - 1].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z > 1e-4))
				{
					ptInterval += abs(scanLines[line][j].pt3D.y - scanLines[line][j - 1].pt3D.y);
					ptIntevalNum++;
				}
			}
		}
	}
	if(ptIntevalNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}
	ptInterval = ptInterval / ptIntevalNum;
	int lineNum_h = linePtNum;
	int linePtNum_h = (int)data_lines_h[0].size();
	double lineInterval = 0;
	int lineIntervalNum = 0;
	for (int line = 0; line< lineNum_h; line++)
	{
		for (int j = 0, j_max = (int)data_lines_h[line].size(); j < j_max; j++)
		{
			data_lines_h[line][j].nPointIdx = j;
			if (j > 0)
			{
				if ((data_lines_h[line][j - 1].pt3D.z > 1e-4) && (data_lines_h[line][j].pt3D.z > 1e-4))
				{
					lineInterval += abs(data_lines_h[line][j].pt3D.y - data_lines_h[line][j - 1].pt3D.y);
					lineIntervalNum++;
				}
			}
		}
	}
	if (lineIntervalNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}
	lineInterval = lineInterval / lineIntervalNum;

	SVzNLRangeD jumpHeight = { scanInfo.mark_height-2.0, scanInfo.mark_height+2.0};
	double markRotateAngle = 0;
	SVzNL3DPoint markCenter = { 0,0,0 };
	std::vector< SVzNL2DPoint> debug_markOrigin;
	std::vector< SVzNL2DPoint> debug_markXDir;

	double vCornerScale = cornerPara.scale * 4;
	std::vector<std::vector<SSG_basicFeature1D>> cornerFeatures;
	std::vector<std::vector<SWD_segFeature>> raisedFeatures;
	//现场要求垂直扫描
	//if (false == scanInfo.isHorizonScan) //
	{
		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;

		int validVCornerSCale = (int)(vCornerScale / ptInterval);
		//垂直扫描检测V型槽和线尾
		for (int line = 0; line < lineNum; line++)
		{
			if ((line == 755) ||  (line == 905))
				int kkk = 1;
			std::vector<SVzNL3DPosition>& lineData = scanLines[line];
			//滤波，滤除异常点
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
			//提取V型槽
			std::vector<SSG_basicFeature1D> line_cornerFeatures;
			std::vector<SSG_RUN_EX> segs;
			int dataSize = (int)lineData.size();
			wd_getLineCorerFeature_accelerate(
				lineData,
				line,
				cornerPara,
				ptInterval,
				segs,
				line_cornerFeatures  //拐点
			);
			//检查V型特征两侧，不能有跳变
			std::vector<SSG_basicFeature1D> valid_cornerFeatures;
			int vCornerSize = (int)line_cornerFeatures.size();
			int segSize = (int)segs.size();
			for (int m = 0; m < vCornerSize; m++)
			{
				int cornerPos = line_cornerFeatures[m].jumpPos2D.y;
				for (int n = 0; n < segSize; n++)
				{
					int segEnd = segs[n].start + segs[n].len - 1;
					if ((cornerPos >= segs[n].start) && (cornerPos <= segEnd))
					{
						int skip_1 = cornerPos - segs[n].start;
						int skip_2 = segEnd - cornerPos;
						if((skip_1 >= validVCornerSCale) && (skip_2 >= validVCornerSCale))
							valid_cornerFeatures.push_back(line_cornerFeatures[m]);
						break;
					}
				}
			}
			cornerFeatures.push_back(valid_cornerFeatures);

			//提取凸起段
			std::vector<SWD_segFeature> line_raisedFeatures;
			wd_getLineRaisedFeature(
				lineData,
				line,
				raisedFeaturePara, //凸起参数
				line_raisedFeatures  //凸起
			);
			raisedFeatures.push_back(line_raisedFeatures);

			//提取标定柱跳变特征
			std::vector<SSG_basicFeature1D> line_jumpFearures;
			wd_getSpecifiedHeightJumping(
				lineData,
				line,
				jumpHeight, //跳变高度参数
				line_jumpFearures  //跳变
			);
			jumpFeatures_v.push_back(line_jumpFearures);
		}
#if 1
		//水平扫描检测标定柱
		std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
		for (int line = 0; line < lineNum_h; line++)
		{
			if ((line == 2240) || (line == 905))
				int kkk = 1;

			std::vector<SVzNL3DPosition>& lineData = data_lines_h[line];
			//滤波，滤除异常点
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam);
			//提取标定柱跳变特征
			std::vector<SSG_basicFeature1D> line_jumpFearures;
			wd_getSpecifiedHeightJumping(
				lineData,
				line,
				jumpHeight, //跳变高度参数
				line_jumpFearures  //跳变
			);
			jumpFeatures_h.push_back(line_jumpFearures);
		}

		//对标定柱进行聚类
		std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
		std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
		featureInfoMask.resize(lineNum);
		feature3DInfo.resize(lineNum);
		for (int line = 0; line < lineNum; line++)
		{
			featureInfoMask[line].resize(linePtNum);
			std::fill(featureInfoMask[line].begin(), featureInfoMask[line].end(), SSG_featureClusteringInfo{ 0,0,0,0,0,0,0 });
			feature3DInfo[line].resize(linePtNum);
			std::fill(feature3DInfo[line].begin(), feature3DInfo[line].end(), SVzNL3DPoint{ 0,0,0 });
		}
		//垂直
		for (int line = 0; line < lineNum; line++)
		{
			int lineJumpNum = (int)jumpFeatures_v[line].size();
			for(int j = 0; j <lineJumpNum; j++)
			{
				SSG_basicFeature1D& a_jump = jumpFeatures_v[line][j];
				SSG_featureClusteringInfo a_feature;
				a_feature.featurType = a_jump.featureType;
				a_feature.featureIdx_v = j;
				a_feature.featureIdx_h = -1;
				a_feature.clusterID = 0;
				a_feature.flag = 0;
				a_feature.lineIdx = line;
				a_feature.ptIdx = a_jump.jumpPos2D.y;

				int ptIdx = a_jump.jumpPos2D.y;
				featureInfoMask[line][ptIdx] = a_feature;
				feature3DInfo[line][ptIdx] = a_jump.jumpPos;
			}
		}
		//水平
		for (int line = 0; line < lineNum_h; line++)
		{
			if (line == 1067) 
				int kkk = 1;
			int lineJumpNum = (int)jumpFeatures_h[line].size();
			for (int j = 0; j < lineJumpNum; j++)
			{
				SSG_basicFeature1D& a_jump = jumpFeatures_h[line][j];
				SSG_featureClusteringInfo a_feature;
				a_feature.featurType = a_jump.featureType;
				a_feature.featureIdx_v = -1;
				a_feature.featureIdx_h = j;
				a_feature.clusterID = 0;
				a_feature.flag = 0;
				a_feature.lineIdx = a_jump.jumpPos2D.y;
				a_feature.ptIdx = line;

				int ptIdx = a_jump.jumpPos2D.y;
				if (featureInfoMask[ptIdx][line].featurType == 0)  //不重复添加
				{
					featureInfoMask[ptIdx][line] = a_feature;
					feature3DInfo[ptIdx][line] = { a_jump.jumpPos.y, a_jump.jumpPos.x, a_jump.jumpPos.z };
				}
			}
		}
		//聚类
		std::vector<std::vector< SVzNL2DPoint>> markClusters; //只记录位置
		std::vector<SWD_clustersInfo> markClustersInfo;
		SSG_treeGrowParam markGrowParam;
		markGrowParam.yDeviation_max = 1.0;//生长时相邻特征最大的Y偏差
		markGrowParam.zDeviation_max = 1.0; //生长时相邻特征最大的Z偏差
		markGrowParam.maxLineSkipNum = 5;  //生长时相邻特征的最大线间隔， -1时使用maxDkipDistance
		markGrowParam.maxSkipDistance = 1.0; //若maxLineSkipNum为-1， 使用此参数.设为-1时，此参数无效
		markGrowParam.minLTypeTreeLen = 5.0;  //生长树最少的节点数目。小于此数目的生长树被移除
		markGrowParam.minVTypeTreeLen = 5.0;  //生长树最少的节点数目。小于此数目的生长树被移除
		cloutPointsClustering(
			featureInfoMask,
			feature3DInfo,
			markGrowParam,
			markClusters, //只记录位置
			markClustersInfo);
		
		//判断标定柱是否检出 
		int clusterNum = (int)markClustersInfo.size();
		if (clusterNum < 2)
		{
			*errCode = SX_ERR_NO_MARK;
			return;
		}
		else
		{
			std::vector<int> validMarks;
			for (int i = 0; i < clusterNum; i++)
			{
				double w = markClustersInfo[i].roi3D.xRange.max - markClustersInfo[i].roi3D.xRange.min;
				double h = markClustersInfo[i].roi3D.yRange.max - markClustersInfo[i].roi3D.yRange.min;
				if ((w >= scanInfo.mark_diameter - 1.0) && (w <= scanInfo.mark_diameter + 1.0) &&
					(h >= scanInfo.mark_diameter - 1.0) && (h <= scanInfo.mark_diameter + 1.0))
				{
					validMarks.push_back(i);
				}
			}

			int mark_O_idx = -1, mark_x_idx = -1;
			if (validMarks.size() < 2)
			{
				*errCode = SX_ERR_NO_MARK;
				return;
			}
			else if (validMarks.size() == 2)
			{
				int mark0Idx = validMarks[0];
				int mark1Idx = validMarks[1];
				double cy_1 = (markClustersInfo[mark0Idx].roi3D.yRange.max + markClustersInfo[mark0Idx].roi3D.yRange.min) / 2;
				double cy_2 = (markClustersInfo[mark1Idx].roi3D.yRange.max + markClustersInfo[mark1Idx].roi3D.yRange.min) / 2;
				if (cy_1 < cy_2)
				{
					mark_O_idx = mark0Idx;
					mark_x_idx = mark1Idx;
				}
				else
				{
					mark_O_idx = mark1Idx;
					mark_x_idx = mark0Idx;
				}
			}
			else// if (validMarks.size() > 2)
			{
				//搜索距离最近的一组
				SSG_intPair obj_pair = {-1,-1,-1};
				double best_dist = -1;
				for (int i = 0; i < (int)validMarks.size(); i++)
				{
					int vldIdx0 = validMarks[i];
					double cx_1 = (markClustersInfo[vldIdx0].roi3D.xRange.max + markClustersInfo[vldIdx0].roi3D.xRange.min) / 2;
					double cy_1 = (markClustersInfo[vldIdx0].roi3D.yRange.max + markClustersInfo[vldIdx0].roi3D.yRange.min) / 2;
					for (int j = i + 1; j < (int)validMarks.size(); j++)
					{
						int vldIdx1 = validMarks[j];
						double cx_2 = (markClustersInfo[vldIdx1].roi3D.xRange.max + markClustersInfo[vldIdx1].roi3D.xRange.min) / 2;
						double cy_2 = (markClustersInfo[vldIdx1].roi3D.yRange.max + markClustersInfo[vldIdx1].roi3D.yRange.min) / 2;
						double dist = sqrt(pow(cx_1 - cx_2, 2) + pow(cy_1 - cy_2, 2));
						if (best_dist < 0)
						{
							best_dist = dist;
							obj_pair.data_0 = vldIdx0;
							obj_pair.data_1 = vldIdx1;
						}
						else
						{
							double dist_diff1 = abs(best_dist - scanInfo.mark_distance);
							double dist_diff2 = abs(dist - scanInfo.mark_distance);
							if (dist_diff1 > dist_diff2)
							{
								best_dist = dist;
								obj_pair.data_0 = vldIdx0;
								obj_pair.data_1 = vldIdx1;
							}
						}
					}
				}
				if (obj_pair.data_0 < 0)
				{
					*errCode = SX_ERR_NO_MARK;
					return;
				}
				else
				{
					double cy_1 = (markClustersInfo[obj_pair.data_0].roi3D.yRange.max + markClustersInfo[obj_pair.data_0].roi3D.yRange.min) / 2;
					double cy_2 = (markClustersInfo[obj_pair.data_1].roi3D.yRange.max + markClustersInfo[obj_pair.data_1].roi3D.yRange.min) / 2;
					if (cy_1 < cy_2)
					{
						mark_O_idx = obj_pair.data_0;
						mark_x_idx = obj_pair.data_1;
					}
					else
					{
						mark_O_idx = obj_pair.data_1;
						mark_x_idx = obj_pair.data_0;
					}
				}
			}
			//圆拟合计算圆心，以点Z值均值作为圆心的Z值
			std::vector<SVzNL3DPoint> fittingData_0;
			double meanZ_0 = 0;
			for (int i = 0; i < markClusters[mark_O_idx].size(); i++)
			{
				SVzNL2DPoint& a_pt2D = markClusters[mark_O_idx][i];
				SVzNL3DPoint& a_pt3D = feature3DInfo[a_pt2D.x][a_pt2D.y];
				meanZ_0 += a_pt3D.z;
				fittingData_0.push_back(a_pt3D);
			}
			meanZ_0 = meanZ_0 / (double)markClusters[mark_O_idx].size();
			SVzNL3DPoint mark0_center;
			double mark0_radius;
			double fitErr1 = fitCircleByLeastSquare(
				fittingData_0,
				mark0_center,
				mark0_radius);
			mark0_center.z = meanZ_0;

			std::vector<SVzNL3DPoint> fittingData_1;
			double meanZ_1 = 0;
			for (int i = 1; i < markClusters[mark_x_idx].size(); i++)
			{
				SVzNL2DPoint& a_pt2D = markClusters[mark_x_idx][i];
				SVzNL3DPoint& a_pt3D = feature3DInfo[a_pt2D.x][a_pt2D.y];
				meanZ_1 += a_pt3D.z;
				fittingData_1.push_back(a_pt3D);
			}
			meanZ_1 = meanZ_1 / (double)markClusters[mark_x_idx].size();
			SVzNL3DPoint mark1_center;
			double mark1_radius;
			double fitErr2 = fitCircleByLeastSquare(
				fittingData_1,
				mark1_center,
				mark1_radius);
			mark1_center.z = meanZ_1;

			output_markCenter.push_back(mark0_center);
			output_markCenter.push_back(mark1_center);
			debug_markOrigin.insert(debug_markOrigin.end(), markClusters[mark_O_idx].begin(), markClusters[mark_O_idx].end());
			debug_markXDir.insert(debug_markXDir.end(), markClusters[mark_x_idx].begin(), markClusters[mark_x_idx].end());

			markCenter = mark0_center;
			//计算旋转角
			SVzNL2DPointD a = { mark1_center.x - mark0_center.x, mark1_center.y - mark0_center.y };
			SVzNL2DPointD b = { 1, 0 };
			double rotAngle = 0;
			bool validRotate = calcRotateAngle(a, b, rotAngle);
			markRotateAngle = rotAngle;
		}
#endif
	}

	//特征生长
	std::vector< SSG_featureTree> cornerGrowTrees;
	sg_cornerFeaturesGrowing(
		cornerFeatures,
		cornerGrowTrees,
		growParam);
	std::vector<SWD_segFeatureTree> raisedFeatureGrowTrees;
	wd_getSegFeatureGrowingTrees(
		raisedFeatures,
		raisedFeatureGrowTrees,
		growParam);

	if (cornerGrowTrees.size() == 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}
	int raisedTreeNum = (int)raisedFeatureGrowTrees.size();
	int threadTailTreeIdx = -1; //线尾所在的tree
	if (raisedTreeNum == 1)
		threadTailTreeIdx = 0;
	else if (raisedTreeNum > 1)
	{
		//取最长的tree作为线尾所在的tree
		int maxLines = 0;
		threadTailTreeIdx = 0;
		for (int i = 0; i < raisedTreeNum; i++)
		{
			int treeLines = raisedFeatureGrowTrees[i].eLineIdx - raisedFeatureGrowTrees[i].sLineIdx;
			if (maxLines < treeLines)
			{
				maxLines = treeLines;
				threadTailTreeIdx = i;
			}
		}
	}
	int cornerTreeNum = (int)cornerGrowTrees.size();
	int objTreeIdx = -1;
	if (threadTailTreeIdx < 0)
	{
		int maxLines = 0;
		//取最长的tree作为线缝所有的生长树
		for (int i = 0; i < cornerTreeNum; i++)
		{
			int treeLines = cornerGrowTrees[i].eLineIdx - cornerGrowTrees[i].sLineIdx;
			if (maxLines < treeLines)
			{
				maxLines = treeLines;
				objTreeIdx = i;
			}
		}
	}
	else
	{
		//取与线尾所在tree最近的tree作为线缝所在的生长树,计算2D距离，所以不需要区分水平和垂直扫描方式

		SVzNL3DPoint pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].startPt;
		SVzNL3DPoint pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].endPt;
		SVzNL3DPoint tail_end_1 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
		pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().startPt;
		pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().endPt;
		SVzNL3DPoint tail_end_2 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
		double minDist = DBL_MAX;
		for (int i = 0; i < cornerTreeNum; i++)
		{
			//端点最近的距离为两个tree间的距离 
			SVzNL3DPoint end_1 = cornerGrowTrees[i].treeNodes[0].jumpPos;
			SVzNL3DPoint end_2 = cornerGrowTrees[i].treeNodes.back().jumpPos;
			int lineIdx_2 = cornerGrowTrees[i].eLineIdx;
			double dist_11 = sqrt(pow(tail_end_1.x - end_1.x, 2) + pow(tail_end_1.y - end_1.y, 2));
			double dist_12 = sqrt(pow(tail_end_1.x - end_2.x, 2) + pow(tail_end_1.y - end_2.y, 2));
			double dist_21 = sqrt(pow(tail_end_2.x - end_1.x, 2) + pow(tail_end_2.y - end_1.y, 2));
			double dist_22 = sqrt(pow(tail_end_2.x - end_2.x, 2) + pow(tail_end_2.y - end_2.y, 2));
			double dist = dist_11 < dist_12 ? dist_11 : dist_12;
			dist = dist < dist_21 ? dist : dist_21;
			dist = dist < dist_22 ? dist : dist_22;
			if (minDist > dist)
			{
				minDist = dist;
				objTreeIdx = i;
			}
		}
	}
	if(objTreeIdx < 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}
	
	//显示 
	//将原始数据的序列清0，转义使用
	for (int line = 0; line < lineNum; line++)
		for (int j = 0; j < linePtNum; j++)
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
	//置标志，用于debug
	{
		int nodeNum = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int lineIdx, ptIdx;
			if (false == scanInfo.isHorizonScan)
			{
				lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
				ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
			}
			else
			{
				lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
				ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
			}
			scanLines[lineIdx][ptIdx].nPointIdx = 1;
		}
		//显示线头
		if (threadTailTreeIdx >= 0)
		{
			int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size();
			for (int j = 0; j < nodeNum; j++)
			{
				int lineIdx, ptIdx;
				if (false == scanInfo.isHorizonScan)
				{
					lineIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
					for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
					{
						ptIdx = m;
						scanLines[lineIdx][ptIdx].nPointIdx = 2;
					}
				}
				else
				{
					ptIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
					for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
					{
						lineIdx = m;
						scanLines[lineIdx][ptIdx].nPointIdx = 2;
					}
				}
			}
		}
		for (int i = 0; i < (int)debug_markOrigin.size(); i++)
		{
			int lineIdx = debug_markOrigin[i].x;
			int ptIdx = debug_markOrigin[i].y;
			scanLines[lineIdx][ptIdx].nPointIdx = 3;
		}
		for (int i = 0; i < (int)debug_markXDir.size(); i++)
		{
			int lineIdx = debug_markXDir[i].x;
			int ptIdx = debug_markXDir[i].y;
			scanLines[lineIdx][ptIdx].nPointIdx = 4;
		}
	}
	//提取针脚(提取4个）
	std::vector<SVzNLRect> stitchROIs;
	int nodeSize = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();

	//重新确定线缝中每条扫描线拐点位置。使用分段最小二乘来优化
	double lenFittingScale = 10.0;//
	int lenFittingScale_lines = (int)(lenFittingScale / lineInterval);

	int totalLines = cornerGrowTrees[objTreeIdx].eLineIdx - cornerGrowTrees[objTreeIdx].sLineIdx + 1;
	int num_intervals = totalLines / lenFittingScale_lines;  //此处剩余部分不再处理，因为只需要取前3个线头
	std::vector<std::vector<SVzNL3DPoint>> segPoints;
	segPoints.resize(num_intervals);
	std::vector<int> cornerNodeLineIndice;
	cornerNodeLineIndice.resize(totalLines);
	std::fill(cornerNodeLineIndice.begin(), cornerNodeLineIndice.end(), -1);
	int startLineIdx = cornerGrowTrees[objTreeIdx].sLineIdx;
	for (int i = 0; i < nodeSize; i++)
	{
		SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[i];
		int lineDiff = a_node.jumpPos2D.x - startLineIdx;
		cornerNodeLineIndice[lineDiff] = i;
		int interval_id = lineDiff / lenFittingScale_lines;
		if (interval_id < num_intervals)
			segPoints[interval_id].push_back(a_node.jumpPos);
	}
	//分段直线拟合
	std::vector<SVzNL3DPoint> baseLinePoints; //线缝上每条扫描线的点
	std::vector<SVzNL2DPoint> baseLinePtPos;
	for (int i = 0; i < num_intervals; i++)
	{
		std::vector<SVzNL3DPoint> fittingPoints;
		if ((i == 0) || (i == (num_intervals - 1)))
		{
			fittingPoints.insert(fittingPoints.end(), segPoints[i].begin(), segPoints[i].end());
		}
		else
		{
			//使用3段数据拟合
			fittingPoints.insert(fittingPoints.end(), segPoints[i-1].begin(), segPoints[i-1].end());
			fittingPoints.insert(fittingPoints.end(), segPoints[i].begin(), segPoints[i].end());
			fittingPoints.insert(fittingPoints.end(), segPoints[i+1].begin(), segPoints[i+1].end());
		}
		//局部拟合
		double a, b, c;  //方程：ax+by+c=0;
		lineFitting_abc(fittingPoints, &a, &b, &c);

		int segStartLineIdx = i * lenFittingScale_lines + startLineIdx;
		for (int j = 0; j < lenFittingScale_lines; j++)
		{
			int idx = i * lenFittingScale_lines + j;
			if (cornerNodeLineIndice[idx] >= 0)
			{
				int nodeIdx = cornerNodeLineIndice[idx];
				SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[nodeIdx];
				baseLinePtPos.push_back(a_node.jumpPos2D);
				baseLinePoints.push_back(a_node.jumpPos);
			}
			else
			{
				double dist = DBL_MAX;
				SVzNL3DPosition bestPoint = _findClosestPoint(scanLines[segStartLineIdx + j], a, b, c, &dist);
				SVzNL2DPoint pos = { segStartLineIdx + j , bestPoint.nPointIdx };
				if (dist > ptInterval * 2)
				{
					bestPoint.pt3D = { 0, 0, -1 };
					pos.y = -1;
				}
				baseLinePtPos.push_back(pos);
				baseLinePoints.push_back(bestPoint.pt3D);
			}
		}

	}

	//提取针脚范围
	int pre_idx = -1;
	for (int i = 0; i < nodeSize; i++)
	{
		int nodeIdx = (true == scanInfo.scanFromThreadHead) ? i : (nodeSize - 1 - i);
		SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[nodeIdx];	
		if(pre_idx >= 0)
		{
			SSG_basicFeature1D& pre_node = cornerGrowTrees[objTreeIdx].treeNodes[pre_idx];
			int line_diff = a_node.jumpPos2D.x < pre_node.jumpPos2D.x ? (pre_node.jumpPos2D.x - a_node.jumpPos2D.x) : (a_node.jumpPos2D.x - pre_node.jumpPos2D.x);
			if (line_diff > 2)
			{
				double width = (true == scanInfo.isHorizonScan)? (line_diff * ptInterval) : (line_diff * lineInterval);
				if (width > scanInfo.stitchWidth)
				{
					//搜索Z值最高点
					int searchStart = pre_node.jumpPos2D.x - startLineIdx;
					int searchEnd = a_node.jumpPos2D.x - startLineIdx;
					
					int mean_Lines = (int)(0.5 / lineInterval) + 1;  //取1mm的距离上的Z平均值，用于搞噪
					int validMeanLinesTh = mean_Lines - 5;
					std::vector<double> validStitch;
					std::vector<int> validStitchIndice;

					//计算平均高度和高度极值
					double sticthPeak = DBL_MAX;
					int debug_pkPos = -1;
					for (int m = searchStart + 1; m < searchEnd; m++)
					{
						if (baseLinePoints[m].z > 1e-4)
						{
							double sumZ = 0;
							int sumZ_counter = 0;
							for (int n = m - mean_Lines; n <= m+ mean_Lines; n++)
							{
								if ((n > searchStart) && (n < searchEnd))
								{
									if (baseLinePoints[n].z > 1e-4)
									{
										sumZ += baseLinePoints[n].z;
										sumZ_counter++;
									}
								}
							}
							validStitchIndice.push_back(m);
							if (sumZ_counter > validMeanLinesTh)
							{
								double meanZ = sumZ / (double)sumZ_counter;
								validStitch.push_back(meanZ);
								
								if (sticthPeak > meanZ)
								{
									sticthPeak = meanZ;
									debug_pkPos = m;
								}
							}
							else
								validStitch.push_back(-1);
						}
						else
							validStitch.push_back(-1);
					}

					//以 （sticthPeak+0.5)为门限，取中间点
					double stitchPeakTh = sticthPeak + 0.5;
					int validStart = -1, validEnd = 0;
					for (int n = 0; n < (int)validStitch.size(); n++)
					{
						if ((validStitch[n] > 1e-4) &&(validStitch[n] < stitchPeakTh))
						{
							if (validStart < 0)
								validStart = n;
							validEnd = n;
						}
					}
					int refCenterLine = (validStart + validEnd)/2 + searchStart + 1;
					SVzNL3DPoint stitchPos = { 0, 0, DBL_MAX };
					int stitchLineIdx = -1;
					int searchRng = (validEnd - validStart) / 2;
					for (int m = 0; m < searchRng; m++)  //搜索范围也设为opDist_lines
					{
						int index_1 = refCenterLine + m;
						if (baseLinePoints[index_1].z > 1e-4)
						{
							stitchLineIdx = index_1;
							stitchPos = baseLinePoints[index_1];
							break;
						}
						int index_2 = refCenterLine - m;
						if (baseLinePoints[index_2].z > 1e-4)
						{
							stitchLineIdx = index_2;
							stitchPos = baseLinePoints[index_2];
							break;
						}
					}

					//搜索下刀位置
					SVzNL3DPoint operatePos = {0,0, -1};
					if (stitchLineIdx >= 0)
					{
						int opDist_lines;
						if (false == scanInfo.isHorizonScan)  //垂直于线缝扫描
							opDist_lines = (int)(scanInfo.operateDist / lineInterval);
						else
							opDist_lines = (int)(scanInfo.operateDist / ptInterval);

						int op_centerLine = stitchLineIdx + opDist_lines;
						for (int m = 0; m < opDist_lines; m++)  //搜索范围也设为opDist_lines
						{
							int index_1 = op_centerLine + m;
							if (index_1 < totalLines)
							{
								if (cornerNodeLineIndice[index_1] > 0)
								{
									int nodeIndex = cornerNodeLineIndice[index_1];
									operatePos = cornerGrowTrees[objTreeIdx].treeNodes[nodeIndex].jumpPos;
									break;
								}
							}
							int index_2 = op_centerLine - m;
							if (index_2 >= 0)
							{
								if (cornerNodeLineIndice[index_2] > 0)
								{
									int nodeIndex = cornerNodeLineIndice[index_2];
									operatePos = cornerGrowTrees[objTreeIdx].treeNodes[nodeIndex].jumpPos;
									break;
								}
							}
						}
						if ( (stitchLineIdx >= 0) && (operatePos.z > 1e-4))
						{
							SSX_bagThreadInfo a_stitchInfo;
							memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo));
							a_stitchInfo.threadPos = stitchPos;
							a_stitchInfo.operatePos = operatePos;
							a_stitchInfo.rotateAngle = 0;
							bagThreadInfo.push_back(a_stitchInfo);
							if (bagThreadInfo.size() >= 4)
								break;
						}
					}
				}
			}
		}
		pre_idx = nodeIdx;
	}
	//输出
	if (bagThreadInfo.size() == 0)
		*errCode = SG_ERR_ZERO_OBJECTS;

	//计算为相对坐标
	double cosTheta = cos(markRotateAngle);
	double sinTheta = sin(markRotateAngle);
	bagThreadInfo_relative.resize(bagThreadInfo.size());
	for (int i = 0; i < (int)bagThreadInfo.size(); i++)
	{
		SSX_bagThreadInfo& a_stitch = bagThreadInfo[i];
		SVzNL3DPoint a_pt = { a_stitch.threadPos.x - markCenter.x, a_stitch.threadPos.y - markCenter.y, a_stitch.threadPos.z - markCenter.z };
		SVzNL3DPoint rot_pt = rotateXoY(a_pt, sinTheta, cosTheta);
		bagThreadInfo_relative[i].threadPos = rot_pt;
		a_pt = { a_stitch.operatePos.x - markCenter.x, a_stitch.operatePos.y - markCenter.y, a_stitch.operatePos.z - markCenter.z };
		rot_pt = rotateXoY(a_pt, sinTheta, cosTheta);
		bagThreadInfo_relative[i].operatePos = rot_pt;
		bagThreadInfo_relative[i].rotateAngle = 0;
	}
	return;
}
#endif