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

bool _checkTypeMatch(int treeType, int featureType)
{
	bool isMatch = false;
	switch (treeType)
	{
	case LINE_FEATURE_L_JUMP_H2L:
		if ((LINE_FEATURE_L_JUMP_H2L == featureType) || (LINE_FEATURE_L_SLOPE_H2L == featureType))
			isMatch = true;
		break;
	case LINE_FEATURE_L_JUMP_L2H:
		if ((LINE_FEATURE_L_JUMP_L2H == featureType) || (LINE_FEATURE_L_SLOPE_L2H == featureType))
			isMatch = true;
		break;
	case LINE_FEATURE_L_SLOPE_H2L:
		if ((LINE_FEATURE_L_SLOPE_H2L == featureType) || (LINE_FEATURE_L_JUMP_H2L == featureType))
			isMatch = true;
		break;
	case LINE_FEATURE_L_SLOPE_L2H:
		if ((LINE_FEATURE_L_SLOPE_L2H == featureType) || (LINE_FEATURE_L_JUMP_L2H == featureType))
			isMatch = true;
		break;
	case LINE_FEATURE_V_SLOPE:
		if ((LINE_FEATURE_V_SLOPE == featureType) ||
			(LINE_FEATURE_L_SLOPE_L2H == featureType) ||
			(LINE_FEATURE_L_SLOPE_H2L == featureType))
			isMatch = true;
		break;
	case LINE_FEATURE_RIGHT_ANGLE_HR:
	case LINE_FEATURE_RIGHT_ANGLE_HF:
	case LINE_FEATURE_RIGHT_ANGLE_RH:
	case LINE_FEATURE_RIGHT_ANGLE_FH:
	case LINE_FEATURE_PEAK_TOP:
	case LINE_FEATURE_PEAK_BOTTOM:
	case LINE_FEATURE_CORNER_V:
	case LINE_FEATURE_LINE_ENDING_0:
	case LINE_FEATURE_LINE_ENDING_1:
		if (featureType == treeType)
			isMatch = true;
		break;
	default:
		break;
	}
	return isMatch;
}

#define TREE_STATE_UNDEF	0
#define TREE_STATE_ALIVE	1
#define TREE_STATE_DEAD		2
//将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
//没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。
bool _featureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vector<SSG_featureTree>& trees, SSG_treeGrowParam growParam)
{
	for (int i = 0, i_max = trees.size(); i < i_max; i++)
	{
		SSG_featureTree& a_tree = trees[i];
		if (TREE_STATE_DEAD == a_tree.treeState)
			continue;
		//检查生长点
		SSG_basicFeature1D last_node = a_tree.treeNodes.back();
		if (last_node.jumpPos2D.x == a_feature.jumpPos2D.x)  //x为lineIdx，同一条扫描线上的不进行生长
			continue;

		//判断生长点
		double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y);
		double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z);
		int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x);
		double x_diff = abs(a_feature.jumpPos.x - last_node.jumpPos.x);
		bool typeMatch = _checkTypeMatch(a_tree.treeType, a_feature.featureType);
		if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
			( (line_diff < growParam.maxLineSkipNum)||(x_diff < growParam.maxSkipDistance)) && (true == typeMatch))
		{
			a_tree.eLineIdx = lineIdx;
			a_tree.treeNodes.push_back(a_feature);
			return true;
		}
	}
	return false;
}
#if 0
//将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
//没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。
bool _hFeatureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vector<SSG_featureTree>& trees, SSG_treeGrowParam growParam)
{
	for (int i = 0, i_max = trees.size(); i < i_max; i++)
	{
		SSG_featureTree& a_tree = trees[i];
		if (TREE_STATE_DEAD == a_tree.treeState)
			continue;
		//检查生长点
		SSG_basicFeature1D last_node = a_tree.treeNodes.back();
		if (last_node.jumpPos2D.x == a_feature.jumpPos2D.x)  //x为lineIdx，同一条扫描线上的不进行生长
			continue;

		//判断生长点
		double x_diff = abs(a_feature.jumpPos.x - last_node.jumpPos.x);
		double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y);
		double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z);
		int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x);
		bool typeMatch = _checkTypeMatch(a_tree.treeType, a_feature.featureType);
		if ((x_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
			((line_diff < growParam.maxLineSkipNum) || (y_diff < growParam.maxSkipDistance)) && (true == typeMatch))
		{
			a_tree.eLineIdx = lineIdx;
			a_tree.treeNodes.push_back(a_feature);
			return true;
		}
	}
	return false;
}
#endif
#if 0
bool compareByLen(const SSG_RUN& a, const SSG_RUN& b) {
	return a.len < b.len;
}

