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

void _seedClustering(
	std::vector< SVzNL3DPosition>& a_cluster,
	std::vector< SVzNL3DPosition>& pts,
	double clusterDist)
{
	int i = 0;
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL3DPosition a_seed = a_cluster[i];
		for (int i = 0, i_max = pts.size(); i < i_max; i++)
		{
			if (pts[i].nPointIdx < 0)
				continue;
			double dist = sqrt(pow(a_seed.pt3D.x - pts[i].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[i].pt3D.y, 2));
			if (dist < clusterDist)
			{
				a_cluster.push_back(pts[i]);
				pts[i].nPointIdx = -1;
			}
		}
		i++;
	}
	return;
}

void sg_pointClustering(
	std::vector< SVzNL3DPosition>& pts, 
	double clusterDist, 
	std::vector<std::vector< SVzNL3DPosition>>& objClusters  //result
)
{
	int ptSize = pts.size();
	if (ptSize == 0)
		return;
	while(pts.size() > 0)
	{
		SVzNL3DPosition a_pt = pts[0];

		//新建一个cluster
		std::vector< SVzNL3DPosition> a_cluster;
		a_cluster.push_back(a_pt);
		pts[0].nPointIdx = -1; //防止重复被计算
		_seedClustering(
			a_cluster,
			pts,
			clusterDist);
		objClusters.push_back(a_cluster); //保存一个聚类
		//将pts中处理过的点去除
		int m_max = pts.size();
		for (int m = m_max - 1; m >= 0; m--)  //从后往前，这样删除不会影响循环
		{
			if(pts[m].nPointIdx < 0)
				pts.erase(pts.begin() + m);
		}
	}
	return;
}
;