#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include <vector>

void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt)
{
	if (a_pt.z > 1E-4)
	{
		if (roi.left < 0)
		{
			roi.left = a_pt.x;
			roi.right = a_pt.x;
			roi.left = a_pt.y;
			roi.right = a_pt.y;
		}
		else
		{
			if (roi.left > a_pt.x)
				roi.left = a_pt.x;
			if (roi.right < a_pt.x)
				roi.right = a_pt.x;
			if (roi.top > a_pt.y)
				roi.top = a_pt.y;
			if (roi.bottom < a_pt.y)
				roi.bottom = a_pt.y;
		}
	}
	return;
}

void _updateRenge(SVzNLRange& range, int idx)
{
	if (range.nMin < 0)
	{
		range.nMin = idx;
		range.nMax = idx;
	}
	else
	{
		if (range.nMin > idx)
			range.nMin = idx;
		if (range.nMax < idx)
			range.nMax = idx;
	}
	return;
}

bool _checkExist(int id, std::vector<int>& buff)
{
	for (int i = 0; i < (int)buff.size(); i++)
	{
		if (id == buff[i])
			return true;
	}
	return false;
}

void _searchNeighbours(
	int selfId, int chkExtening,
	int sLineIdx, int eLineIdx,
	SVzNLRange ptIdxRange,
	std::vector<std::vector<int>>& treeMask,
	std::vector<int>& neighbours)
{
	int lineNum = (int)treeMask.size();
	int ptNum = treeMask[0].size();
	for (int line = sLineIdx - chkExtening; line <= eLineIdx + chkExtening; line++)
	{
		if ((line >= 0) && (line < lineNum))
		{
			for (int ptIdx = ptIdxRange.nMin - chkExtening; ptIdx <= ptIdxRange.nMax + chkExtening; ptIdx++)
			{
				if ((ptIdx >= 0) && (ptIdx < ptNum))
				{
					if ((treeMask[line][ptIdx] >= 0) && (treeMask[line][ptIdx] != selfId))
					{
						bool isExist = _checkExist(treeMask[line][ptIdx], neighbours);
						if (false == isExist)
							neighbours.push_back(treeMask[line][ptIdx]);
					}
				}
			}
		}
	}
	return;
}