bool _checkCombine(SSG_featureTree* a_tree, SSG_featureTree* chk_tree)
{
	
	double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y);
	double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z);
	int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x);
	if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
		(line_diff < growParam.maxLineSkipNum) && (a_feature.featureType == a_tree.treeType))
	{
		a_tree.treeNodes.push_back(a_feature);
		return true;
	}
}

void _growTreeCombine(std::vector<SSG_featureTree>& trees)
{
	std::vector< SSG_RUN> treeInfo;
	treeInfo.resize(trees.size());
	for (int i = 0, i_max = trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_tree = &trees[i];
		SSG_lineFeature* first_node = &(a_tree->treeNodes[0]);
		SSG_lineFeature* last_node = &(a_tree->treeNodes[a_tree->treeNodes.size() - 1]);
		int len = last_node->jumpPos2D.x - first_node->jumpPos2D.x;
		SSG_RUN an_info = { i, len, a_tree->treeType };
		treeInfo.push_back(an_info);
	}
	//从长到短排序
	std::sort(treeInfo.begin(), treeInfo.end(), compareByLen);
	//以长的准
	int treeSize = treeInfo.size();
	for (int i = 0; i < treeSize; i++)
	{
		SSG_RUN* an_info = &treeInfo[i];
		if (an_info->value == 0)
			continue;

		SSG_featureTree* a_tree = &trees[an_info->start];
		bool toChk = true;
		while (toChk)
		{
			for (int j = i + 1; j < treeSize; j++)
			{
				SSG_featureTree* chk_tree = &trees[j];
				if (chk_tree->treeType == 0)
					continue;
				bool vldToCombine = _checkCombine(a_tree, chk_tree);
			}
			
		}
	}
}
#endif

//对于V型生长树，如果是单调上升或单调下降，且最高点没有继续生长的去除（褶皱)
bool _invalidateVSlopeTrees(SSG_featureTree& a_tree, double minLTypeTreeLen, double minVTypeTreeLen)
{
	SSG_basicFeature1D* first_node = &(a_tree.treeNodes[0]);
	SSG_basicFeature1D* last_node = &(a_tree.treeNodes[a_tree.treeNodes.size() - 1]);
	double len = sqrt(pow(first_node->jumpPos.x - last_node->jumpPos.x, 2) + pow(first_node->jumpPos.y - last_node->jumpPos.y, 2));
	if ( ((LINE_FEATURE_V_SLOPE!=a_tree.treeType)&&(len <= minLTypeTreeLen)) ||
		((LINE_FEATURE_V_SLOPE == a_tree.treeType) && (len <= minVTypeTreeLen)))
		return false;
	else
	{
		if (LINE_FEATURE_V_SLOPE != a_tree.treeType)
			return true;
		else
		{
			//为了防止两端的点的扫描点的高度异常变化， 取中间段进行坡度计算
			int size = a_tree.treeNodes.size();
			int pos_0 = size / 4;
			int pos_1 = size * 3 / 4;
			SSG_basicFeature1D* node_0 = &(a_tree.treeNodes[pos_0]);
			SSG_basicFeature1D* node_1 = &(a_tree.treeNodes[pos_1]);
			double node_len = sqrt(pow(node_0->jumpPos.x - node_1->jumpPos.x, 2) + pow(node_0->jumpPos.y - node_1->jumpPos.y, 2));
			double height = abs(node_0->jumpPos.z - node_1->jumpPos.z);
			double k = height / len;
			if (k < 0.3) //大约为16度的坡度
				return true;
			else
				return false;
		}
	}
}

bool _checkTwoFeatureVldToGrow(SSG_basicFeature1D& a_node, SSG_basicFeature1D* a_feature, SSG_treeGrowParam growParam)
{
	//判断是否可以生长
	double y_diff = abs(a_feature->jumpPos.y - a_node.jumpPos.y);
	double z_diff = abs(a_feature->jumpPos.z - a_node.jumpPos.z);
	int line_diff = abs(a_feature->jumpPos2D.x - a_node.jumpPos2D.x);
	if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
		(line_diff < growParam.maxLineSkipNum))
	{
		return true;
	}
	return false;
}

