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

//version 1.0.0 : base version release to customer
std::string	m_strVersion = "1.0.0";
const char* wd_workpieceHolePositioningVersion(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);
}

SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9])
{
	SVzNL3DPoint _r_pt;
	_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
	_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
	_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
	return _r_pt;
}

//搜索最接近distance的目标
int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, double distance, double distDeviation)
{
	int result = -1;
	int holeSize = (int)holes.size();
	double minDistDiff = DBL_MAX;
	int minDistIndex = -1;
	for (int i = 0; i < holeSize; i++)
	{
		if (holes[i].radius < 0)
			continue;

		double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
		double distDiff = abs(dist - distance);
		if (minDistDiff > distDiff)
		{
			minDistDiff = distDiff;
			minDistIndex = i;
		}
	}
	if ((minDistIndex >= 0) && (minDistDiff < distDeviation))
		result = minDistIndex;
	return result;
}

//搜索最接近distance且角度为angle的目标, 以角度为优先
int angleConditionDistanceSearch(
	SVzNL3DPoint seed, SVzNL3DPoint angleSide,
	std::vector<SWD_HoleInfo>& holes,
	double distance, double distDeviation,
	SVzNLRangeD angleRange)
{
	int result = -1;
	int holeSize = (int)holes.size();
	std::vector< int> distValidHoleIndex;
	for (int i = 0; i < holeSize; i++)
	{
		if (holes[i].radius < 0)
			continue;

		double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
		double distDiff = abs(dist - distance);
		if (distDiff < distDeviation)
		{
			distValidHoleIndex.push_back(i);
		}
	}
	if (distValidHoleIndex.size() == 1)
	{
		int idx = distValidHoleIndex[0];
		double angle = computeXOYVertexAngle(seed, angleSide, holes[idx].center);
		if( (angle >= angleRange.min) &&(angle <= angleRange.max))
			result = idx;
	}
	else if (distValidHoleIndex.size() > 1)
	{
		double bestAngle = (angleRange.min + angleRange.max) / 2;
		double minAngleDeviateion = DBL_MAX;
		int minAngleIdx = -1;
		for (int i = 0, i_max = (int)distValidHoleIndex.size(); i < i_max; i++)
		{
			int idx = distValidHoleIndex[i];
			double angle = computeXOYVertexAngle(seed, angleSide, holes[idx].center);
			if ((angle >= angleRange.min) && (angle <= angleRange.max))
			{
				double angleDiff = abs(angle - bestAngle);
				if (minAngleDeviateion > angleDiff)
				{
					minAngleDeviateion = angleDiff;
					minAngleIdx = idx;
				}
			}
		}
		result = minAngleIdx;
	}
	return result;
}

