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

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

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

SVzNL2DPoint getFootPoint(double x0, double y0, double k, double b)
{
	double A = k;
	double B = -1;
	double C = b;
	SVzNL2DPoint foot;
	foot.x = (B * B * x0 - A * B * y0 - A * C) / (A * A + B * B);
	foot.y = (-A * B * x0 + A * A * y0 - B * C) / (A * A + B * B);
	return foot;
}

double getWinMeanZ(std::vector<SVzNL3DPoint>& points, int startIdx, int win)
{
	int size = (int)points.size();
	double sumZ = 0;
	double sumSize = 0;
	for (int i = 0; i < win; i++)
	{
		int idx = i + startIdx;
		if (idx < size)
		{
			if (points[idx].z > 1e-4)
			{
				sumZ += points[idx].z;
				sumSize++;
			}
		}
	}
	if (sumSize > 0)
		sumZ = sumZ / (double)sumSize;
	return sumZ;
}
//从焊缝的扫描点获取焊缝的起点和终点信息
void getWeldPoint(std::vector<SVzNL3DPoint>& a_weld_contour, int midPtNum, std::vector<SVzNL3DPoint>& a_weld)
{
	if (a_weld_contour.size() == 0)
		return;

	//拟合直线
	double k, b;
	lineFitting(a_weld_contour, &k, &b);
	//寻找起点和终点
	SVzNL3DPoint startPt = a_weld_contour[0];
	SVzNL3DPoint endPt = a_weld_contour.back();
	SVzNL2DPoint foot_s = getFootPoint(startPt.x, startPt.y, k, b);
	SVzNL2DPoint foot_e = getFootPoint(endPt.x, endPt.y, k, b);
	double meanZ_s = getWinMeanZ(a_weld_contour, 0, 5);
	double meanZ_e = getWinMeanZ(a_weld_contour, (int)a_weld_contour.size()-1-5, 5);

	SVzNL3DPoint pt_0 = { foot_s.x, foot_s.y, meanZ_s };
	SVzNL3DPoint pt_1 = { foot_e.x, foot_e.y, meanZ_e };
	a_weld.push_back(pt_0);
	double ratio = 1.0 / ((double)midPtNum + 1.0);

	for (int i = 1; i <= midPtNum; i++)
	{
		SVzNL3DPoint a_pt;
		a_pt.x = (double)i * ratio * (pt_1.x - pt_0.x) + pt_0.x;
		a_pt.y = (double)i * ratio * (pt_1.y - pt_0.y) + pt_0.y;
		a_pt.z = (double)i * ratio * (pt_1.z - pt_0.z) + pt_0.z;
		a_weld.push_back(a_pt);
	}
	a_weld.push_back(pt_1);
}