void _growingFromHead(SSG_featureTree* a_tree, std::vector<SSG_lineFeature>& lineFeatures, SSG_treeGrowParam growParam)
{
	SSG_basicFeature1D& head_node = a_tree->treeNodes[0];
	int head_line = head_node.jumpPos2D.x;
	for (int i = head_line-1; i >= 0; i--)
	{
		SSG_lineFeature* a_line = &lineFeatures[i];
		int endingNum = a_line->endings.size();
		
		bool growFlag = false;
		for (int m = 0; m < endingNum; m++)
		{
			bool vld_0 = _checkTwoFeatureVldToGrow(head_node, &a_line->endings[m], growParam);
			if (true == vld_0)
			{
				a_line->endings[m].featureType = a_tree->treeType;
				a_tree->treeNodes.insert(a_tree->treeNodes.begin(), a_line->endings[m]);
				growFlag = true;
				break;
			}
		}
		if (false == growFlag)
		{
			int line_diff = abs(a_line->lineIdx - a_tree->treeNodes[0].jumpPos2D.x);
			if (line_diff > growParam.maxLineSkipNum)
				break;
		}
	}
}

void _growingFromTail(SSG_featureTree* a_tree, std::vector<SSG_lineFeature>& lineFeatures, SSG_treeGrowParam growParam)
{
	SSG_basicFeature1D& tail_node = a_tree->treeNodes[a_tree->treeNodes.size()-1];
	int tail_line = tail_node.jumpPos2D.x;
	for (int i = tail_line + 1; i < lineFeatures.size(); i++)
	{
		SSG_lineFeature* a_line = &lineFeatures[i];
		int endingNum = a_line->endings.size();

		bool growFlag = false;
		for (int m = 0; m < endingNum; m++)
		{
			bool vld_0 = _checkTwoFeatureVldToGrow(tail_node, &a_line->endings[m], growParam);
			if (true == vld_0)
			{
				a_line->endings[m].featureType = a_tree->treeType;
				a_tree->treeNodes.push_back(a_line->endings[m]);
				growFlag = true;
				break;
			}
		}
		if (false == growFlag)
		{
			int line_diff = abs(a_line->lineIdx - a_tree->treeNodes.back().jumpPos2D.x);
			if (line_diff > growParam.maxLineSkipNum)
				break;
		}
	}

}

//获取生长树的ROI
void _getTreeROI(SSG_featureTree* a_tree)
{
	if (a_tree->treeNodes.size() == 0)
	{
		a_tree->roi.left = 0;
		a_tree->roi.right = 0;
		a_tree->roi.top = 0;
		a_tree->roi.bottom = 0;
	}
	else
	{
		a_tree->roi.left = a_tree->treeNodes[0].jumpPos.x;
		a_tree->roi.right = a_tree->treeNodes[0].jumpPos.x;
		a_tree->roi.top = a_tree->treeNodes[0].jumpPos.y;
		a_tree->roi.bottom = a_tree->treeNodes[0].jumpPos.y;
		for (int i = 1, i_max = a_tree->treeNodes.size(); i < i_max; i++)
		{
			if(a_tree->roi.left > a_tree->treeNodes[i].jumpPos.x)
				a_tree->roi.left = a_tree->treeNodes[i].jumpPos.x;
			if (a_tree->roi.right < a_tree->treeNodes[i].jumpPos.x)
				a_tree->roi.right = a_tree->treeNodes[i].jumpPos.x;
			if (a_tree->roi.top > a_tree->treeNodes[i].jumpPos.y)
				a_tree->roi.top = a_tree->treeNodes[i].jumpPos.y;
			if (a_tree->roi.bottom < a_tree->treeNodes[i].jumpPos.y)
				a_tree->roi.bottom = a_tree->treeNodes[i].jumpPos.y;
		}
	}
	return;
}

