#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "rodAndBarDetection_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_rodAndBarDetectionVersion(void)
{
	return m_strVersion.c_str();
}

SVzNL3DPoint getArcPeak(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines, 
	SWD_segFeature & a_arcFeature)
{
	SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D;
	for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++)
	{
		if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
		{
			if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z)
				arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
		}
	}
	return arcPeak;
}

//投影，提取ROI内的数据
void xoyROIProjection(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const double* rtMatrix,
	SSG_ROIRectD& roi_xoy,
	std::vector<SVzNL3DPoint>& projectPoints
	)
{
	int lineNum = (int)scanLines.size();
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SVzNL3DPosition>& a_line = scanLines[line];
		int ptNum = (int)a_line.size();
		for (int i = 0; i < (int)a_line.size(); i++)
		{
			SVzNL3DPoint a_pt = a_line[i].pt3D;
			if (a_pt.z < 1e-4)
				continue;
			double x = a_pt.x * rtMatrix[0] + a_pt.y * rtMatrix[1] + a_pt.z * rtMatrix[2];
			double y = a_pt.x * rtMatrix[3] + a_pt.y * rtMatrix[4] + a_pt.z * rtMatrix[5];
			double z = a_pt.x * rtMatrix[6] + a_pt.y * rtMatrix[7] + a_pt.z * rtMatrix[8];
			if ((x >= roi_xoy.left) && (x <= roi_xoy.right) &&
				(y >= roi_xoy.top) && (y <= roi_xoy.bottom))
			{
				a_pt.x = x;
				a_pt.y = y;
				a_pt.z = z;
				projectPoints.push_back(a_pt);
			}
		}
	}
}

SVzNLRangeD getZRange(std::vector<SVzNL3DPoint>& projectPoints)
{
	int ptNum = (int)projectPoints.size();
	SVzNLRangeD zRange;
	zRange.min = DBL_MAX;
	zRange.max = DBL_MIN;
	for (int i = 0; i < ptNum; i++)
	{
		zRange.min = zRange.min > projectPoints[i].z ? projectPoints[i].z : zRange.min;
		zRange.max = zRange.max < projectPoints[i].z ? projectPoints[i].z : zRange.max;
	}
	return zRange;
}

void zCutPointClouds(
	std::vector<SVzNL3DPoint>& projectPoints, 
	SVzNLRangeD& zRange, 
	std::vector<SVzNL3DPoint>& cutLayerPoints)
{
	int ptNum = (int)projectPoints.size();
	for (int i = 0; i < ptNum; i++)
	{
		if ((projectPoints[i].z >= zRange.min) && (projectPoints[i].z <= zRange.max))
			cutLayerPoints.push_back(projectPoints[i]);
	}
}

SVzNL3DPoint getXoYCentroid(std::vector<SVzNL3DPoint>& points)
{
	int ptNum = (int)points.size();
	SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 };
	if (ptNum == 0)
		return centroid;
	for (int i = 0; i < ptNum; i++)
	{
		centroid.x += points[i].x;
		centroid.y += points[i].y;
	}
	centroid.x = centroid.x / ptNum;
	centroid.y = centroid.y / ptNum;
	return centroid;
}

SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, 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;
}

 void sx_hexHeadScrewMeasure(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	bool isHorizonScan,  //true:激光线平行槽道；false:激光线垂直槽道
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	double rodRidius,
	std::vector<SSX_hexHeadScrewInfo>& screwInfo,
	int* errCode)
{
	*errCode = 0;
	SSX_hexHeadScrewInfo screwInfo;
	memset(&screwInfo, 0, sizeof(SSX_hexHeadScrewInfo));
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		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;
	}

	std::vector< std::vector<SVzNL3DPosition>> data_lines;
	if (false == isHorizonScan)
	{
		data_lines.resize(lineNum);
		for (int line = 0; line < lineNum; line++)
		{
			data_lines[line].insert(data_lines[line].end(), scanLines[line].begin(), scanLines[line].end());
			for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
			{
				data_lines[line][j].nPointIdx = j;
				scanLines[line][j].nPointIdx = 0; //转义复用
			}
		}
	}
	else
	{
		data_lines.resize(linePtNum);
		for (int i = 0; i < linePtNum; i++)
			data_lines[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[j][line] = scanLines[line][j];
				data_lines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
				data_lines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
			}

		}

		lineNum = linePtNum;
		linePtNum = (int)data_lines[0].size();
		for (int line = 0; line < lineNum; line++)
		{
			for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
				data_lines[line][j].nPointIdx = j;
		}
	}

	std::vector<std::vector<SWD_segFeature>> arcFeatures;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 44)
			int kkk = 1;
		std::vector<SVzNL3DPosition>& lineData = data_lines[line];

		//滤波，滤除异常点
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
		std::vector<SWD_segFeature> line_ringArcs;
		int dataSize = (int)lineData.size();
		//提取Arc特征
		wd_getRingArcFeature(
			lineData,
			line, //当前扫描线序号
			cornerPara,
			rodRidius / 2, //环宽度
			line_ringArcs  //环
		);
		arcFeatures.push_back(line_ringArcs);
	}
	//特征生长
	std::vector<SWD_segFeatureTree> growTrees;
	wd_getSegFeatureGrowingTrees(
		arcFeatures,
		growTrees,
		growParam);

	if (growTrees.size() == 0)
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	int objNum = (int)growTrees.size();
	for (int i = 0; i < objNum; i++)
	{
		//空间直线拟合
		std::vector<SVzNL3DPoint> fitPoints;
		int nodeSize = (int)growTrees[i].treeNodes.size();
		for (int j = 0; j < nodeSize; j++)
		{
			SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j]);
			fitPoints.push_back(a_pt);
		}
		if (fitPoints.size() < 12)
			continue;
		//去除头尾各5个点，防止在端部和根部扫描时的数据有干扰
		fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 5);
		fitPoints.erase(fitPoints.end() - 5, fitPoints.end());
		//拟合
		SVzNL3DPoint P0_center, P1_dir;
		bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir);
		if (false == result)
			continue;
		//投影
		//计算旋转向量
		SVzNL3DPoint vector1 = P1_dir;
		SVzNL3DPoint vector2 = { 0, 0, -1.0 };
		SSG_planeCalibPara rotatePara = wd_conputeRTMatrix(	vector1, vector2);
		//
		SSG_ROIRectD roi_xoy;
		roi_xoy.left = P0_center.x - rodRidius * 4; //2D范围
		roi_xoy.right = P0_center.x + rodRidius * 4; //2D范围
		roi_xoy.top = P0_center.y - rodRidius * 4; //2D范围
		roi_xoy.bottom = P0_center.y + rodRidius * 4; //2D范围
		std::vector< SVzNL3DPoint> roiProjectionData;
		xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
		//取端面
		SVzNLRangeD zRange = getZRange(roiProjectionData);
		SVzNLRangeD cutZRange;
		cutZRange.min = zRange.min;
		cutZRange.max = zRange.min + 10.0;  //取10mm的端面
		std::vector<SVzNL3DPoint> surfacePoints;
		zCutPointClouds(roiProjectionData, 	cutZRange, surfacePoints);
		//计算中心点
		SVzNL3DPoint projectionCenter = getXoYCentroid(surfacePoints);
		projectionCenter.z = zRange.min;
		//旋转回原坐标系
		SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
		//生成Rod信息
		SSX_hexHeadScrewInfo a_rod;
		a_rod.center = surfaceCenter;
		a_rod.axialDir = P1_dir;
		a_rod.rotateAngle = 0;
		screwInfo.push_back(a_rod);
	}
	if (true == isHorizonScan)
	{
		int objNum = (int)screwInfo.size();
		for (int i = 0; i < objNum; i++)
		{
			double tmp = screwInfo[i].center.x;
			screwInfo[i].center.x = screwInfo[i].center.y;
			screwInfo[i].center.y = tmp;
			tmp = screwInfo[i].axialDir.x;
			screwInfo[i].axialDir.x = screwInfo[i].axialDir.y;
			screwInfo[i].axialDir.y = tmp;
			//screwInfo[i].rotateAngle += 90;
		}
	}
	return;
}