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

#define _DEBUG_OUTPUT

//version 1.0.0 : base version release to customer
//version 1.1.0 : 海瑞马码垛规划版本
std::string	m_strVersion = "HybridPositioning 1.1.0";
const char* wd_hybridPositioningVersion(void)
{
	return m_strVersion.c_str();
}

//相机水平安装计算地面调平参数。
//相机Z轴基本平行地面时，需要以地面为参照，将相机调水平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara wd_getGroundCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_getPlaneCalibPara2(scanLines);
}

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

SVzNL3DPosition _computeMinZPoint(std::vector<SVzNL3DPosition>& points)
{
	SVzNL3DPosition peak;
	peak.nPointIdx = 0;
	peak.pt3D = { 0.0, 0.0, 0.0 };
	for (int i = 0; i < (int)points.size(); i++)
	{
		if (points[i].pt3D.z < 1e-4)
			continue;

		if (peak.pt3D.z < 1e-4)
			peak = points[i];
		else if (peak.pt3D.z > points[i].pt3D.z)
			peak = points[i];

	}
	return peak;
}

SVzNL3DPoint _computeCentroid(std::vector<SVzNL3DPosition>& points)
{
	SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 };
	int counter = 0;
	for (int i = 0; i < (int)points.size(); i++)
	{
		if (points[i].pt3D.z < 1e-4)
			continue;

		counter++;
		centroid.x += points[i].pt3D.x;
		centroid.y += points[i].pt3D.y;
		centroid.z += points[i].pt3D.z;
	}
	centroid.x = centroid.x / counter;
	centroid.y = centroid.y / counter;
	centroid.z = centroid.z / counter;
	return centroid;
}

int _get2DRegion(SVzNLPositionD& a_pt2D3D, std::vector<WD_objArea2D>& objROIs)
{
	for (int i = 0; i < (int)objROIs.size(); i++)
	{
		if ((a_pt2D3D.ptLeft2D.x >= objROIs[i].roi.left) && (a_pt2D3D.ptLeft2D.x <= objROIs[i].roi.right) &&
			(a_pt2D3D.ptLeft2D.y >= objROIs[i].roi.top) && (a_pt2D3D.ptLeft2D.y <= objROIs[i].roi.bottom))
			return i;
	}
	return -1;
}

bool _compareByZValue(SVzNL3DPosition& a, SVzNL3DPosition& b)
{
	return a.pt3D.z < b.pt3D.z;
}

//生成圆周扫描数据
void _genPolarScanData(
	std::vector<SVzNL3DPosition>& points,
	const double angleScale,
	const SVzNL3DPoint polarCener,
	std::vector<std::vector<SWD_polarPt>>& polarScanData)
{
	int polarLines = (int)(360.0 / angleScale + 0.5);
	polarScanData.resize(polarLines);

	int dataSize = (int)points.size();
	for (int i = 0; i < dataSize; i++)
	{
		int line = points[i].nPointIdx >> 16;
		int ptIdx = points[i].nPointIdx & 0x0000FFFF;
		SVzNL3DPoint& a_pt = points[i].pt3D;

		double angle = atan2(a_pt.y - polarCener.y, a_pt.x - polarCener.x);
		angle = (angle / PI) * 180 + 180.0;
		double R = sqrt(pow(a_pt.y - polarCener.y, 2) + pow(a_pt.x - polarCener.x, 2));

		int angleLine = (int)(angle / angleScale + 0.5);
		angleLine = angleLine % (int)polarScanData.size();

		SWD_polarPt a_polarPt;
		a_polarPt.lineIdx = line;
		a_polarPt.ptIdx = ptIdx;
		a_polarPt.x = a_pt.x;
		a_polarPt.y = a_pt.y;
		a_polarPt.z = a_pt.z;
		a_polarPt.R = R;
		a_polarPt.angle = angle;
		polarScanData[angleLine].push_back(a_polarPt);
	}
}