SSG_featureTree* _getIncludingBigTree(SSG_featureTree* a_tree, std::vector<SSG_featureTree>& trees, bool chk_up)
{
	SSG_featureTree* _bigTree = NULL;
	for (int i = 0, i_max = trees.size(); i < i_max; i++)
	{
		SSG_featureTree* chk_tree = &trees[i];
		if (a_tree == chk_tree)
			continue;

		if (true == chk_up)
		{
			if (chk_tree->roi.bottom < a_tree->roi.top)
			{
				if ((chk_tree->roi.left < a_tree->roi.left) && 
					(chk_tree->roi.right > a_tree->roi.right)) //包含关系成立
				{
					if (NULL == _bigTree)
						_bigTree = chk_tree;
					else
					{
						if(chk_tree->roi.bottom > _bigTree->roi.bottom)
							_bigTree = chk_tree;
					}
				}
			}
		}
		else
		{
			if (chk_tree->roi.top > a_tree->roi.bottom)
			{
				if ((chk_tree->roi.left < a_tree->roi.left) &&
					(chk_tree->roi.right > a_tree->roi.right)) //包含关系成立
				{
					if (NULL == _bigTree)
						_bigTree = chk_tree;
					else
					{
						if (chk_tree->roi.bottom < _bigTree->roi.bottom)
							_bigTree = chk_tree;
					}
				}
			}
		}
	}
	return _bigTree;
}

void sg_getFeatureGrowingTrees(
	std::vector<SSG_lineFeature>& lineFeatures,
	std::vector<SSG_featureTree>& trees,
	SSG_treeGrowParam growParam)
{
	for (int i = 0, i_max = lineFeatures.size(); i < i_max; i++)
	{
		SSG_lineFeature& a_line = lineFeatures[i];
		if (a_line.features.size() > 0)
		{
			for (int j = 0, j_max = a_line.features.size(); j < j_max; j++)
			{
				SSG_basicFeature1D& a_feature = a_line.features[j];
				if (a_feature.jumpPos2D.x == 207)
					int kkk = 1;
				bool isMatched = _featureGrowing(a_feature, a_line.lineIdx, trees, growParam);
				if (false == isMatched)
				{
					//新的生长树
					SSG_featureTree a_newTree;
					a_newTree.treeNodes.push_back(a_feature);
					a_newTree.treeState = TREE_STATE_ALIVE;
					a_newTree.treeType = a_feature.featureType;
					a_newTree.sLineIdx = a_line.lineIdx;
					a_newTree.eLineIdx = a_line.lineIdx;
					trees.push_back(a_newTree);
				}
			}
		}
		//检查停止生长的树,加速。
		//将生长节点为1的生长树移除
		int lineIdx = a_line.lineIdx;
		int m_max = trees.size();
		for (int m = m_max - 1; m >= 0; m--)  //从后往前，这样删除不会影响循环
		{
			if (TREE_STATE_ALIVE == trees[m].treeState)
			{
				int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x);
				if ( ((growParam.maxLineSkipNum > 0) &&(line_diff > growParam.maxLineSkipNum)) || 
					(i == i_max - 1))
				{
					trees[m].treeState = TREE_STATE_DEAD;
					bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
					if (false == isValid)
						trees.erase(trees.begin() + m);
				}
			}
		}
	}
}

void sg_getEndingGrowingTrees(
	std::vector<SSG_2DValueI>& lineEndings,
	SVzNL3DLaserLine* laser3DPoints,
	bool isVScan,
	int featureType,
	std::vector<SSG_featureTree>& trees,
	SSG_treeGrowParam growParam)
{
	for (int i = 0, i_max = lineEndings.size(); i < i_max; i++)
	{
		if (i == 175)
			int kkk = 1;
		SSG_2DValueI& an_ending = lineEndings[i];
		SSG_basicFeature1D endingFeature;
		endingFeature.featureType = featureType;
		endingFeature.jumpPos2D.x = an_ending.x;
		endingFeature.jumpPos2D.y = an_ending.y;
		if(true == isVScan)
			endingFeature.jumpPos = laser3DPoints[an_ending.x].p3DPosition[an_ending.y].pt3D;
		else
		{
			endingFeature.jumpPos.x = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.y;
			endingFeature.jumpPos.y = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.x;
			endingFeature.jumpPos.z = laser3DPoints[an_ending.y].p3DPosition[an_ending.x].pt3D.z;
		}
		bool isMatched = _featureGrowing(endingFeature, an_ending.x, trees, growParam);
		if (false == isMatched)
		{
			//新的生长树
			SSG_featureTree a_newTree;
			a_newTree.treeNodes.push_back(endingFeature);
			a_newTree.treeState = TREE_STATE_ALIVE;
			a_newTree.treeType = endingFeature.featureType;
			a_newTree.sLineIdx = an_ending.x;
			a_newTree.eLineIdx = an_ending.x;
			trees.push_back(a_newTree);
		}

		//检查停止生长的树,加速。
		//将生长节点为1的生长树移除
		int lineIdx = an_ending.x;
		int m_max = trees.size();
		for (int m = m_max - 1; m >= 0; m--)  //从后往前，这样删除不会影响循环
		{
			if (TREE_STATE_ALIVE == trees[m].treeState)
			{
				int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x);
				if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
					(i == i_max - 1))
				{
					trees[m].treeState = TREE_STATE_DEAD;
					bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
					if (false == isValid)
						trees.erase(trees.begin() + m);
				}
			}
		}
	}
}

