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

void sg_lineDataR(SVzNL3DLaserLine* a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT(a_line, camPoseR, groundH);
}

//扫描线处理，进行垂直方向的特征提取和生长
void sg_sieveNodeDetection_lineProc(
	SVzNL3DLaserLine* a_line,
	int lineIdx,
	int* errCode,
	std::vector<std::vector< SSG_featureSemiCircle>>& all_vLineFeatures,
	std::vector<std::vector<int>>& noisePts,
	const SSG_sieveNodeDetectionParam sieveDetectParam)
{
	std::vector< SSG_featureSemiCircle> a_line_features;
	//滤波，滤除异常点
	std::vector<SVzNL3DPosition> filterData;
	std::vector<int> lineNoisePts;
	sg_lineDataRemoveOutlier(a_line->p3DPosition, a_line->nPositionCnt, sieveDetectParam.filterParam, filterData, lineNoisePts);
	noisePts.push_back(lineNoisePts);

	sg_getLineUpperSemiCircleFeature(
		filterData.data(),
		filterData.size(),
		lineIdx,
		sieveDetectParam.sieveDiameter,
		sieveDetectParam.slopeParam,
		a_line_features);

	all_vLineFeatures.push_back(a_line_features);  //空行也加入，保证能按行号索引
	return;
}

int _checkFeatureSplit(
	SSG_featureSemiCircle& a_feaurue, 
	std::vector< SSG_featureSemiCircle>& chk_line_feature,
	double splitMaxDist)  //在此距离内为有效分叉
{
	int split = -1;
	
	for (int i = 0, i_max = chk_line_feature.size(); i < i_max; i++)
	{
		if (i < i_max - 1)
		{
			if ((chk_line_feature[i].midY < a_feaurue.midY) && (chk_line_feature[i + 1].midY > a_feaurue.midY))
			{
				double dist_1 = abs(chk_line_feature[i].midY - a_feaurue.midY);
				double dist_2 = abs(chk_line_feature[i+1].midY - a_feaurue.midY);
				if ((dist_1 < splitMaxDist) && (dist_2 < splitMaxDist))
				{
					split = i;
					break;
				}
			}
		}
	}
	return split;
}

void sg_getSieveNodes(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	const SSG_sieveNodeDetectionParam sieveDetectParam,
	std::vector<SVzNL3DPoint>& nodePos)
{
	int errCode = 0;
	std::vector<std::vector<int>> noisePts;
	std::vector<std::vector< SSG_featureSemiCircle>> all_vLineFeatures;
	for (int i = 0; i < lineNum; i++)
	{
		if (i == 19)
			int kkk = 1;
		//将nPointIdx转义使用前清零
		for (int j = 0; j < laser3DPoints[i].nPositionCnt; j++)
			laser3DPoints[i].p3DPosition[j].nPointIdx = 0;
		//行处理
		sg_sieveNodeDetection_lineProc(
			&laser3DPoints[i],
			i,
			&errCode,
			all_vLineFeatures,
			noisePts,
			sieveDetectParam);
	}

	//根据筛网的特点去除无效的feature。当上下两个feature在下一条扫描线被合并成一个feature时，说明上一条扫描线的两个feature是无效feature。其相邻的feature均为无效feature
	for (int i = 0; i < lineNum; i++)
	{
		//与前一条扫描线比较,寻找开始
		std::vector< SSG_featureSemiCircle>& line_features = all_vLineFeatures[i];
		if (i > 0)
		{
			std::vector< SSG_featureSemiCircle>& pre_line_features = all_vLineFeatures[i-1];
			for (int j = 0, j_max = line_features.size(); j < j_max; j++)
			{
				int split = _checkFeatureSplit(line_features[j], pre_line_features, sieveDetectParam.sieveHoleSize);
				if (split >= 0)
				{
					pre_line_features[split].flag = FEATURE_FLAG_INVLD_END;
					pre_line_features[split + 1].flag = FEATURE_FLAG_INVLD_END;
					line_features[j].flag = FEATURE_FLAG_VALID_START;
				}
			}
		}
		//与后一条扫描线比较,寻找结束
		if (i < lineNum - 1)
		{
			std::vector< SSG_featureSemiCircle>& post_line_features = all_vLineFeatures[i + 1];
			for (int j = 0, j_max = line_features.size(); j < j_max; j++)
			{
				int split = _checkFeatureSplit(line_features[j], post_line_features, sieveDetectParam.sieveHoleSize);
				if (split >= 0)
				{
					post_line_features[split].flag = FEATURE_FLAG_INVLD_START;
					post_line_features[split + 1].flag = FEATURE_FLAG_INVLD_START;
					line_features[j].flag = FEATURE_FLAG_VALID_END;
				}
			}
		}
	}

	//feature生长。碰到无效feature生长停止。无效生长可以作为生长起点。其生长树上的所有feature均为无效feature
	sg_getFeatureGrowingTrees_semiCircle(
		lineFeatures,
		trees,
		growParam);


#if _OUTPUT_LINE_PROC_RESULT
	//输出扫描线处理结果
	for (int i = 0, i_max = all_vLineFeatures.size(); i < i_max; i++)
	{
		std::vector< SSG_featureSemiCircle>& a_line_features = all_vLineFeatures[i];
		for (int j = 0, j_max = a_line_features.size(); j < j_max; j++)
		{
			SSG_featureSemiCircle& a_feature = a_line_features[j];
			for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
				laser3DPoints[i].p3DPosition[m].nPointIdx = 1; //此处nPointIdx转义
			laser3DPoints[i].p3DPosition[a_feature.midPtIdx].nPointIdx = 2;
		}
	}
#endif


}

//计算一个平面调平参数。
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sg_getSieveBaseCalibPara(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	std::vector<SVzNL3DRangeD>& ROIs)
{
	return sg_getPlaneCalibPara_ROIs(laser3DPoints, lineNum, ROIs);
}