#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include <vector>
#ifdef _WIN32
#include <corecrt_math_defines.h>
#endif
#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 = (int)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 = (int)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 = (int)pts.size();
		for (int m = m_max - 1; m >= 0; m--)  //从后往前，这样删除不会影响循环
		{
			if(pts[m].nPointIdx < 0)
				pts.erase(pts.begin() + m);
		}
	}
	return;
}

void wd_gridPointClustering(
	std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
	std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
	int clusterCheckWin, //搜索窗口
	SSG_treeGrowParam growParam,//聚类条件
	int clusterID, //当前Cluster的ID
	std::vector< SVzNL2DPoint>& a_cluster,  //result
	SVzNL3DRangeD& clusterRoi  //roi3D
)
{
	int i = 0;
	int lineNum = (int)featureMask.size();
	int linePtNum = (int)featureMask[0].size();
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL2DPoint a_seedPos = a_cluster[i];
		if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
			int kkk = 1;

		SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y];
		SVzNL3DPoint& seedValue = feature3DInfo[a_seedPos.x][a_seedPos.y];
		if (0 == a_seed.clusterID)  //clusterID == 0, 未被处理，搜索邻域
		{
			for (int i = -clusterCheckWin; i <= clusterCheckWin; i++)
			{
				for (int j = -clusterCheckWin; j <= clusterCheckWin; j++)
				{
					int y = j + a_seedPos.y;
					int x = i + a_seedPos.x;
					if ((x == 390) && (y == 949))
						int kkk = 1;

					if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
					{
						SSG_featureClusteringInfo& chk_seed = featureMask[x][y];
						if ((chk_seed.featurType ==0) || (chk_seed.clusterID > 0))  //只检查未聚类的特征点
							continue;

						SVzNL3DPoint& chkValue = feature3DInfo[x][y];
						double y_diff = abs(seedValue.y - chkValue.y);
						double z_diff = abs(seedValue.z - chkValue.z);
						double x_diff = abs(seedValue.x - chkValue.x);
						if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
							(x_diff < growParam.maxSkipDistance))
						{
							if (0 == chk_seed.flag)//防止被重复添加
							{
								chk_seed.flag = 1;
								SVzNL2DPoint new_seed = { x, y };
								a_cluster.push_back(new_seed);
							}
						}
					}

				}
			}
		}
		a_seed.clusterID = clusterID;
		//更新ROI
		clusterRoi.xRange.min = clusterRoi.xRange.min > seedValue.x ? seedValue.x : clusterRoi.xRange.min;
		clusterRoi.xRange.max = clusterRoi.xRange.max < seedValue.x ? seedValue.x : clusterRoi.xRange.max;
		clusterRoi.yRange.min = clusterRoi.yRange.min > seedValue.y ? seedValue.y : clusterRoi.yRange.min;
		clusterRoi.yRange.max = clusterRoi.yRange.max < seedValue.y ? seedValue.y : clusterRoi.yRange.max;
		clusterRoi.zRange.min = clusterRoi.zRange.min > seedValue.z ? seedValue.z : clusterRoi.zRange.min;
		clusterRoi.zRange.max = clusterRoi.zRange.max < seedValue.z ? seedValue.z : clusterRoi.zRange.max;
		i++;
	}
	return;
}

//使用聚类方法完成8连通连通域分析
void wd_gridPointClustering_labelling(
	std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
	std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
	int clusterID, //当前Cluster的ID
	std::vector< SVzNL2DPoint>& a_cluster,  //result
	SVzNLRect& clusterRoi  //roi2D
)
{
	int i = 0;
	int lineNum = (int)featureMask.size();
	int linePtNum = (int)featureMask[0].size();
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL2DPoint a_seedPos = a_cluster[i];
		if ((a_seedPos.x == 390) && (a_seedPos.y == 949))
			int kkk = 1;

		SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y];
		if (0 == a_seed.clusterID)  //clusterID == 0, 未被处理，搜索邻域
		{
			//8连通
			for (int i = -1; i <= 1; i++)
			{
				for (int j = -1; j <= 1; j++)
				{
					int y = j + a_seedPos.y;
					int x = i + a_seedPos.x;
					if ((x == 390) && (y == 949))
						int kkk = 1;

					if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum))
					{
						SSG_featureClusteringInfo& chk_seed = featureMask[x][y];
						if ((chk_seed.featurType == 0) || (chk_seed.clusterID > 0))  //只检查未聚类的特征点
							continue;

						if (0 == chk_seed.flag)//防止被重复添加
						{
							chk_seed.flag = 1;
							SVzNL2DPoint new_seed = { x, y };
							a_cluster.push_back(new_seed);
						}
					}

				}
			}
		}
		a_seed.clusterID = clusterID;
		//更新ROI
		clusterRoi.left = clusterRoi.left > a_seedPos.x ? a_seedPos.x : clusterRoi.left;
		clusterRoi.right = clusterRoi.right < a_seedPos.x ? a_seedPos.x : clusterRoi.right;
		clusterRoi.top = clusterRoi.top > a_seedPos.y ? a_seedPos.y : clusterRoi.top;
		clusterRoi.bottom = clusterRoi.bottom < a_seedPos.y ? a_seedPos.y : clusterRoi.bottom;
		i++;
	}
	return;
}