//对扫描线的V型特征进行生长
void sg_LVFeatureGrowing(
	std::vector<SSG_lineFeature>& lineFeatures, 
	std::vector<SSG_featureTree>& trees, 
	SSG_bagParam bagParam, 
	SSG_treeGrowParam growParam, 
	std::vector<SSG_2DValueI>& edgePts_0,
	std::vector<SSG_2DValueI>& edgePts_1)
{
	//特征生长
	sg_getFeatureGrowingTrees(
		lineFeatures,
		trees,
		growParam);

	//迭代一次：重新对每棵树头尾检查，看是否可以继续生长到LINE_EDGE
	for (int i = 0, i_max = trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_tree = &trees[i];

		//Tree类型重新统计确定。
		std::vector<int> typeStat;
		typeStat.resize(LINE_FEATURE_NUM);
		for (int m = 0, m_max = a_tree->treeNodes.size(); m < m_max; m++)
		{
			int type = a_tree->treeNodes[m].featureType;
			typeStat[type]++;
		}
		int maxId = -1;
		int maxSize = 0;
		for (int m = 0; m < typeStat.size(); m++)
		{
			if (maxSize < typeStat[m])
			{
				maxId = m;
				maxSize = typeStat[m];
			}
		}
		if (maxId > 0)
			a_tree->treeType = maxId;

		_growingFromHead(a_tree, lineFeatures, growParam);
		_growingFromTail(a_tree, lineFeatures, growParam);
	}

	///过滤
	///两种过滤方法：
	///（1）针对SLOPE和V型生长树，检查生长方向和垂直方向的比例。
	///（2）宏观检查：检查短的生长树是否被长的生长树所包含
	int tree_size = trees.size();
	for (int m = tree_size - 1; m >= 0; m--)  //从后往前，这样删除不会影响循环
	{
		SSG_featureTree* a_tree = &trees[m]; 
		_getTreeROI(a_tree);
		if ((LINE_FEATURE_V_SLOPE == a_tree->treeType) ||
			(LINE_FEATURE_L_SLOPE_H2L == a_tree->treeType) ||
			(LINE_FEATURE_L_SLOPE_L2H == a_tree->treeType) ||
			(LINE_FEATURE_CORNER_V == a_tree->treeType))
		{
			//检查生长方向（x方向）的长度
			double grow_len = a_tree->roi.right - a_tree->roi.left;
			double grow_width = a_tree->roi.bottom - a_tree->roi.top;
			if ((grow_len < bagParam.bagW / 4) && (grow_width > grow_len / 2))
			{
				trees.erase(trees.begin() + m);
			}
		}
	}
	tree_size = trees.size();
	for (int m = tree_size - 1; m >= 0; m--)  //从后往前，这样删除不会影响循环
	{
if (m == 24)
	int kkk = 1;
		SSG_featureTree* a_tree = &trees[m];
		double grow_len = a_tree->roi.right - a_tree->roi.left;
		//检查前后是否被包含
		//取前面最近的包含a_tree的生长树
		if ((grow_len < bagParam.bagW / 2) && ((LINE_FEATURE_V_SLOPE == a_tree->treeType) ||
			(LINE_FEATURE_L_SLOPE_H2L == a_tree->treeType) ||
			(LINE_FEATURE_L_SLOPE_L2H == a_tree->treeType) ||
			(LINE_FEATURE_CORNER_V == a_tree->treeType)))
		{
			SSG_featureTree* up_bigTree = _getIncludingBigTree(a_tree, trees, true);
			SSG_featureTree* down_bigTree = _getIncludingBigTree(a_tree, trees, false);
			if ((NULL != up_bigTree) && (NULL != down_bigTree))
			{
				double dist = (down_bigTree->roi.top + down_bigTree->roi.bottom - up_bigTree->roi.top - up_bigTree->roi.bottom) / 2;
				if (dist < bagParam.bagL * 1.3)
				{
					trees.erase(trees.begin() + m);
				}
			}
		}
	}

	std::vector<SSG_2DValueI> edge0_pts;
	std::vector<SSG_2DValueI> edge1_pts;
	std::vector<double> edge0_y;
	std::vector<double> edge1_y;
	for (int i = 0, i_max = lineFeatures.size(); i < i_max; i++)
	{
		SSG_lineFeature& a_line = lineFeatures[i];
		int a_line_endingNum = a_line.endings.size();
		if (a_line_endingNum > 0)
		{
			for (int m = 0; m < a_line_endingNum; m++)
			{
				SSG_2DValueI an_edge = { a_line.endings[m].jumpPos2D.x, a_line.endings[m].jumpPos2D.y, a_line.endings[m].featureType, a_line.endings[m].jumpPos.z };
				if (LINE_FEATURE_LINE_ENDING_0 == a_line.endings[m].featureType)
				{
					edge0_pts.push_back(an_edge);
					edge0_y.push_back(a_line.endings[m].jumpPos.y);
				}
				else if(LINE_FEATURE_LINE_ENDING_1 == a_line.endings[m].featureType)
				{
					edge1_pts.push_back(an_edge);
					edge1_y.push_back(a_line.endings[m].jumpPos.y);
				}
			}
		}
	}
#if 1
	//区分左右
	edgePts_0.insert(edgePts_0.end(), edge0_pts.begin(), edge0_pts.end());
	edgePts_1.insert(edgePts_1.end(), edge1_pts.begin(), edge1_pts.end());
#else
	//Edge点需要进行滤除离群点（由于扫描原因，可能会出现拉条子现象）
	const double outlier_sigma_k = 5.0;//5倍sigma
	SSG_meanVar meanVar_edge0 = _computeMeanVar(edge0_y.data(), edge0_y.size());
	double edge0_dyTh = meanVar_edge0.var * outlier_sigma_k;  //5倍sigma
	for (int i = 0; i < edge0_pts.size(); i++)
	{
if (edge0_pts[i].x == 622)
	int kkk = 1;
		double yDiff = abs(edge0_y[i] - meanVar_edge0.mean);
		if (yDiff < edge0_dyTh)
			edgePts.push_back(edge0_pts[i]);
	}
	SSG_meanVar meanVar_edge1 = _computeMeanVar(edge1_y.data(), edge1_y.size());
	double edge1_dyTh = meanVar_edge1.var * outlier_sigma_k;  //5倍sigma
	for (int i = 1; i < edge1_pts.size(); i++)
	{
if (edge1_pts[i].x == 622)
	int kkk = 1;
		double yDiff = abs(edge1_y[i] - meanVar_edge1.mean);
		if (yDiff < edge1_dyTh)
			edgePts.push_back(edge1_pts[i]);
	}
#endif
}