void WD_getHoleInfo(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_lineSegParam lineSegPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	const double valieCommonNumRatio,
	std::vector<SWD_segFeatureTree>& segTrees_v,
	std::vector<SWD_segFeatureTree>& segTrees_h,
	std::vector<SSG_intPair>& validObjects
)
{
	int lineNum = (int)scanLines.size();
	int linePtNum = (int)scanLines[0].size();

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

	//提取空白线段特征（孔特征）
	std::vector<std::vector<SWD_segFeature>> holeGaps;
	//提取线段端点特征
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 1047)
			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_getLineDataNullIntervals(lineData, lineSegPara, segs);

		//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
		std::vector<SWD_segFeature> line_gaps;
		for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
		{
			int ptIdx_1 = segs[i].start;
			int ptIdx_2 = segs[i].start + segs[i].len - 1;
			SWD_segFeature a_gap;
			a_gap.lineIdx = line;
			a_gap.startPtIdx = ptIdx_1;
			a_gap.endPtIdx = ptIdx_2;
			a_gap.startPt = lineData[ptIdx_1].pt3D;
			a_gap.endPt = lineData[ptIdx_2].pt3D;
			a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
			line_gaps.push_back(a_gap);
		}
		holeGaps.push_back(line_gaps);
	}
	//特征生长
	wd_getSegFeatureGrowingTrees_2(holeGaps, segTrees_v, growParam);

	//生成水平扫描
	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<SWD_segFeature>> holeGaps_h;
	for (int line = 0; line < linePtNum; 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_getLineDataNullIntervals(lineData, lineSegPara, segs);

		//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
		std::vector<SWD_segFeature> line_gaps;
		for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
		{
			int ptIdx_1 = segs[i].start;
			int ptIdx_2 = segs[i].start + segs[i].len - 1;
			SWD_segFeature a_gap;
			a_gap.lineIdx = line;
			a_gap.startPtIdx = ptIdx_1;
			a_gap.endPtIdx = ptIdx_2;
			a_gap.startPt = lineData[ptIdx_1].pt3D;
			a_gap.endPt = lineData[ptIdx_2].pt3D;
			a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
			line_gaps.push_back(a_gap);
		}
		holeGaps_h.push_back(line_gaps);
	}
	//特征生长
	wd_getSegFeatureGrowingTrees_2(holeGaps_h, segTrees_h, growParam);

	//创建Tree所在孔洞的Mask
	std::vector<std::vector<int>> treeMask_v;
	treeMask_v.resize(lineNum);
	std::vector<std::vector<int>> treeMask_h;
	treeMask_h.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		treeMask_v[i].resize(linePtNum);
		std::fill(treeMask_v[i].begin(), treeMask_v[i].end(), -1);
		treeMask_h[i].resize(linePtNum);
		std::fill(treeMask_h[i].begin(), treeMask_h[i].end(), -1);
	}
	//标注
	std::vector<SSG_treeInfo> treeInfo_v;
	treeInfo_v.resize(segTrees_v.size());
	for (int i = 0; i < (int)segTrees_v.size(); i++)
	{
		SWD_segFeatureTree& a_tree = segTrees_v[i];
		treeInfo_v[i].treeIdx = i;
		treeInfo_v[i].sLineIdx = a_tree.sLineIdx;
		treeInfo_v[i].eLineIdx = a_tree.eLineIdx;
		treeInfo_v[i].vTreeFlag = 0;
		treeInfo_v[i].treeType = 0;
		treeInfo_v[i].roi = { -1, -1, -1, -1 };
		int nullPtSize = 0;
		SSG_ROIRectD roi = { -1, -1, -1, -1 };
		SVzNLRange ptIdxRange = { -1, -1 };
		if (a_tree.treeNodes.size() > 0)
		{
			for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
			{
				SWD_segFeature& a_seg = a_tree.treeNodes[m];
				for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
					treeMask_v[a_seg.lineIdx][n] = i;

				nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
				_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
				_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
				_updateRenge(ptIdxRange, a_seg.startPtIdx);
				_updateRenge(ptIdxRange, a_seg.endPtIdx);
				//scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = 0x01;
				//scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = 0x01;
			}
			treeInfo_v[i].treeType = nullPtSize;
			treeInfo_v[i].roi = roi;
			treeInfo_v[i].ptIdxRange = ptIdxRange;
		}
	}
	std::vector<SSG_treeInfo> treeInfo_h;
	treeInfo_h.resize(segTrees_h.size());
	for (int i = 0; i < (int)segTrees_h.size(); i++)
	{
		SWD_segFeatureTree& a_tree = segTrees_h[i];
		treeInfo_h[i].treeIdx = i;
		treeInfo_h[i].sLineIdx = a_tree.sLineIdx;
		treeInfo_h[i].eLineIdx = a_tree.eLineIdx;
		treeInfo_h[i].vTreeFlag = 0;
		treeInfo_h[i].treeType = 0;
		treeInfo_h[i].roi = { -1, -1, -1, -1 };
		int nullPtSize = 0;
		SSG_ROIRectD roi = { -1, -1, -1, -1 };
		SVzNLRange ptIdxRange = { -1, -1 };
		if (a_tree.treeNodes.size() > 0)
		{
			for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
			{
				SWD_segFeature& a_seg = a_tree.treeNodes[m];
				for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
					treeMask_h[n][a_seg.lineIdx] = i;

				nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
				_updateROI(roi, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
				_updateROI(roi, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
				_updateRenge(ptIdxRange, a_seg.startPtIdx);
				_updateRenge(ptIdxRange, a_seg.endPtIdx);
				//scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
				//scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
			}
			treeInfo_h[i].treeType = nullPtSize;
			treeInfo_h[i].roi = roi;
			treeInfo_h[i].ptIdxRange = ptIdxRange;
		}
	}
	//水平和垂直目标合并
	int vTreeSize = (int)segTrees_v.size();
	int hTreeSize = (int)segTrees_h.size();
	std::vector<std::vector<int>> treeHVInfo;  //统计垂直和水平的tree的位置信息
	treeHVInfo.resize(vTreeSize);
	for (int i = 0; i < vTreeSize; i++)
	{
		treeHVInfo[i].resize(hTreeSize);
		std::fill(treeHVInfo[i].begin(), treeHVInfo[i].end(), 0);
	}
	for (int line = 0; line < lineNum; line++)
	{
		for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
		{
			int idx_v = treeMask_v[line][ptIdx];
			int idx_h = treeMask_h[line][ptIdx];
			if ((idx_v >= 0) && (idx_h >= 0))
				treeHVInfo[idx_v][idx_h]++;
		}
	}

	//生成候选目标
	std::vector<SSG_intPair> objects;
	for (int i = 0; i < vTreeSize; i++)
	{
		SWD_segFeatureTree& a_tree = segTrees_v[i];
		if ((a_tree.sLineIdx <= 1047) && (a_tree.eLineIdx >= 1047))
			int kkk = 1;

		int totalSize_v = treeInfo_v[i].treeType;
		int commonSize = 0;
		int hTreeIdx = -1;
		for (int j = 0; j < hTreeSize; j++)
		{
			if (commonSize < treeHVInfo[i][j])
			{
				hTreeIdx = j;
				commonSize = treeHVInfo[i][j];
			}
		}
		if (hTreeIdx >= 0)
		{
			int totalSize_h = treeInfo_h[hTreeIdx].treeType;
			int sizeV_th = (int)((double)totalSize_v * valieCommonNumRatio);
			int sizeH_th = (int)((double)totalSize_h * valieCommonNumRatio);
			if ((commonSize > sizeH_th) && (commonSize > sizeV_th))
			{
				SSG_intPair a_obj;
				a_obj.data_0 = i;
				a_obj.data_1 = hTreeIdx;
				a_obj.idx = commonSize;
				objects.push_back(a_obj);
				treeInfo_v[i].data = commonSize;
				treeInfo_h[hTreeIdx].data = commonSize;
			}
		}
	}

	//滤除相邻。每个目标保留一个
	for (int i = 0; i < (int)objects.size(); i++)
	{
		int vTreeIdx = objects[i].data_0;
		if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
			continue;

		std::vector<int> neighbours;
		_searchNeighbours(vTreeIdx, 3,
			treeInfo_v[vTreeIdx].sLineIdx, treeInfo_v[vTreeIdx].eLineIdx,
			treeInfo_v[vTreeIdx].ptIdxRange, treeMask_v, neighbours);

		int bestIdx = vTreeIdx;
		int maxValue = treeInfo_v[vTreeIdx].data;
		if (neighbours.size() > 0)
		{
			for (int j = 0; j < (int)neighbours.size(); j++)
			{
				int idx = neighbours[j];
				if (maxValue < treeInfo_v[idx].data)
				{
					maxValue = treeInfo_v[idx].data;
					bestIdx = idx;
				}
			}
			if (bestIdx != vTreeIdx)
				treeInfo_v[vTreeIdx].vTreeFlag = -1;
			for (int j = 0; j < (int)neighbours.size(); j++)
			{
				int idx = neighbours[j];
				if (bestIdx != idx)
					treeInfo_v[idx].vTreeFlag = -1;
			}
		}
	}

	for (int i = 0; i < (int)objects.size(); i++)
	{
		int vTreeIdx = objects[i].data_0;
		if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
			continue;

		validObjects.push_back(objects[i]);
	}
}