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

void wd_noiseFilter(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_outlierFilterParam filterParam,
	int* errCode)
{
	*errCode = 0;
	int lineNum = (int)scanLines.size();
	int nPointCnt = (int)scanLines[0].size();
	bool vldGrid = true;
	//垂直方向过滤
	for (int i = 0; i < lineNum; i++)
	{
		if (nPointCnt != (int)scanLines[i].size())
			vldGrid = false;
		wd_vectorDataRemoveOutlier_overwrite(
			scanLines[i],
			filterParam);
	}
	if (false == vldGrid)
	{
		*errCode = SG_ERR_3D_DATA_INVLD;
		return;
	}
	//水平方向过滤
	int hLineNum = nPointCnt;  //Grid格式，所有扫描线的点数是一样的
	//生成水平扫描数据
	std::vector<std::vector<SVzNL3DPosition>> filterHLines;
	filterHLines.resize(hLineNum);
	for (int i = 0; i < hLineNum; i++)
		filterHLines[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < hLineNum; j++)
		{
			filterHLines[j][line] = scanLines[line][j];
			filterHLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			filterHLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	for (int hLine = 0; hLine < hLineNum; hLine++)
	{
		//滤波，滤除异常点
		std::vector<SVzNL3DPosition> filterData;
		std::vector<int> lineNoise;
		sg_lineDataRemoveOutlier(
			(SVzNL3DPosition*)filterHLines[hLine].data(),
			(int)filterHLines[hLine].size(),
			filterParam,
			filterData,
			lineNoise);
		for (int j = 0; j < lineNoise.size(); j++)
		{
			int lineIdx = lineNoise[j];
			scanLines[lineIdx][hLine].pt3D.z = 0;
		}
	}
	return;
}