void sg_getPeakGrowingTrees(
	std::vector<std::vector< SSG_basicFeature1D>>& peakFeatures,
	std::vector<SSG_featureTree>& trees,
	SSG_treeGrowParam growParam)
{
	for (int i = 0, i_max = peakFeatures.size(); i < i_max; i++)
	{
		int lineIdx = i;
if (i == 761)
	int kkk = 1;
		std::vector< SSG_basicFeature1D>& a_linePeak = peakFeatures[i];
		if (a_linePeak.size() > 0)
		{
			for (int j = 0, j_max = a_linePeak.size(); j < j_max; j++)
			{
				SSG_basicFeature1D& a_feature = a_linePeak[j];
				if (a_feature.jumpPos2D.x == 207)
					int kkk = 1;
				bool isMatched = _featureGrowing(a_feature, lineIdx, trees, growParam);
				if (false == isMatched)
				{
					//新的生长树
					SSG_featureTree a_newTree;
					a_newTree.treeNodes.push_back(a_feature);
					a_newTree.treeState = TREE_STATE_ALIVE;
					a_newTree.treeType = a_feature.featureType;
					a_newTree.sLineIdx = lineIdx;
					a_newTree.eLineIdx = lineIdx;
					trees.push_back(a_newTree);
				}
			}
		}
		//检查停止生长的树,加速。
		//将生长节点为1的生长树移除
		int m_max = trees.size();
		for (int m = m_max - 1; m >= 0; m--)  //从后往前，这样删除不会影响循环
		{
			if (TREE_STATE_ALIVE == trees[m].treeState)
			{
				int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x);
				if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
					(i == i_max - 1))
				{
					trees[m].treeState = TREE_STATE_DEAD;
					bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
					if (false == isValid)
						trees.erase(trees.begin() + m);
				}
			}
		}
	}
}