//生成圆周扫描数据, 同心圆环结构
void _genPolarScanData_2(
	std::vector<SVzNL3DPosition>& points,
	const double radiusScale,
	const SVzNL3DPoint polarCener,
	std::vector<std::vector<SWD_polarPt>>& polarScanData)
{
	std::vector< SWD_polarPt> polarPoints;
	int dataSize = (int)points.size();
	double rMax = 0;
	for (int i = 0; i < dataSize; i++)
	{
		int line = points[i].nPointIdx >> 16;
		int ptIdx = points[i].nPointIdx & 0x0000FFFF;
		SVzNL3DPoint& a_pt = points[i].pt3D;

		double angle = atan2(a_pt.y - polarCener.y, a_pt.x - polarCener.x);
		angle = (angle / PI) * 180 + 180.0;
		double R = sqrt(pow(a_pt.y - polarCener.y, 2) + pow(a_pt.x - polarCener.x, 2));
		rMax = rMax < R ? R : rMax;
	
		SWD_polarPt a_polarPt;
		a_polarPt.lineIdx = line;
		a_polarPt.ptIdx = ptIdx;
		a_polarPt.x = a_pt.x;
		a_polarPt.y = a_pt.y;
		a_polarPt.z = a_pt.z;
		a_polarPt.R = R;
		a_polarPt.angle = angle;

		polarPoints.push_back(a_polarPt);
	}

	int circleNum = (int)(rMax / radiusScale) + 1;
	polarScanData.resize(circleNum);
	for (int i = 0; i < (int)polarPoints.size(); i++)
	{
		double r = polarPoints[i].R;
		int idx = r / radiusScale;
		polarScanData[idx].push_back(polarPoints[i]);
	}
}

bool compareByPolarScanR(const SWD_polarPt& a, const SWD_polarPt& b) {
	return a.R < b.R;
}
bool compareByPolarScanAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
	return a.angle < b.angle;
}

WD_workpieceInfo _computeWorkpiecePose(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	WD_workpieceInfo a_pose;
	memset(&a_pose, 0, sizeof(WD_workpieceInfo));

	int lineNum = (int)scanLines.size();
	int linePtNum = (int)scanLines[0].size();

	std::vector<std::vector<SVzNL3DPosition>> hLines;
	hLines.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		hLines[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			hLines[j][line] = scanLines[line][j];
			hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	//垂直和水平扫描，计算水平和垂直方向角度
	
	

	void wd_computeDirAngle_wholeLine2(
		std::vector< SVzNL3DPosition>&line_data,
		const double steppingScale,
		const double invalidScale, //超出此尺度，方向角计算无效
		std::vector< SSG_pntDirAngle>&ptDirAngles
	);
	 
	//检测是否有凸起
	return a_pose;
}


void wd_HRM_RotorCorePositioning(
	std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
	std::vector<WD_objArea2D>& objROIs,
	const SSG_planeCalibPara groundCalibPara,
	std::vector< WD_workpieceInfo>& workpiecePositions,
	int* errCode)
{
	*errCode = 0;

	if (objROIs.size() == 0)
	{
		*errCode = SX_ERR_ZERO_2D_OBJECTS;
		return;
	}

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

	int lineNum = (int)scanLinesInput.size();
	int linePtNum = (int)scanLinesInput[0].size();
	int maxU = 0; //2D图像的最大Col
	int maxV = 0; //2D图像的最大Row
	for (int line = 0; line < lineNum; line++)
	{
		for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++)
		{
			if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
				continue;

			maxU = maxU < scanLinesInput[line][ptIdx].ptLeft2D.x ? scanLinesInput[line][ptIdx].ptLeft2D.x : maxU;
			maxV = maxV < scanLinesInput[line][ptIdx].ptLeft2D.y ? scanLinesInput[line][ptIdx].ptLeft2D.y : maxV;
		}
	}

	//生成图像与3d的对应表
	const int imgCols = maxU;
	const int imgRows = maxV;
	std::vector<std::vector<SWDIndexingVzPoint>> mappingTable;  //按图像大小建立。
	mappingTable.resize(imgCols);  //与扫描线方向对应
	for (int i = 0; i < imgCols; i++)
		mappingTable[i].resize(imgRows);

	for (int line = 0; line < lineNum; line++)
	{
		for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
		{
			scanLinesInput[line][ptIdx].nPointIdx = 0;
			if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
				continue;

			SWDIndexingVzPoint indexingPt;
			indexingPt.lineIdx = line;
			indexingPt.ptIdx = ptIdx;
			indexingPt.point = scanLinesInput[line][ptIdx].pt3D;
			int u = scanLinesInput[line][ptIdx].ptLeft2D.x;
			int v = scanLinesInput[line][ptIdx].ptLeft2D.y;
			mappingTable[u][v] = indexingPt;
		}
	}

	int objNum = (int)objROIs.size();
	for (int idx = 0; idx < objNum; idx++)
	{
		WD_objArea2D& obj_roi = objROIs[idx];
		int L = (int)(obj_roi.roi.left + 0.5);
		int R = (int)(obj_roi.roi.right + 0.5);
		int T = (int)(obj_roi.roi.top + 0.5);
		int B = (int)(obj_roi.roi.bottom + 0.5);

		//统计ROI中的扫描线和PtIdx范围
		SVzNLRange roiLineIndice = { INT_MAX, 0 };
		SVzNLRange roiPtIndice = { INT_MAX, 0 };
		for (int x = L; x <= R; x++)
		{
			for (int y = T; y <= B; y++)
			{
				if (mappingTable[x][y].point.z > 1e-4)
				{
					int lineIdx = mappingTable[x][y].lineIdx;
					int ptIdx = mappingTable[x][y].ptIdx;
					scanLinesInput[lineIdx][ptIdx].nPointIdx = idx + 1;

					roiLineIndice.nMin = roiLineIndice.nMin > lineIdx ? lineIdx : roiLineIndice.nMin;
					roiLineIndice.nMax = roiLineIndice.nMax < lineIdx ? lineIdx : roiLineIndice.nMax;
					roiPtIndice.nMin = roiPtIndice.nMin > ptIdx ? ptIdx : roiPtIndice.nMin;
					roiPtIndice.nMax = roiPtIndice.nMax < ptIdx ? ptIdx : roiPtIndice.nMax;
				}
			}
		}

		//生成ROI中的扫描数据
		int roiLines = roiLineIndice.nMax - roiLineIndice.nMin + 1;
		int roiLinePtNum = roiPtIndice.nMax - roiPtIndice.nMin + 1;
		std::vector< std::vector<SVzNL3DPosition>> roiScanLines;
		roiScanLines.resize(roiLines);
		for (int line = 0; line < roiLines; line++)
			roiScanLines[line].resize(roiLinePtNum);
		for (int x = L; x <= R; x++)
		{
			for (int y = T; y <= B; y++)
			{
				if (mappingTable[x][y].point.z > 1e-4)
				{
					int lineIdx = mappingTable[x][y].lineIdx - roiLineIndice.nMin;
					int ptIdx = mappingTable[x][y].ptIdx - roiPtIndice.nMin;
					roiScanLines[lineIdx][ptIdx].pt3D = mappingTable[x][y].point;
				}
			}
		}

		//判断工件种类，计算工件姿态


	}
	return;

}

