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

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

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

int _counterLinePtNum(std::vector<SVzNL3DPosition>& lineData)
{
	int ptNum = 0;
	for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++)
	{
		if ((abs(lineData[i].pt3D.z) > 1e-4) ||
			(abs(lineData[i].pt3D.x > 1e-4)) ||
			(abs(lineData[i].pt3D.y > 1e-4)))
			ptNum++;
	}
	return ptNum;
}

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

SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	SSG_treeGrowParam growParam,
	SSG_planeCalibPara groundCalibPara,
	SSX_BQAssemblyPara assemblyParam,
	int* errCode)
{
	*errCode = 0;
	SSX_BQAssemblyInfo assemblyPose;
	memset(&assemblyPose, 0, sizeof(SSX_BQAssemblyInfo));
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return assemblyPose;
	}

	//将开始和结束的空白扫描线去除，获得扫描边界
	int validStartLine = -1;
	for (int i = 0; i < lineNum; i++)
	{
		int linePtNum = _counterLinePtNum(scanLines[i]);
		if (linePtNum > 0)
		{
			validStartLine = i;
			break;
		}
	}
	int validEndLine = -1;
	for (int i = lineNum - 1; i >= 0; i--)
	{
		int linePtNum = _counterLinePtNum(scanLines[i]);
		if (linePtNum > 0)
		{
			validEndLine = i;
			break;
		}
	}
	if ((validStartLine < 0) || (validEndLine < 0))
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return assemblyPose;
	}

	int linePtNum = (int)scanLines[0].size();
	bool isGridData = true;

	//自适应各种旋转角度
	//垂直跳变特征提取
	std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v_raw;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 250)
			int kkk = 1;

		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
		if (linePtNum != (int)lineData.size())
			isGridData = false;

		//滤波，滤除异常点
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);

		std::vector<SSG_basicFeature1D> line_features;
		int dataSize = (int)lineData.size();
		sg_getLineCornerFeature_BQ(
			&lineData[0],
			dataSize,
			line,
			groundCalibPara.planeHeight,
			cornerPara, //scale通常取bagH的1/4
			line_features);
		jumpFeatures_v_raw.push_back(line_features);
	}

	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return assemblyPose;
	}

	//生成水平扫描
	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特征提取
	std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h_raw;
	int lineNum_h_raw = (int)hLines_raw.size();
	for (int line = 0; line < lineNum_h_raw; line++)
	{
		if (line == 416)
			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_basicFeature1D> line_features;
		int dataSize = (int)lineData.size();
		sg_getLineCornerFeature_BQ(
			&hLines_raw[line][0],
			dataSize,
			line,
			groundCalibPara.planeHeight,
			cornerPara, //scale通常取bagH的1/4
			line_features);
		jumpFeatures_h_raw.push_back(line_features);
	}

	//特征生长，用于滤除噪点
	//垂直方向特征生长（激光线方向）
	std::vector<SSG_featureTree> v_trees;
	for (int line = 0; line < lineNum; line++)
	{
		bool isLastLine = false;
		if (line == lineNum - 1)
			isLastLine = true;
		std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v_raw[line];
		if (a_lineJumpFeature.size() > 0)
			int kkk = 1;
		if (line == 202)
			int kkk = 1;
		sg_lineFeaturesGrowing(
			line,
			isLastLine,
			a_lineJumpFeature,
			v_trees,
			growParam);
	}

	//水平方向特征生长（扫描运动方向）
	std::vector<SSG_featureTree> h_trees;
	for (int line = 0; line < lineNum_h_raw; line++)
	{
		if (line == 650)
			int kkk = 1;
		bool isLastLine = false;
		if (line == lineNum_h_raw - 1)
			isLastLine = true;
		std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h_raw[line];
		sg_lineFeaturesGrowing(
			line,
			isLastLine,
			a_lineJumpFeature,
			h_trees,
			growParam);
	}

	std::vector<SWD_polarPt> polarPoints;
	for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_vTree = &v_trees[i];
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
		{
			int lineIdx = a_vTree->treeNodes[j].jumpPos2D.x;
			int ptIdx = a_vTree->treeNodes[j].jumpPos2D.y;
			if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
			{
				SWD_polarPt a_polarPt;
				a_polarPt.lineIdx = lineIdx;
				a_polarPt.ptIdx = ptIdx;
				a_polarPt.R = 0;
				a_polarPt.angle = 0;
				a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
				a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
				a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
				polarPoints.push_back(a_polarPt);
				scanLines[lineIdx][ptIdx].nPointIdx = -1;
			}
		}
	}
	for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_hTree = &h_trees[i];
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
		{
			int lineIdx = a_hTree->treeNodes[j].jumpPos2D.y;
			int ptIdx = a_hTree->treeNodes[j].jumpPos2D.x;
			if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
			{
				SWD_polarPt a_polarPt;
				a_polarPt.lineIdx = lineIdx;
				a_polarPt.ptIdx = ptIdx;
				a_polarPt.cptIndex = -1;
				a_polarPt.R = 0;
				a_polarPt.angle = 0;
				a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
				a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
				a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
				polarPoints.push_back(a_polarPt);
				scanLines[lineIdx][ptIdx].nPointIdx = -1;
			}
		}
	}

	//计算几何中心
	int contourPtSize = (int)polarPoints.size();
	if (contourPtSize == 0)
	{
		*errCode = SX_ERR_ZERO_CONTOUR_PT;
		return assemblyPose;
	}
	double center_x = 0;
	double center_y = 0;
	for (int pi = 0; pi < contourPtSize; pi++)
	{
		center_x += polarPoints[pi].x;
		center_y += polarPoints[pi].y;
	}
	center_x = center_x / (double)contourPtSize;
	center_y = center_y / (double)contourPtSize;
	//计算极坐标的R和Theta
	for (int pi = 0; pi < contourPtSize; pi++)
	{
		double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x);
		angle = (angle / PI) * 180 + 180.0;
		double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2));
		polarPoints[pi].R = R;
		polarPoints[pi].angle = angle;
	}
	//按角度大小排序
	std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle);
	for (int pi = 0; pi < contourPtSize; pi++)
		polarPoints[pi].cptIndex = pi;  // index

	//提取R极值点
	double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支
	int minRPos = -1;
	std::vector<SWD_polarPt> polarRPeakPts;
	int winSize = contourPtSize / 36;  //+-10度范围
	if (winSize < 5)
		winSize = 5;
	for (int pi = 0; pi < contourPtSize; pi++)
	{
		double currR = polarPoints[pi].R;
		if (minR < 0)
		{
			minR = currR;
			maxR = currR;
			minRPos = pi;
		}
		else
		{
			minRPos = minR > currR ? pi : minRPos;
			minR = minR > currR ? currR : minR;
			maxR = maxR < currR ? currR : maxR;
		}
		bool isPeak = true;
		for (int k = -winSize; k <= winSize; k++)
		{
			int idx = (pi + k + contourPtSize) % contourPtSize; //筒形结构
			if (polarPoints[idx].R > currR)
			{
				isPeak = false;
				break;
			}
		}
		if (true == isPeak)
			polarRPeakPts.push_back(polarPoints[pi]);
	}

	double ratio_MaxMin = maxR / minR;
	bool hasBranch = ratio_MaxMin < 1.25 ? false : true;

	std::vector<SWD_polarPt> validPolarRPeakPts;
	std::vector<SWD_polarPeakInfo> polarPeakInfo;
	int pkId = 0;
	//过滤圆弧段的极值：由于重心偏移，圆弧段也会形成极值。根据极值两边L=直线段长度构成的张角判断
	double arcAngleChkLen = 100; //检测圆弧张角的尺度
	for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++)
	{
		int ptidx = polarRPeakPts[i].cptIndex;
		double px, py, pz;
		px = polarRPeakPts[i].x;
		py = polarRPeakPts[i].y;

		int LL1 = -1;
		int halfLL1 = -1;
		for (int j = ptidx - 1; j > -contourPtSize; j--)
		{
			int idx = (j + contourPtSize) % contourPtSize; //筒形结构
			double cx = polarPoints[idx].x;
			double cy = polarPoints[idx].y;
			double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
			if (len < arcAngleChkLen)
				halfLL1 = idx;
			if (len > (assemblyParam.lineLen))
			{
				LL1 = idx;
				break;
			}
		}

		int LL2 = -1;
		int halfLL2 = -1;
		for (int j = ptidx + 1; j < contourPtSize * 2; j++)
		{
			int idx = j % contourPtSize; //筒形结构
			double cx = polarPoints[idx].x;
			double cy = polarPoints[idx].y;
			double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
			if (len < arcAngleChkLen)
				halfLL2 = idx;
			if (len > (assemblyParam.lineLen))
			{
				LL2 = idx;
				break;
			}
		}

		if ((LL1 >= 0) && (LL2 >= 0))
		{
			double len1 = sqrt(pow(px - polarPoints[halfLL1].x, 2) + pow(py - polarPoints[halfLL1].y, 2));
			double len2 = sqrt(pow(px - polarPoints[halfLL2].x, 2) + pow(py - polarPoints[halfLL2].y, 2));
			double len3 = sqrt(pow(polarPoints[halfLL1].x - polarPoints[halfLL2].x, 2) +
				pow(polarPoints[halfLL1].y - polarPoints[halfLL2].y, 2));
			double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2);
			double theta = acos(cosTheta) * 180.0 / PI;
			if (theta < 150)
			{
				SWD_polarPeakInfo a_pkInfo;
				a_pkInfo.cptIndex = ptidx;
				a_pkInfo.L1_ptIndex = LL1;
				a_pkInfo.L2_ptIndex = LL2;
				a_pkInfo.cornerAngle = theta;
				a_pkInfo.cornerDir = 0;
				polarRPeakPts[i].cptIndex = ptidx;
				polarRPeakPts[i].pkId = pkId;
				pkId++;
				validPolarRPeakPts.push_back(polarRPeakPts[i]);
				polarPeakInfo.push_back(a_pkInfo);
			}
		}
	}
	//处理矩形工件
	if (validPolarRPeakPts.size() != 4)
	{
		*errCode = SX_ERR_INVLID_RPEAK_NUM;
		return assemblyPose;
	}

	//取长边作X轴，短边作Y轴，法向为Z轴，中心点为O点
	std::vector<SVzNL3DPoint> sidePts_1;
	std::vector<SVzNL3DPoint> sidePts_2;
	std::vector<SVzNL3DPoint> sidePts_3;
	std::vector<SVzNL3DPoint> sidePts_4;
	return assemblyPose;
}

//根据Mark计算工件位置和姿态
SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFromMark(
	SSX_BQAssemblyInfo originPos,
	std::vector<cv::Point3d> originMarkPos,
	std::vector<cv::Point3d> currMarkPos,
	int* errCode)
{
	*errCode = 0;
	SSX_BQAssemblyInfo resultPos;
	memset(&resultPos, 0, sizeof(SSX_BQAssemblyInfo));
	if ((originMarkPos.size() < 3) || (currMarkPos.size() < 3))
	{
		*errCode = SX_ERR_INVLID_MARK_NUM;
		return resultPos;
	}

	cv::Mat R, T; //旋转平移
	cv::Point3d C_origin, C_curr;  //质心
	caculateRT(originMarkPos, currMarkPos, R, T, C_origin, C_curr);

	// 5. 推算Pn的坐标
	pointRT(R, T, C_origin, C_curr, originPos.O, resultPos.O);
	pointRT(R, T, C_origin, C_curr, originPos.X, resultPos.X);
	pointRT(R, T, C_origin, C_curr, originPos.Y, resultPos.Y);
	pointRT(R, T, C_origin, C_curr, originPos.Z, resultPos.Z);
	return resultPos;
}