//对扫描线的peak特征进行生长
void sg_peakFeatureGrowing(
	std::vector<std::vector< SSG_basicFeature1D>>& lineFeatures,
	std::vector<SSG_featureTree>& trees,
	SSG_treeGrowParam growParam)
{
	//特征生长
	sg_getPeakGrowingTrees(
		lineFeatures,
		trees,
		growParam);

	for (int i = 0, i_max = trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_tree = &trees[i];
		_getTreeROI(a_tree);
	}
	///过滤
	
}
#if 0
void sg_fillingNullNodes(std::vector<SSG_featureTree>& trees)
{
	int treeSize = trees.size();
	for (int i = 0; i < treeSize; i++)
	{
		SSG_featureTree* a_tree = &trees[i];
		int nodeSize = a_tree->treeNodes.size();
		for (int j = nodeSize-2; j >= 0; j--)
		{
			SSG_lineFeature* curr_node = &(a_tree->treeNodes[j]);
			SSG_lineFeature* nxt_node = &(a_tree->treeNodes[j+1]);
			int line_diff = nxt_node->jumpPos2D.x - curr_node->jumpPos2D.x;
			if (line_diff > 1)
			{

			}
		}
	}
}
#endif


//判断生长点
bool validGrowing(
	SSG_featureSemiCircle& a_feature, 
	SSG_semiCircleFeatureTree& a_tree,
	SSG_treeGrowParam growParam)
{
	SSG_featureSemiCircle last_node = a_tree.treeNodes.back();
	double y_diff = abs(a_feature.midPt.y - last_node.midPt.y);
	int line_diff = abs(a_feature.lineIdx - last_node.lineIdx);
	double x_diff = abs(a_feature.midPt.x - last_node.midPt.x);
	if ((y_diff < growParam.yDeviation_max) &&
		((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)))
		return true;
	else
		return false;
}
// 将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
//没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。
int _featureGrowing_semiCircle(
	SSG_featureSemiCircle& a_feature, 
	std::vector<SSG_semiCircleFeatureTree>&trees, 
	SSG_treeGrowParam growParam)
{
	if ((FEATURE_FLAG_INVLD_START == a_feature.flag) || (FEATURE_FLAG_VALID_START == a_feature.flag))
		return -1;

	for (int i = 0, i_max = trees.size(); i < i_max; i++)
	{
		SSG_semiCircleFeatureTree& a_tree = trees[i];
		if (TREE_STATE_DEAD == a_tree.treeState)
			continue;
		//检查生长点
		SSG_featureSemiCircle last_node = a_tree.treeNodes.back();
		if ( (last_node.lineIdx == a_feature.lineIdx)|| //x为lineIdx，同一条扫描线上的不进行生长
			(FEATURE_FLAG_INVLD_END == last_node.flag) || (FEATURE_FLAG_VALID_END == last_node.flag)) 
			continue;

		//判断生长点
		bool vldGrowing = validGrowing(a_feature, a_tree, growParam);
		if (true == vldGrowing)
			return i;
	}
	return -1;
}