void wd_HRM_TaperedWorkpiecePositioning(
	std::vector< std::vector<SVzNLPositionD>>& scanLinesInput,
	std::vector<WD_objArea2D>& objROIs,
	const SSG_planeCalibPara groundCalibPara,
	std::vector< WD_workpieceInfo>& workpiecePositions,
	int* errCode)
{
	*errCode = 0;

	if (objROIs.size() == 0)
	{
		*errCode = SX_ERR_ZERO_2D_OBJECTS;
		return;
	}

	std::vector<std::vector<SVzNL3DPosition>> rgnPoints;
	rgnPoints.resize(objROIs.size());
	for (int line = 0; line < (int)scanLinesInput.size(); line++)
	{
		for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++)
		{
			if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4)
				continue;

			int rgnIdx = _get2DRegion(scanLinesInput[line][ptIdx], objROIs);
			if (rgnIdx >= 0)
			{
				SVzNL3DPosition a_rgnPt;
				a_rgnPt.nPointIdx = (line << 16) | (ptIdx & 0xffff);
				a_rgnPt.pt3D = scanLinesInput[line][ptIdx].pt3D;
				rgnPoints[rgnIdx].push_back(a_rgnPt);
				scanLinesInput[line][ptIdx].nPointIdx = rgnIdx + 1;
			}
		}
	}

	//统计每个region的最高点
	std::vector< SVzNL3DPosition> rgnPeaks;
	for (int i = 0; i < (int)rgnPoints.size(); i++)
	{
		SVzNL3DPosition peakPoint = _computeMinZPoint(rgnPoints[i]);
		SVzNL3DPosition a_peak;
		a_peak.nPointIdx = i;
		a_peak.pt3D = peakPoint.pt3D;
		rgnPeaks.push_back(a_peak);
	}

	//按高度排序 
	std::sort(rgnPeaks.begin(), rgnPeaks.end(), _compareByZValue);
	for (int i = 0; i < (int)rgnPeaks.size(); i++)
	{
		WD_workpieceInfo a_obj;
		memset(&a_obj, 0, sizeof(WD_workpieceInfo));
		a_obj.center = rgnPeaks[i].pt3D;
		workpiecePositions.push_back(a_obj);
	}
	return;

}

SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData)
{
	if (listData.size() == 0)
		return { 0,0,0,0 };
	SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y };
	for (int i = 0; i < (int)listData.size(); i++)
	{
		roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left;
		roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right;
		roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top;
		roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom;
	}
	return roi;
}

double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRange)
{
	if (listData.size() == 0)
		return 0;
	double meanZ = 0;
	zRange.max = -1;
	zRange.min = 0;
	for (int i = 0; i < (int)listData.size(); i++)
	{
		meanZ += listData[i].pt3D.z;
		if (zRange.max < 0)
		{
			zRange.max = listData[i].pt3D.z;
			zRange.min = listData[i].pt3D.z;
		}
		else
		{
			zRange.max = zRange.max < listData[i].pt3D.z ? listData[i].pt3D.z : zRange.max;
			zRange.min = zRange.min > listData[i].pt3D.z ? listData[i].pt3D.z : zRange.min;
		}
	}
	meanZ = meanZ / (double)listData.size();
	return meanZ;
}

//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0 
cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta)
{
	return (cv::Point2f((float)(pt.x * cosTheta - pt.y * sinTheta), (float)(pt.x * sinTheta + pt.y * cosTheta)));
}

//料筐码放：获取料筐尺寸、料筐姿态、料筐中心点坐标
#if 0
//检测层板边缘。层板小于料筐
WD_HRM_BinInfo wd_HRM_getBinSize(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	int* errCode)
{
	*errCode = 0;
	WD_HRM_BinInfo resultPose;
	memset(&resultPose, 0, sizeof(WD_HRM_BinInfo));

	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return resultPose;
	}

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

	//产生水平扫描数据
	std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
	scanLines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		scanLines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines_h[j][line] = scanLines[line][j];
			scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
			scanLines_h[line][j].nPointIdx = j;
	}

	//算法流程：
	//1、检查垂直方向数据并去除
	//2、聚类
	//3、保留最大目标
	//4、拟合

	//内部参数
	SSG_cornerParam removeVertialPara = cornerPara;
	removeVertialPara.scale = 3.0;
	removeVertialPara.cornerTh = 60;

	std::vector<std::vector<int>> flags;
	flags.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		flags[i].resize(linePtNum);
		std::fill(flags[i].begin(), flags[i].end(), 0);
	}
	std::vector<std::vector<int>> zVertivalFlags;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 700)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[line][i] = 1;
		}
	}

	std::vector<std::vector<int>> zVertivalFlags_h;
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 1177)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines_h[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags_h.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[i][line] = 1;
		}
	}
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
			{
				scanLines[line][j].pt3D.z = 0;
				scanLines_h[j][line].pt3D.z = 0;
			}
		}
	}
	//迭代一次
	SSG_lineSegParam lineSegPara;
	lineSegPara.distScale = 5.0;
	lineSegPara.segGapTh_y = 5.0;
	lineSegPara.segGapTh_z = 5.0;
	const int minSegLen = 5;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[line][idx0 + j] = 1;
			}
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines_h[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[idx0 + j][line] = 1;
			}
		}
	}

	//标注
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
	}
	//将垂直线段去除
	std::vector< SVzNL3DPosition> validPoints;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
				scanLines[line][j].pt3D.z = 0;

			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				SVzNL3DPosition a_vldPt;
				a_vldPt.pt3D = scanLines[line][j].pt3D;
				a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
				validPoints.push_back(a_vldPt);
			}
		}
	}

	//聚类
	//内部参数
	//double minObjSize_w = 150;
	//double minObjSize_h = 150;

	int clusterCheckWin = 5;
	double clusterDist = 5.0;
	int distType = 1;  //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;  //result
	wd_pointClustering_speedUp(
		validPoints,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType,
		objClusters  //result
	);

	//保留最大的目标
	std::vector<double> objMeanZ;
	std::vector<SVzNLRangeD> objZRange;
	objMeanZ.resize(objClusters.size());
	objZRange.resize(objClusters.size());
	int maxSizeId = -1;
	double maxSize = 0;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
		double w = a_roi.right - a_roi.left;
		double h = a_roi.bottom - a_roi.top;
		double size = w * h;

		SVzNLRangeD zRange;
		double meanZ = _getListMeanZ(objClusters[i], zRange);
		objMeanZ[i] = meanZ;
		objZRange[i] = zRange;

		if (maxSize < size)
		{
			maxSize = size;
			maxSizeId = i;
		}

	}

	//迭代：消除底板和层板的边缘的扫描毛刺



	std::vector< SVzNL3DPosition>& bottomCluster = objClusters[maxSizeId];
	//标注
	//重新将flags设置为目标的mask
	for (int i = 0; i < lineNum; i++)
		std::fill(flags[i].begin(), flags[i].end(), -1);
	for (int i = 0; i < (int)bottomCluster.size(); i++)
	{
		int line = bottomCluster[i].nPointIdx >> 16;
		int ptIdx = bottomCluster[i].nPointIdx & 0x0000FFFF;
		scanLines[line][ptIdx].nPointIdx = 2;
		flags[line][ptIdx] = i;  //indexing
	}

	//使用PCA方法计算法向量
	SVzNL3DPoint vec_normal, vec_centroid;

	computePlaneNormalByPCA(
		bottomCluster,
		vec_normal,
		vec_centroid);

	//投影





	if (vec_normal.z < 0)
		vec_normal = { -vec_normal.x, -vec_normal.y, -vec_normal.z };
	resultPose.center = vec_centroid;
	resultPose.bottomNormal = vec_normal;


	return resultPose;
}
#else
//检测料筐边缘，以中心点为基准进行规划
WD_HRM_BinInfo wd_HRM_getBinSize(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_planeCalibPara calibPara,
	const double binHeight, //料筐高度
	int* errCode)
{
	*errCode = 0;
	WD_HRM_BinInfo resultPose;
	memset(&resultPose, 0, sizeof(WD_HRM_BinInfo));

	//内部参数
	SVzNLRangeD binTopSliceRange = {calibPara.planeHeight- binHeight-5.0, calibPara.planeHeight - binHeight + 5.0};  //料筐高度Z层切范围

	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return resultPose;
	}

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

	//地面调平
	for (int i = 0; i < lineNum; i++)
		wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//调平

	//Z层切
	std::vector<SVzNL3DPosition> zSliceData;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0;
			if ((scanLines[line][j].pt3D.z > binTopSliceRange.min) && (scanLines[line][j].pt3D.z < binTopSliceRange.max))
			{
				SVzNL3DPosition a_pt;
				a_pt.nPointIdx = (line << 16) | j & 0xffff;
				a_pt.pt3D = scanLines[line][j].pt3D;
				zSliceData.push_back(a_pt);
				scanLines[line][j].nPointIdx = 1;  //标注
			}
		}
	}

	SVzNLRangeD dataZRange;
	double zSliceZ = _getListMeanZ(zSliceData, dataZRange);
	//计算轮廓
	// 最小外接矩形
	std::vector<cv::Point2f> points;
	for (int i = 0; i < (int)zSliceData.size(); i++)
	{
		cv::Point2f a_pt = cv::Point2f(zSliceData[i].pt3D.x, zSliceData[i].pt3D.y);
		points.push_back(a_pt);
	}
	cv::RotatedRect rect = minAreaRect(points);
	cv::Point2f vertices[4];
	rect.points(vertices);
	double width = rect.size.width;  //投影的宽和高
	double height = rect.size.height;
	if (width < height)
	{
		double tmp = height;
		height = width;
		width = tmp;
	}
	//计算姿态角vertices[0]是旋转点
	double dist_v0v3 = sqrt(pow(vertices[0].x - vertices[3].x, 2) + pow(vertices[0].y - vertices[3].y, 2));
	double width_diff = abs(dist_v0v3 - width);
	double pose_yaw;
	if (CV_VERSION == "3.2.0")
	{
		if (width_diff < 10.0)//width方向
		{
			pose_yaw = -rect.angle;
		}
		else //长度方向
		{
			pose_yaw = -rect.angle - 90;
			if (pose_yaw < -90)
				pose_yaw = 180 + pose_yaw;
		}
	}
	else  //if (CV_VERSION == "4.8.0")
	{
		if (width_diff < 10.0)  //width方向
		{
			pose_yaw = -rect.angle;
		}
		else//长度方向
		{
			pose_yaw = -rect.angle + 90;
			if (pose_yaw > 90)
				pose_yaw = pose_yaw - 180;
		}
	}
	//生成料筐信息
	double binZ = calibPara.planeHeight - binHeight;
	double sinTheta = sin(-PI * pose_yaw / 180);
	double cosTheta = cos(-PI * pose_yaw / 180);
	resultPose.center = { rect.center.x, rect.center.y, binZ };
	resultPose.bottomNormal = { 0.0, 0.0, 1.0 };
	resultPose.x_dir = { cosTheta , sinTheta , 0.0 };
	resultPose.y_dir = vec3_cross(resultPose.bottomNormal, resultPose.x_dir);  //叉乘出y_dir
	resultPose.length = width;
	resultPose.width = height;
	resultPose.binTopZ = zSliceZ;
	resultPose.minRectVertex[0] = { vertices[0].x, vertices[0].y, binZ };
	resultPose.minRectVertex[1] = { vertices[1].x, vertices[1].y, binZ };
	resultPose.minRectVertex[2] = { vertices[2].x, vertices[2].y, binZ };
	resultPose.minRectVertex[3] = { vertices[3].x, vertices[3].y, binZ };
	
	return resultPose;
}
#endif

