#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,
	std::vector<int> flags,
	double clusterDist)
{
	int i = 0;
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL3DPosition a_seed = a_cluster[i];
		for (int j = 0, j_max = (int)pts.size(); j < j_max; j++)
		{
			if (flags[j] < 0)
				continue;
			double dist_x = abs(a_seed.pt3D.x - pts[j].pt3D.x);
			double dist_y = abs(a_seed.pt3D.y - pts[j].pt3D.y);
			if ( (dist_x < clusterDist) && (dist_y < clusterDist))
			{
				a_cluster.push_back(pts[j]);
				flags[j] = -1;
			}
		}
		i++;
	}
	return;
}

void _seedClustering_speedUp(
	std::vector< SVzNL3DPosition>& a_cluster,
	std::vector< SVzNL3DPosition>& pts,
	std::vector<int>& flags,
	double clusterDist,
	int distType, //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector<int>>& indexing2D,
	int lineNum, int ptSize, int clusterCheckWin) //搜索窗口
{
	int i = 0;
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL3DPosition a_seed = a_cluster[i];
		int seed_line = a_seed.nPointIdx >> 16;
		int seed_ptIdx =a_seed.nPointIdx & 0x0000FFFF;
		for (int line = seed_line - clusterCheckWin; line <= seed_line + clusterCheckWin; line++)
		{
			if ((line >= 0) && (line < lineNum))
			{
				for (int ptIdx = seed_ptIdx - clusterCheckWin; ptIdx <= seed_ptIdx + clusterCheckWin; ptIdx++)
				{
					if ((ptIdx >= 0) && (ptIdx < ptSize))
					{
						int backIdx = indexing2D[line][ptIdx];
						if (backIdx < 0)
							continue;
						if (flags[backIdx] < 0)
							continue;
						double dist;
						if(0 == distType)
							dist = sqrt(pow(a_seed.pt3D.x - pts[backIdx].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[backIdx].pt3D.y, 2));
						else
							dist = sqrt(pow(a_seed.pt3D.x - pts[backIdx].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[backIdx].pt3D.y, 2) + pow(a_seed.pt3D.z - pts[backIdx].pt3D.z, 2));
						if (dist < clusterDist) 
						{
							a_cluster.push_back(pts[backIdx]);
							flags[backIdx] = -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;

	std::vector<int> flags;
	flags.resize(ptSize);
	std::fill(flags.begin(), flags.end(), 0);

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

//使用SVzNL3DPosition的nPointIdx表示2D信息（高16位Line， 低16位ptIdx）
//搜索时搜索邻域以加速
void wd_pointClustering_speedUp(
	std::vector< SVzNL3DPosition>& pts,
	int lineNum, int linePtSize, int clusterCheckWin, //搜索窗口
	double clusterDist,
	int distType, //0 - 2d distance; 1- 3d distance 
	std::vector<std::vector< SVzNL3DPosition>>& objClusters  //result
)
{
	std::vector<std::vector<int>> indexing2D;
	indexing2D.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		indexing2D[i].resize(linePtSize);
		std::fill(indexing2D[i].begin(), indexing2D[i].end(), -1);
	}
	for (int i = 0; i < (int)pts.size(); i++)
	{
		int line = pts[i].nPointIdx >> 16;
		int ptIdx = pts[i].nPointIdx & 0x0000FFFF;
		indexing2D[line][ptIdx] = i;
	}

	int ptSize = (int)pts.size();
	if (ptSize == 0)
		return;

	std::vector<int> flags;
	flags.resize(ptSize);
	std::fill(flags.begin(), flags.end(), 0);

	int idx = 0;
	while (idx < (int)pts.size())
	{
		SVzNL3DPosition a_pt = pts[idx];
		if (flags[idx] >= 0)
		{
			flags[idx] = -1;//防止重复被计算
			//新建一个cluster
			std::vector< SVzNL3DPosition> a_cluster;
			a_cluster.push_back(a_pt);
			//pts[0].nPointIdx = -1; 
			_seedClustering_speedUp(
				a_cluster,
				pts,
				flags,
				clusterDist,
				distType,
				indexing2D,
				lineNum, linePtSize, clusterCheckWin);
			if(a_cluster.size() >= 5)
				objClusters.push_back(a_cluster); //保存一个聚类
		}
		idx++;
	}
	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();
	featureMask[a_cluster[0].x][a_cluster[0].y].flag = 1;  //防止第一个被重复添加
	while (1)
	{
		if (i >= a_cluster.size())
			break;

		SVzNL2DPoint a_seedPos = a_cluster[i];
		if ((a_seedPos.x == 752) && (a_seedPos.y == 2399))
			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 == 645) && (y == 2240))
						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;
}

//对扫描数据的聚类
//SSG_intPair成员变量定义: 
//		data_0: flag
//		data_1: valid point flag
//		idx: cluster ID
void wd_gridPointClustering_2(
	std::vector<std::vector<SVzNL3DPosition>>& gridData,
	std::vector<std::vector<SSG_intPair>>& pointMaskInfo, //防止聚类时重复选中
	int clusterCheckWin, //搜索窗口
	SSG_treeGrowParam growParam,//聚类条件
	int clusterID, //当前Cluster的ID
	std::vector< SVzNL2DPoint>& a_cluster,  //result
	SVzNL3DRangeD& clusterRoi  //roi3D
)
{
	int i = 0;
	int lineNum = (int)gridData.size();
	int linePtNum = (int)gridData[0].size();
	pointMaskInfo[a_cluster[0].x][a_cluster[0].y].data_0 = 1;  //防止第一个被重复添加
	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_intPair& a_seed = pointMaskInfo[a_seedPos.x][a_seedPos.y];
		SVzNL3DPosition& seedValue = gridData[a_seedPos.x][a_seedPos.y];
		if (0 == a_seed.idx)  //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_intPair& chk_seed = pointMaskInfo[x][y];
						if ((chk_seed.data_1 == 0) || (chk_seed.idx > 0))  //idx:clusterID, data_1:valid point
							continue;

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

				}
			}
		}
		a_seed.idx = clusterID;
		//更新ROI
		clusterRoi.xRange.min = clusterRoi.xRange.min > seedValue.pt3D.x ? seedValue.pt3D.x : clusterRoi.xRange.min;
		clusterRoi.xRange.max = clusterRoi.xRange.max < seedValue.pt3D.x ? seedValue.pt3D.x : clusterRoi.xRange.max;
		clusterRoi.yRange.min = clusterRoi.yRange.min > seedValue.pt3D.y ? seedValue.pt3D.y : clusterRoi.yRange.min;
		clusterRoi.yRange.max = clusterRoi.yRange.max < seedValue.pt3D.y ? seedValue.pt3D.y : clusterRoi.yRange.max;
		clusterRoi.zRange.min = clusterRoi.zRange.min > seedValue.pt3D.z ? seedValue.pt3D.z : clusterRoi.zRange.min;
		clusterRoi.zRange.max = clusterRoi.zRange.max < seedValue.pt3D.z ? seedValue.pt3D.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;
}

//使用聚类方法完成8连通连通域分析
void wd_gridPointClustering_labelling_2(
	std::vector<std::vector<SVzNL3DPoint>>& srcData,
	std::vector<std::vector<SSG_clusterLabel>>& labelMask,
	int clusterID, //当前Cluster的ID
	std::vector< SVzNL2DPoint>& a_cluster,  //result
	SVzNL3DRangeD& clusterRoi  //roi3D
)
{
	int i = 0;
	int lineNum = (int)srcData.size();
	int linePtNum = (int)srcData[0].size();

	SVzNL2DPoint& inputSeed = a_cluster[0];
	labelMask[inputSeed.x][inputSeed.y].flag = 1; //第一个种子
	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_clusterLabel& a_seed = labelMask[a_seedPos.x][a_seedPos.y];
		SVzNL3DPoint& a_feature3DValue = srcData[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_clusterLabel& chk_seed = labelMask[x][y];
						if ((chk_seed.validFlag == 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.xRange.min = clusterRoi.xRange.min > a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.min;
		clusterRoi.xRange.max = clusterRoi.xRange.max < a_feature3DValue.x ? a_feature3DValue.x : clusterRoi.xRange.max;
		clusterRoi.yRange.min = clusterRoi.yRange.min > a_feature3DValue.y ? a_feature3DValue.y : clusterRoi.yRange.min;
		clusterRoi.yRange.max = clusterRoi.yRange.max < a_feature3DValue.y ? a_feature3DValue.y : clusterRoi.yRange.max;
		clusterRoi.zRange.min = clusterRoi.zRange.min > a_feature3DValue.z ? a_feature3DValue.z : clusterRoi.zRange.min;
		clusterRoi.zRange.max = clusterRoi.zRange.max < a_feature3DValue.z ? a_feature3DValue.z : clusterRoi.zRange.max;
		i++;
	}
	return;
}