bool _invalidateSemiCircleTrees(SSG_semiCircleFeatureTree& a_tree, double minTreeLen)
{
	SSG_featureSemiCircle* first_node = &(a_tree.treeNodes[0]);
	SSG_featureSemiCircle* last_node = &(a_tree.treeNodes[a_tree.treeNodes.size() - 1]);
	double len = sqrt(pow(first_node->midPt.x - last_node->midPt.x, 2) + pow(first_node->midPt.y - last_node->midPt.y, 2));
	if (len <= minTreeLen)
		return false;
	else
		return true;
}

void sg_getFeatureGrowingTrees_semiCircle(
	std::vector< SSG_featureSemiCircle>& lineFeatures,
	const int lineIdx,
	const int lineSize,
	std::vector<SSG_semiCircleFeatureTree>& trees,
	std::vector<SSG_semiCircleFeatureTree>& stopTrees,
	std::vector<SSG_semiCircleFeatureTree>& invalidTrees,
	SSG_treeGrowParam growParam)
{
	for (int i = 0, i_max = lineFeatures.size(); i < i_max; i++)
	{
		SSG_featureSemiCircle& a_feature = lineFeatures[i];
		if (FEATURE_FLAG_INVALID == a_feature.flag)
			continue;

		if ( (a_feature.lineIdx >= 205) && (i == 5))
			int kkk = 1;
		int matchedTreeIdx = _featureGrowing_semiCircle(a_feature, trees, growParam);
		if (matchedTreeIdx >= 0)
		{
			//检查最佳匹配
			std::vector<int> otherMatching;
			for (int j = i + 1; j < i_max; j++)
			{
				bool vldGrowing = validGrowing(lineFeatures[j], trees[matchedTreeIdx], growParam);
				if (false == vldGrowing)
					break;
				otherMatching.push_back(j);
			}
			if (otherMatching.size() > 0)
			{
				SSG_featureSemiCircle last_node = trees[matchedTreeIdx].treeNodes.back();
				double minDist = abs(a_feature.midPt.y - last_node.midPt.y);
				int bestMatch = i;
				//取最小的匹配
				for (int m = 0, m_max = otherMatching.size(); m < m_max; m++)
				{
					int idx = otherMatching[m];
					double y_diff = abs(lineFeatures[idx].midPt.y - last_node.midPt.y);
					if (y_diff < minDist)
					{
						minDist = y_diff;
						bestMatch = idx;
					}
				}
				otherMatching.push_back(i);
				for (int m = 0, m_max = otherMatching.size(); m < m_max; m++)
				{
					int idx = otherMatching[m];
					if (bestMatch == idx)
					{
						trees[matchedTreeIdx].eLineIdx = lineFeatures[idx].lineIdx;
						trees[matchedTreeIdx].treeNodes.push_back(lineFeatures[idx]);
					}
					lineFeatures[idx].flag = FEATURE_FLAG_INVALID;
				}
			}
			else
			{
				trees[matchedTreeIdx].eLineIdx = a_feature.lineIdx;
				trees[matchedTreeIdx].treeNodes.push_back(a_feature);
			}
		}
		else //(false == isMatched)
		{
			//新的生长树
			SSG_semiCircleFeatureTree a_newTree;
			a_newTree.treeNodes.push_back(a_feature);
			a_newTree.treeState = TREE_STATE_ALIVE;
			a_newTree.treeType = 0;
			a_newTree.sLineIdx = a_feature.lineIdx;
			a_newTree.eLineIdx = a_feature.lineIdx;
			trees.push_back(a_newTree);
		}
	}
	//检查停止生长的树,加速。
	//将生长节点为1的生长树移除
	int m_max = trees.size();
	for (int m = m_max - 1; m >= 0; m--)  //从后往前，这样删除不会影响循环
	{
		if (TREE_STATE_ALIVE == trees[m].treeState)
		{
			int line_diff = abs(lineIdx - trees[m].treeNodes.back().lineIdx);
			if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
				(lineIdx == lineSize-1))
			{
				trees[m].treeState = TREE_STATE_DEAD;
				bool isValid = _invalidateSemiCircleTrees(trees[m], growParam.minVTypeTreeLen);
				if (true == isValid)
					stopTrees.push_back(trees[m]);
				else
					invalidTrees.push_back(trees[m]);
				trees.erase(trees.begin() + m);
			}
		}
	}
}