//工件孔定位
void wd_workpieceHolePositioning(
	std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
	const WD_workpieceHoleParam workpiecePara,
	const SSG_lineSegParam lineSegPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	const SSG_planeCalibPara groundCalibPara,
	std::vector< WD_workpieceInfo>& workpiecePositioning,
	int* errCode)
{
	*errCode = 0;

	int lineNum = (int)scanLinesInput.size();
	std::vector< std::vector<SVzNL3DPosition>> scanLines;
	scanLines.resize(lineNum);
	int linePtNum = (int)scanLinesInput[0].size();
	bool isGridData = true;
	for (int i = 0; i < lineNum; i++)
	{
		if (linePtNum != (int)scanLinesInput[i].size())
			isGridData = false;

		scanLines[i].resize(scanLinesInput[i].size());
		std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}
	for (int i = 0; i < lineNum; i++)
	{																					 //行处理
		//调平，去除地面
		wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
	}

	//生成量化数据，以1mm为量化尺度，用于确定工件表面高度



	std::vector<std::vector<int>> pointMask;
	pointMask.resize(lineNum);

	std::vector<SVzNL3DPoint> endingPoints;
	//提取线段端点特征
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 1677)
			int kkk = 1;

		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
		pointMask[line].resize(lineData.size());
		std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//初始化为0
		//滤波，滤除异常点
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);

		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			lineData,
			lineSegPara,
			segs);
		//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
		for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
		{
			int ptIdx = segs[i].start;
			endingPoints.push_back(lineData[ptIdx].pt3D);
			pointMask[line][ptIdx] = 1;  //防止重复
			ptIdx = segs[i].start + segs[i].len - 1;
			endingPoints.push_back(lineData[ptIdx].pt3D);
			pointMask[line][ptIdx] = 1;
		}
	}

	//生成水平扫描
	std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
	hLines_raw.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		hLines_raw[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			hLines_raw[j][line] = scanLines[line][j];
			hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	//水平arc特征提取
	int lineNum_h_raw = (int)hLines_raw.size();
	for (int line = 0; line < lineNum_h_raw; line++)
	{
		if (line == 974)
			int kkk = 1;
		std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
		//滤波，滤除异常点
		int ptNum = (int)lineData.size();
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);

		std::vector<SSG_RUN> segs;
		wd_getLineDataIntervals(
			lineData,
			lineSegPara,
			segs);
		//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
		for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
		{
			int ptIdx = segs[i].start;
			if (pointMask[ptIdx][line] == 0)  //防止点重复
			{
				SVzNL3DPoint an_ending;
				an_ending.x = lineData[ptIdx].pt3D.y;
				an_ending.y = lineData[ptIdx].pt3D.x;
				an_ending.z = lineData[ptIdx].pt3D.z;
				endingPoints.push_back(an_ending);
				pointMask[ptIdx][line] = 1;
			}
			ptIdx = segs[i].start + segs[i].len - 1;
			if (pointMask[ptIdx][line] == 0)  //防止点重复
			{
				SVzNL3DPoint an_ending;
				an_ending.x = lineData[ptIdx].pt3D.y;
				an_ending.y = lineData[ptIdx].pt3D.x;
				an_ending.z = lineData[ptIdx].pt3D.z;
				endingPoints.push_back(an_ending);
				pointMask[ptIdx][line] = 1;
			}
		}
	}

	//标注
	std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
	std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
	featureInfoMask.resize(lineNum);
	feature3DInfo.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		featureInfoMask[i].resize(lineNum_h_raw);
		feature3DInfo[i].resize(lineNum_h_raw);
	}

	//标注
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<int>& a_lineMask = pointMask[line];
		for (int m = 0; m < lineNum_h_raw; m++)
		{
			if (a_lineMask[m] > 0)
			{
				SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[line][m];
				a_featureInfo.clusterID = 0;
				a_featureInfo.featurType = 1;
				a_featureInfo.featureIdx_v = 0;
				a_featureInfo.featureIdx_h = 0;
				a_featureInfo.lineIdx = line;
				a_featureInfo.ptIdx = m;
				a_featureInfo.flag = 0;
				feature3DInfo[line][m] = scanLines[line][m].pt3D;
			}
		}
	}
	//聚类
	//采用迭代思想，回归思路进行高效聚类
	std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
	std::vector<SVzNL3DRangeD> clustersRoi3D;
	int clusterID = 1;
	int clusterCheckWin = 5;
	for (int y = 0; y < lineNum_h_raw; 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);
			clustersRoi3D.push_back(a_clusterRoi);
			clusterID++;
		}
	}
	//聚类结果分析
	std::vector<int> validCluserIndexing;
	int clusterSize = (int)clusters.size();
	for (int i = 0; i < clusterSize; i++)
	{
		SVzNL3DRangeD& a_roi = clustersRoi3D[i];
		double L = a_roi.xRange.max - a_roi.xRange.min;
		double W = a_roi.yRange.max - a_roi.yRange.min;
		if ((L > workpiecePara.holeDiameter * 0.5) && (L < workpiecePara.holeDiameter * 2) &&
			(W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2))
			validCluserIndexing.push_back(i);
	}
	//生成结果
	std::vector< SWD_HoleInfo> holes;
	int objectSize = (int)validCluserIndexing.size();
	for (int objIdx = 0; objIdx < objectSize; objIdx++)
	{
		std::vector<SVzNL3DPoint> pointArray;
		int clusterIdx = validCluserIndexing[objIdx];
		//取cluster上的点
		int clusterPtSize = (int)clusters[clusterIdx].size();
		double minZ = DBL_MAX;
		for (int i = 0; i < clusterPtSize; i++)
		{
			SVzNL2DPoint a_pos = clusters[clusterIdx][i];
			SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[a_pos.x][a_pos.y];
			int lineIdx = a_featureInfo.lineIdx;
			int ptIdx = a_featureInfo.ptIdx;
			SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
			if (minZ > a_pt3d.z)
				minZ = a_pt3d.z;
			pointArray.push_back(a_pt3d);
		}
		//圆拟合
		SVzNL3DPoint center;
		double radius;
		double err = fitCircleByLeastSquare(pointArray,	center,	radius);
		center.z = minZ;
		SWD_HoleInfo a_hole;
		a_hole.center = { center.x, center.y, center.z };
		a_hole.radius = radius;
		holes.push_back(a_hole);
	}
	//分割
	//方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点
	double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效
	for (int objIdx = 0; objIdx < objectSize; objIdx++)
	{
		if (holes[objIdx].radius < 0)
			continue;

		holes[objIdx].radius = -1;
		SWD_HoleInfo& p0 = holes[objIdx];
		int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation);
		if (idx1 < 0)
			continue;

		SVzNLRangeD angleRange = { 85, 95 }; //垂直，5度范围
		SWD_HoleInfo& p1 = holes[idx1];
		//搜索最接近distance且角度为angle的目标, 以角度为优先
		int idx2 = angleConditionDistanceSearch(
			p0.center, p1.center,
			holes,
			workpiecePara.holeDist_L, distDeviation,
			angleRange);
		if (idx2 < 0)
			continue;

		SWD_HoleInfo& p2 = holes[idx2];
		//搜索最接近distance且角度为angle的目标, 以角度为优先
		int idx3 = angleConditionDistanceSearch(
			p1.center, p0.center,
			holes,
			workpiecePara.holeDist_L, distDeviation,
			angleRange);
		if (idx3 < 0)
			continue;
		SWD_HoleInfo& p3 = holes[idx3];
		p1.radius = -1;
		p2.radius = -1;
		p3.radius = -1;

		//重新计算Z值。因为沉孔的原因，Z值会不准确。取四条边的中点处的Z值的均值作为整个的Z值

		WD_workpieceInfo a_workpiece;
		a_workpiece.workpieceType = workpiecePara.workpieceType;
		a_workpiece.holes.push_back(p0.center);
		a_workpiece.holes.push_back(p1.center);
		a_workpiece.holes.push_back(p2.center);
		a_workpiece.holes.push_back(p3.center);
		a_workpiece.center = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
								(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
								(p0.center.z + p1.center.z + p2.center.z + p3.center.z) / 4 };

		SVzNL3DPoint y_dir;
		if (p0.center.y < p2.center.y)
			y_dir = { p0.center.x - p2.center.x, p0.center.y - p2.center.y, a_workpiece.center.z };
		else
			y_dir = { p2.center.x - p0.center.x, p2.center.y - p0.center.y, a_workpiece.center.z };
		a_workpiece.y_dir = { y_dir.x + a_workpiece.center.x, y_dir.y + a_workpiece.center.y, y_dir.z };
		a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z - 10 };
		workpiecePositioning.push_back(a_workpiece);
	}

	int workpieceNum = (int)workpiecePositioning.size();
	//旋转回去
	for (int i = 0; i < workpieceNum; i++)
	{
		SVzNL3DPoint rpt;
		rpt = _ptRotate(workpiecePositioning[i].center, groundCalibPara.invRMatrix);
		workpiecePositioning[i].center = rpt;
		rpt = _ptRotate(workpiecePositioning[i].y_dir, groundCalibPara.invRMatrix);
		workpiecePositioning[i].y_dir = rpt;
		rpt = _ptRotate(workpiecePositioning[i].z_dir, groundCalibPara.invRMatrix);
		workpiecePositioning[i].z_dir = rpt;
		for (int j = 0, j_max = (int)workpiecePositioning[i].holes.size(); j < j_max; j++)
		{
			rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix);
			workpiecePositioning[i].holes[j] = rpt;
		}
	}
	return;

}