#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;
}

void wd_pointCloudHoleFilling(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines_src,
	std::vector< std::vector<SVzNL3DPosition>>& scanLines_dst,
	const int fillingHoleSize //填孔大小
)
{
	int lineNum = (int)scanLines_src.size();
	scanLines_dst.resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		int ptNum = (int)scanLines_src[line].size();
		scanLines_dst[line].resize(ptNum);
		for (int i = 0; i < ptNum; i++)
		{
			scanLines_dst[line][i] = scanLines_src[line][i];
			scanLines_src[line][i].nPointIdx = 0;  //未填充
		}
	}

	for (int line = 0; line < lineNum; line++)
	{
		int ptNum = (int)scanLines_src[line].size();
		for (int i = 0; i < ptNum; i++)
		{
			SVzNL3DPosition& a_pt = scanLines_src[line][i];
			if ( (a_pt.pt3D.z < 1e-4) && (a_pt.nPointIdx == 0))  //未填充的空白点
			{
				//检查是否需要填充
				//扫描线方向
				int j_top = -1;
				for (int j = i - 1; j >= i - fillingHoleSize; j--)
				{
					if (j < 0)
						break;
					else if (scanLines_src[line][j].pt3D.z > 1e-4)
					{
						j_top = j;
						break;
					}
				}
				int j_btm = -1;
				for (int j = i + 1; j <= i + fillingHoleSize; j++)
				{
					if (j >= ptNum)
						break;
					else if (scanLines_src[line][j].pt3D.z > 1e-4)
					{
						j_btm = j;
						break;
					}
				}
				if ((j_top >= 0) && (j_btm >= 0))
				{
					SVzNL3DPoint& top_pt = scanLines_src[line][j_top].pt3D;
					SVzNL3DPoint& btm_pt = scanLines_src[line][j_btm].pt3D;
					//插值
					double dist = (double)(j_btm - j_top);
					for (int j = j_top + 1; j <= j_btm - 1; j++)
					{
						double d = (double)(j - j_top);
						double k2 = d / dist;
						double k1 = 1.0 - k2;
						scanLines_dst[line][j].pt3D = { top_pt.x * k1 + btm_pt.x * k2, top_pt.y * k1 + btm_pt.y * k2 , top_pt.z * k1 + btm_pt.z * k2 };
						scanLines_src[line][j].nPointIdx = 1;
					}
				}
				else
				{
					//检查水平方向
					int line_left = -1;
					for (int j = line - 1; j >= line - fillingHoleSize; j--)
					{
						if (j < 0)
							break;
						else if (scanLines_src[j][i].pt3D.z > 1e-4)
						{
							line_left = j;
							break;
						}
					}
					int line_right = -1;
					for (int j = line + 1; j <= line + fillingHoleSize; j++)
					{
						if (j >= lineNum)
							break;
						else if (scanLines_src[j][i].pt3D.z > 1e-4)
						{
							line_right = j;
							break;
						}
					}
					if ((line_left >= 0) && (line_right >= 0))
					{
						SVzNL3DPoint& left_pt = scanLines_src[line_left][i].pt3D;
						SVzNL3DPoint& right_pt = scanLines_src[line_right][i].pt3D;
						//插值
						double dist = (double)(line_right - line_left);
						for (int j = line_left + 1; j <= line_right - 1; j++)
						{
							double d = (double)(j - line_left);
							double k2 = d / dist;
							double k1 = 1.0 - k2;
							scanLines_dst[j][i].pt3D = { left_pt.x * k1 + right_pt.x * k2, left_pt.y * k1 + right_pt.y * k2 , left_pt.z * k1 + right_pt.z * k2 };
							scanLines_src[j][i].nPointIdx = 1;
						}
					}
				}
			}
		}
	}
}