//料筐码放：获取工件尺寸
WD_HRM_workpieceSizeInfo wd_HRM_getWorkpieceSize(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	std::vector<WD_HRM_workpieceSizeInfo>& standardWorkpieceSize, 
	const SSG_cornerParam cornerPara,
	const SSG_planeCalibPara calibPara,
	int* errCode)
{
	*errCode = 0;
	WD_HRM_workpieceSizeInfo resultInfo;
	memset(&resultInfo, 0, sizeof(WD_HRM_workpieceSizeInfo));

	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return resultInfo;
	}

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

	//地面调平
	for (int i = 0; i < lineNum; i++)
		wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//调平

	//产生水平扫描数据
	std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
	scanLines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		scanLines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			scanLines_h[j][line] = scanLines[line][j];
			scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
			scanLines_h[line][j].nPointIdx = j;
	}

	//算法流程：
	//1、检查垂直方向数据并去除
	//2、聚类
	//3、保留最大目标
	//4、拟合

	//内部参数
	SSG_cornerParam removeVertialPara = cornerPara;
	removeVertialPara.scale = 3.0;
	removeVertialPara.cornerTh = 60;

	std::vector<std::vector<int>> flags;
	flags.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		flags[i].resize(linePtNum);
		std::fill(flags[i].begin(), flags[i].end(), 0);
	}
	std::vector<std::vector<int>> zVertivalFlags;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 700)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[line][i] = 1;
		}
	}

	std::vector<std::vector<int>> zVertivalFlags_h;
	for (int line = 0; line < linePtNum; line++)
	{
		if (line == 1177)
			int kkk = 1;
		std::vector<int> line_verticalFlags;
		wd_getXYVertialFeature_dirAngleMethod(
			scanLines_h[line],
			line,
			removeVertialPara,
			line_verticalFlags
		);
		zVertivalFlags_h.push_back(line_verticalFlags);

		for (int i = 0; i < (int)line_verticalFlags.size(); i++)
		{
			if (line_verticalFlags[i] > 0)
				flags[i][line] = 1;
		}
	}
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
			{
				scanLines[line][j].pt3D.z = 0;
				scanLines_h[j][line].pt3D.z = 0;
			}
		}
	}
	//迭代一次
	SSG_lineSegParam lineSegPara;
	lineSegPara.distScale = 5.0;
	lineSegPara.segGapTh_y = 5.0;
	lineSegPara.segGapTh_z = 5.0;
	const int minSegLen = 5;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[line][idx0 + j] = 1;
			}
		}
	}
	for (int line = 0; line < linePtNum; line++)
	{
		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			scanLines_h[line],
			lineSegPara,
			segs);
		for (int i = 0; i < (int)segs.size(); i++)
		{
			if (segs[i].len <= minSegLen)
			{
				int idx0 = segs[i].start;
				for (int j = 0; j < segs[i].len; j++)
					flags[idx0 + j][line] = 1;
			}
		}
	}

	//标注
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
	}
	//将垂直线段去除
	std::vector< SVzNL3DPosition> validPoints;
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			if (flags[line][j] > 0)
				scanLines[line][j].pt3D.z = 0;

			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				SVzNL3DPosition a_vldPt;
				a_vldPt.pt3D = scanLines[line][j].pt3D;
				a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
				validPoints.push_back(a_vldPt);
			}
		}
	}

	//聚类
	//内部参数
	//double minObjSize_w = 150;
	//double minObjSize_h = 150;

	int clusterCheckWin = 5;
	double clusterDist = 10.0;
	int distType = 1;  //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>> objClusters;  //result
	wd_pointClustering_speedUp(
		validPoints,
		lineNum, linePtNum, clusterCheckWin, //搜索窗口
		clusterDist,
		distType,
		objClusters  //result
	);

	//取最大的目标为层板
	std::vector<double> objMeanZ;
	std::vector<SVzNLRangeD> objZRange;
	std::vector< SSG_ROIRectD> objROIs;
	objMeanZ.resize(objClusters.size());
	objZRange.resize(objClusters.size());
	objROIs.resize(objClusters.size());
	int maxSizeId = -1;
	double maxSize = 0;
	for (int i = 0; i < (int)objClusters.size(); i++)
	{
		SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
		objROIs[i] = a_roi;

		SVzNLRangeD zRange;
		double meanZ = _getListMeanZ(objClusters[i], zRange);
		objMeanZ[i] = meanZ;
		objZRange[i] = zRange;

		double w = a_roi.right - a_roi.left;
		double h = a_roi.bottom - a_roi.top;
		double size = w * h;
		if (maxSize < size)
		{
			maxSize = size;
			maxSizeId = i;
		}
	}

	//取层板ROI内的目标为工件
	SSG_ROIRectD& layerBoardROI = objROIs[maxSizeId];
	double layerBoardZ = objMeanZ[maxSizeId];
	int workpieceClusterId = -1;
	for (int i = 0; i< (int)objClusters.size(); i++)
	{
		if (i == maxSizeId)
			continue;

		SSG_ROIRectD& a_roi = objROIs[i];
		double obj_z = objMeanZ[i];
		if ((a_roi.left > layerBoardROI.left) && (a_roi.right < layerBoardROI.right) &&
			(a_roi.top > layerBoardROI.top) && (a_roi.bottom < layerBoardROI.bottom) && (obj_z < layerBoardZ))
		{
			if (workpieceClusterId < 0)
				workpieceClusterId = i;
			else if (objClusters[workpieceClusterId].size() < objClusters[i].size())
				workpieceClusterId = i;
		}
	}

	if(workpieceClusterId <0)
	{
		*errCode = SX_ERR_ZERO_OBJECTS;
		return resultInfo;
	}

	std::vector< SVzNL3DPosition>& layerBoardCluster = objClusters[maxSizeId];
	std::vector< SVzNL3DPosition>& workpieceCluster = objClusters[workpieceClusterId];
	double workpieceHeight = layerBoardZ - objZRange[workpieceClusterId].min;
	//标注
	for (int i = 0; i < (int)layerBoardCluster.size(); i++)
	{
		int line = layerBoardCluster[i].nPointIdx >> 16;
		int ptIdx = layerBoardCluster[i].nPointIdx & 0x0000FFFF;
		scanLines[line][ptIdx].nPointIdx = 1;
	}
	for (int i = 0; i < (int)workpieceCluster.size(); i++)
	{
		int line = workpieceCluster[i].nPointIdx >> 16;
		int ptIdx = workpieceCluster[i].nPointIdx & 0x0000FFFF;
		scanLines[line][ptIdx].nPointIdx = 2;
	}

	//计算工件的最小外接圆
	std::vector<cv::Point2f> points_2d;
	for (int i = 0; i < (int)workpieceCluster.size(); i++)
	{
		cv::Point2f a_pt = cv::Point2f(workpieceCluster[i].pt3D.x, workpieceCluster[i].pt3D.y);
		points_2d.push_back(a_pt);
	}
	cv::Point2f center;
	float r;
	cv::minEnclosingCircle(points_2d, center, r);

	//关联标准工件
	double bestError = 0;
	int bestId = -1;
	for (int i = 0; i < (int)standardWorkpieceSize.size(); i++)
	{
		double err = abs(standardWorkpieceSize[i].workpieceHeight - workpieceHeight) + abs(standardWorkpieceSize[i].workpieceRadius - r);
		if (bestId < 0)
		{
			bestId = i;
			bestError = err;
		}
		else if (bestError > err)
		{
			bestId = i;
			bestError = err;
		}
	}
	//生成投影结果
	resultInfo.center = { center.x, center.y, layerBoardZ };
	resultInfo.layerZValue = layerBoardZ;
	resultInfo.workpieceHeight = standardWorkpieceSize[bestId].workpieceHeight;
	resultInfo.workpieceRadius = standardWorkpieceSize[bestId].workpieceRadius;
	return resultInfo;
}