//提取搭接焊缝
void sx_getLapWeldPostion(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	SSG_treeGrowParam growParam,
	SSX_lapWeldParam lapWeldParam,
	SSG_planeCalibPara groundCalibPara,
	std::vector<std::vector<SVzNL3DPoint>>& objOps,
	int* errCode)
{
	*errCode = 0;
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}

	int linePtNum = (int)scanLines[0].size();
	bool isGridData = true;
	//垂直跳变特征提取
	std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
	if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
	{
		for (int line = 0; line < lineNum; line++)
		{
			if (line == 400)
				int kkk = 1;

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

			std::vector<SSG_basicFeature1D> a_line_features;
			int dataSize = (int)lineData.size();
			sg_getLineJumpFeature_cornerMethod(
				&scanLines[line][0],
				dataSize,
				line,
				cornerPara, //scale通常取bagH的1/4
				a_line_features);
			//滤除地面
			std::vector<SSG_basicFeature1D> vld_features;
			for (int i = 0, i_max = a_line_features.size(); i < i_max; i++)
			{
				//将与地面形成的跳变去除
				if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25))
				{
					vld_features.push_back(a_line_features[i]);
				}
			}
			jumpFeatures_v.push_back(vld_features);
		}
	}

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

	//生成水平扫描
	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;
		}
	}
	//水平arc特征提取
	std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
	int lineNum_h = (int)hLines.size();
	if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
	{
		for (int line = 0; line < lineNum_h; line++)
		{
			std::vector<SVzNL3DPosition>& lineData = hLines[line];

			std::vector<SSG_basicFeature1D> a_line_features;
			int dataSize = (int)lineData.size();
			sg_getLineJumpFeature_cornerMethod(
				&hLines[line][0],
				dataSize,
				line,
				cornerPara, //scale通常取bagH的1/4
				a_line_features);

			//滤除地面
			std::vector<SSG_basicFeature1D> vld_features;
			for (int i = 0, i_max = a_line_features.size(); i < i_max; i++)
			{
				//将与地面形成的跳变去除
				if (a_line_features[i].featureValue < (groundCalibPara.planeHeight - 0.25))
				{
					vld_features.push_back(a_line_features[i]);
				}
			}
			jumpFeatures_h.push_back(vld_features);
		}
	}

	//特征生长
	//垂直方向特征生长（激光线方向）
	std::vector<SSG_featureTree> v_trees;
	if ((keSX_ScanMode_V == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
	{
		for (int line = 0; line < lineNum; line++)
		{
			bool isLastLine = false;
			if (line == lineNum - 1)
				isLastLine = true;
			std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v[line];
			if (a_lineJumpFeature.size() > 0)
				int kkk = 1;
			if (line == 400)
				int kkk = 1;
			sg_lineFeaturesGrowing(
				line,
				isLastLine,
				a_lineJumpFeature,
				v_trees,
				growParam);
		}
	}
	//水平方向特征生长（扫描运动方向）
	std::vector<SSG_featureTree> h_trees;
	if ((keSX_ScanMode_H == lapWeldParam.scanMode) || (keSX_ScanMode_Both == lapWeldParam.scanMode))
	{
		for (int line = 0; line < lineNum_h; line++)
		{
			if (line == 650)
				int kkk = 1;
			bool isLastLine = false;
			if (line == lineNum_h - 1)
				isLastLine = true;
			std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h[line];
			sg_lineFeaturesGrowing(
				line,
				isLastLine,
				a_lineJumpFeature,
				h_trees,
				growParam);
		}
	}

	//tree信息
	std::vector<SSG_treeInfo> allTreesInfo; //不包含边界
	SSG_treeInfo a_nullTree;
	memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
	allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
	//标记，根据起点的生长树进行标注
	int hvTreeIdx = 1;
	for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_vTree = &v_trees[i];

		//记录Tree的信息
		SSG_treeInfo a_treeInfo;
		a_treeInfo.vTreeFlag = 1;
		a_treeInfo.treeIdx = hvTreeIdx;
		a_treeInfo.treeType = a_vTree->treeType;
		a_treeInfo.sLineIdx = a_vTree->sLineIdx;
		a_treeInfo.eLineIdx = a_vTree->eLineIdx;
		a_treeInfo.roi = a_vTree->roi;
		allTreesInfo.push_back(a_treeInfo);

		std::vector<SVzNL3DPoint> a_weld_contour;
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
			if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16;
				if (existEdgeId == 0)
				{
					scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
					scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
					scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
				}
				a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D);
			}
		}
		std::vector<SVzNL3DPoint> a_weld;
		getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints,  a_weld);
		if (a_weld.size() > 0)
			objOps.push_back(a_weld);
		hvTreeIdx++;
	}
	int hTreeStart = hvTreeIdx;
	////标注:水平特征
	for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_hTree = &h_trees[i];
		//记录Tree的信息
		SSG_treeInfo a_treeInfo;
		a_treeInfo.vTreeFlag = 0;
		a_treeInfo.treeIdx = hvTreeIdx;
		a_treeInfo.treeType = a_hTree->treeType;
		a_treeInfo.sLineIdx = a_hTree->sLineIdx;
		a_treeInfo.eLineIdx = a_hTree->eLineIdx;
		a_treeInfo.roi.left = a_hTree->roi.top;  //水平扫描xy是交换的
		a_treeInfo.roi.right = a_hTree->roi.bottom;
		a_treeInfo.roi.top = a_hTree->roi.left;
		a_treeInfo.roi.bottom = a_hTree->roi.right;
		allTreesInfo.push_back(a_treeInfo);

		std::vector<SVzNL3DPoint> a_weld_contour;
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
			if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
				if (existEdgeId == 0)
				{
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
				}
				a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D);
			}
		}
		std::vector<SVzNL3DPoint> a_weld;
		getWeldPoint(a_weld_contour, lapWeldParam.weldRefPoints, a_weld);
		if (a_weld.size() > 0)
			objOps.push_back(a_weld);

		hvTreeIdx++;
	}
	int hvTreeSize = hvTreeIdx;

	//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
	for (int i = 0; i < lineNum; i++)
		sx_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1);
	//将检测结果重新投射回原来的坐标系
	for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++)
	{
		std::vector<SVzNL3DPoint>& a_weld = objOps[i];
		for (int j = 0, j_max = a_weld.size(); j < j_max; j++)
		{
			double x = a_weld[j].x * groundCalibPara.invRMatrix[0] +
				a_weld[j].y * groundCalibPara.invRMatrix[1] +
				a_weld[j].z * groundCalibPara.invRMatrix[2];
			double y = a_weld[j].x * groundCalibPara.invRMatrix[3] +
				a_weld[j].y * groundCalibPara.invRMatrix[4] +
				a_weld[j].z * groundCalibPara.invRMatrix[5];
			double z = a_weld[j].x * groundCalibPara.invRMatrix[6] +
				a_weld[j].y * groundCalibPara.invRMatrix[7] +
				a_weld[j].z * groundCalibPara.invRMatrix[8];
			a_weld[j].x = x;
			a_weld[j].y = y;
			a_weld[j].z = z;
		}
	}

	//输出结果
}