void planningFromCenter_1D(int num, double interval, double centerValue, std::vector<double>& pos)
{
	pos.resize(num);
	double halfInterval = interval / 2;
	if (num % 2 == 1) //奇数
	{
		int centerIdx = num / 2;
		pos[centerIdx] = centerValue;
		int j = 1;
		for (int idx = centerIdx - 1; idx >= 0; idx--)
		{
			pos[idx] = centerValue - j * interval;
			j++;
		}
		j = 1;
		for (int idx = centerIdx + 1; idx < num; idx++)
		{
			pos[idx] = centerValue + j * interval;
			j++;
		}
	}
	else
	{
		int j = 0;
		int halfSize = num / 2;
		for (int idx = halfSize - 1; idx >= 0; idx--)
		{
			pos[idx] = centerValue - halfInterval - j * interval;
			j++;
		}
		j = 0;
		for (int idx = halfSize; idx < num; idx++)
		{
			pos[idx] = centerValue + halfInterval + j * interval;
			j++;
		}
	}
	return;
}

//料筐码放：码放位置规划
void wd_HRM_PlanBinPlacement(
	const WD_HRM_BinInfo binInfo,
	const SSG_size2D realBoardSize, //实际的托板大小
	const WD_HRM_workpieceSizeInfo workpieceInfo,
	const SSG_planeCalibPara calibPara,
	const double guardingInterval,  //工作与工件的保护间隔
	std::vector< WD_workpieceInfo>& planningPositions,
	int* out_rows,
	int* out_cols,
	int* isLastLayere)
{
	double diamter = workpieceInfo.workpieceRadius * 2 + guardingInterval;
	double L = realBoardSize.width;// binInfo.length - guardingToSide * 2 + guardingInterval;
	double W = realBoardSize.height; //binInfo.width - guardingToSide * 2 + guardingInterval;

	int cols = (int)(L / diamter);
	int rows = (int)(W / diamter);

	//以中心为基准进行位置计算
	std::vector<double> rowPos;
	planningFromCenter_1D(rows, diamter, 0, rowPos);
	std::vector<double> colPos;
	planningFromCenter_1D(cols, diamter, 0, colPos);

	int objNum = rows * cols;
	planningPositions.resize(objNum);
	for (int row = 0; row < rows; row++)
	{
		for(int col = 0; col < cols; col++)
		{
			WD_workpieceInfo a_pos;
			a_pos.center = { colPos[col], rowPos[row], workpieceInfo.layerZValue };
			a_pos.value = workpieceInfo.workpieceRadius;
			a_pos.workpieceType = 1;
			a_pos.z_dir = { 0, 0, 1.0 };
			a_pos.y_dir = { 0, 0, 0 };
			a_pos.x_dir = { 0, 0, 0 };
			planningPositions[row * cols + col] = a_pos;
		}
	}

	//旋转
	double cosTheta = binInfo.x_dir.x;
	double sinTheta = binInfo.x_dir.y;
	for (int i = 0; i < (int)planningPositions.size(); i++)
	{
		cv::Point2f a_pt2D = cv::Point2f(planningPositions[i].center.x, planningPositions[i].center.y);
		a_pt2D = _rotate2D(a_pt2D, sinTheta, cosTheta);
		planningPositions[i].center.x = a_pt2D.x + binInfo.center.x;
		planningPositions[i].center.y = a_pt2D.y + binInfo.center.y;
	}
	
	double resiH = workpieceInfo.layerZValue - binInfo.binTopZ - workpieceInfo.workpieceHeight;
	if (resiH < workpieceInfo.workpieceHeight)
		*isLastLayere = 1;
	else
		*isLastLayere = 0;
	*out_rows = rows;
	*out_cols = cols;

	//旋转回原坐标系
	for (int i = 0; i < (int)planningPositions.size(); i++)
	{
		planningPositions[i].center = wd_ptRotate(planningPositions[i].center, calibPara.invRMatrix);
		planningPositions[i].x_dir = wd_ptRotate(planningPositions[i].x_dir, calibPara.invRMatrix);
		planningPositions[i].y_dir = wd_ptRotate(planningPositions[i].y_dir, calibPara.invRMatrix);
		planningPositions[i].z_dir = wd_ptRotate(planningPositions[i].z_dir, calibPara.invRMatrix);
	}

	return;
}