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

//version 1.0.0 : base version release to customer
std::string	m_strVersion = "1.0.0";
const char* wd_bagThreadPositioningVersion(void)
{
	return m_strVersion.c_str();
}
#if 1
//线头位置检测定位
void wd_bagThreadPositioning(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSX_ScanInfo scanInfo,  //true:激光线平行槽道；false:激光线垂直槽道
	const SSG_outlierFilterParam filterParam, //噪点过滤参数
	const SSG_cornerParam cornerPara, //V型特征参数
	const SSG_raisedFeatureParam raisedFeaturePara,//线尾凸起参数
	const SSG_treeGrowParam growParam, //特征生长参数
	std::vector<SSX_bagThreadInfo>& bagThreadInfo,
	int* errCode)
{
	*errCode = 0;
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}

	int linePtNum = (int)scanLines[0].size();
	//判断数据格式是否为grid。算法只能处理grid数据格式
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		if (linePtNum != (int)scanLines[line].size())
		{
			isGridData = false;
			break;
		}
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	//生成水平扫描数据
	//统计平均线间隔和点间隔，用于算法在计算前向角和后向角时加速
	double ptInterval = 0;
	int ptIntevalNum = 0;
	std::vector< std::vector<SVzNL3DPosition>> data_lines_h;  //水平扫描数据
	data_lines_h.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		data_lines_h[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			//scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			data_lines_h[j][line] = scanLines[line][j];
			data_lines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			data_lines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
			if (j > 0)
			{
				if ((scanLines[line][j - 1].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z > 1e-4))
				{
					ptInterval += abs(scanLines[line][j].pt3D.y - scanLines[line][j - 1].pt3D.y);
					ptIntevalNum++;
				}
			}
		}
	}
	if(ptIntevalNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}
	ptInterval = ptInterval / ptIntevalNum;
	int lineNum_h = linePtNum;
	int linePtNum_h = (int)data_lines_h[0].size();
	double lineInterval = 0;
	int lineIntervalNum = 0;
	for (int line = 0; line< lineNum_h; line++)
	{
		for (int j = 0, j_max = (int)data_lines_h[line].size(); j < j_max; j++)
		{
			data_lines_h[line][j].nPointIdx = j;
			if (j > 0)
			{
				if ((data_lines_h[line][j - 1].pt3D.z > 1e-4) && (data_lines_h[line][j].pt3D.z > 1e-4))
				{
					lineInterval += abs(data_lines_h[line][j].pt3D.y - data_lines_h[line][j - 1].pt3D.y);
					lineIntervalNum++;
				}
			}
		}
	}
	if (lineIntervalNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}
	lineInterval = lineInterval / lineIntervalNum;

	double vCornerScale = cornerPara.scale * 4;
	std::vector<std::vector<SSG_basicFeature1D>> cornerFeatures;
	std::vector<std::vector<SWD_segFeature>> raisedFeatures;
	if (false == scanInfo.isHorizonScan)
	{
		int validVCornerSCale = (int)(vCornerScale / ptInterval);
		//垂直扫描检测V型槽和线尾
		for (int line = 0; line < lineNum; line++)
		{
			if ((line == 577) ||  (line == 932))
				int kkk = 1;
			std::vector<SVzNL3DPosition>& lineData = scanLines[line];
			//滤波，滤除异常点
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
			//提取V型槽
			std::vector<SSG_basicFeature1D> line_cornerFeatures;
			std::vector<SSG_RUN_EX> segs;
			int dataSize = (int)lineData.size();
			wd_getLineCorerFeature_accelerate(
				lineData,
				line,
				cornerPara,
				ptInterval,
				segs,
				line_cornerFeatures  //拐点
			);
			//检查V型特征两侧，不能有跳变
			std::vector<SSG_basicFeature1D> valid_cornerFeatures;
			int vCornerSize = (int)line_cornerFeatures.size();
			int segSize = (int)segs.size();
			for (int m = 0; m < vCornerSize; m++)
			{
				int cornerPos = line_cornerFeatures[m].jumpPos2D.y;
				for (int n = 0; n < segSize; n++)
				{
					int segEnd = segs[n].start + segs[n].len - 1;
					if ((cornerPos >= segs[n].start) && (cornerPos <= segEnd))
					{
						int skip_1 = cornerPos - segs[n].start;
						int skip_2 = segEnd - cornerPos;
						if((skip_1 >= validVCornerSCale) && (skip_2 >= validVCornerSCale))
							valid_cornerFeatures.push_back(line_cornerFeatures[m]);
						break;
					}
				}
			}
			cornerFeatures.push_back(valid_cornerFeatures);

			//提取凸起段
			std::vector<SWD_segFeature> line_raisedFeatures;
			wd_getLineRaisedFeature(
				lineData,
				line,
				raisedFeaturePara, //凸起参数
				line_raisedFeatures  //凸起
			);
			raisedFeatures.push_back(line_raisedFeatures);
		}
#if 0
		//水平扫描检测袋子边
		std::vector<std::vector<SSG_basicFeature1D>> jumpdFeatures;
#endif
	}
	else
	{
		int validVCornerSCale = (int)(vCornerScale / lineInterval);
		//水平扫描检测V型槽和线尾
		for (int line = 0; line < lineNum_h; line++)
		{
			if (line == 329)
				int kkk = 1;
			std::vector<SVzNL3DPosition>& lineData = data_lines_h[line];
			//滤波，滤除异常点
			sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum_h, filterParam);
			//提取V型槽
			std::vector<SSG_basicFeature1D> line_cornerFeatures;
			std::vector<SSG_RUN_EX> segs;
			int dataSize = (int)lineData.size();
			wd_getLineCorerFeature_accelerate(
				lineData,
				line,
				cornerPara,
				lineInterval,
				segs,
				line_cornerFeatures  //拐点
			);
			//检查V型特征两侧，不能有跳变
			std::vector<SSG_basicFeature1D> valid_cornerFeatures;
			int vCornerSize = (int)line_cornerFeatures.size();
			int segSize = (int)segs.size();
			for (int m = 0; m < vCornerSize; m++)
			{
				int cornerPos = line_cornerFeatures[m].jumpPos2D.y;
				for (int n = 0; n < segSize; n++)
				{
					int segEnd = segs[n].start + segs[n].len - 1;
					if ((cornerPos >= segs[n].start) && (cornerPos <= segEnd))
					{
						int skip_1 = cornerPos - segs[n].start;
						int skip_2 = segEnd - cornerPos;
						if ((skip_1 >= validVCornerSCale) && (skip_2 >= validVCornerSCale))
							valid_cornerFeatures.push_back(line_cornerFeatures[m]);
						break;
					}
				}
			}
			cornerFeatures.push_back(valid_cornerFeatures);
			//提取凸起段
			std::vector<SWD_segFeature> line_raisedFeatures;
			wd_getLineRaisedFeature(
				lineData,
				line,
				raisedFeaturePara, //凸起参数
				line_raisedFeatures  //凸起
			);
			raisedFeatures.push_back(line_raisedFeatures);
		}
		//垂直扫描检测袋子边
	}

	//特征生长
	std::vector< SSG_featureTree> cornerGrowTrees;
	sg_cornerFeaturesGrowing(
		cornerFeatures,
		cornerGrowTrees,
		growParam);
	std::vector<SWD_segFeatureTree> raisedFeatureGrowTrees;
	wd_getSegFeatureGrowingTrees(
		raisedFeatures,
		raisedFeatureGrowTrees,
		growParam);

	if (cornerGrowTrees.size() == 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}
	int raisedTreeNum = (int)raisedFeatureGrowTrees.size();
	int threadTailTreeIdx = -1; //线尾所在的tree
	if (raisedTreeNum == 1)
		threadTailTreeIdx = 0;
	else if (raisedTreeNum > 1)
	{
		//取最长的tree作为线尾所在的tree
		int maxLines = 0;
		threadTailTreeIdx = 0;
		for (int i = 0; i < raisedTreeNum; i++)
		{
			int treeLines = raisedFeatureGrowTrees[i].eLineIdx - raisedFeatureGrowTrees[i].sLineIdx;
			if (maxLines < treeLines)
			{
				maxLines = treeLines;
				threadTailTreeIdx = i;
			}
		}
	}
	int cornerTreeNum = (int)cornerGrowTrees.size();
	int objTreeIdx = -1;
	if (threadTailTreeIdx < 0)
	{
		int maxLines = 0;
		//取最长的tree作为线缝所有的生长树
		for (int i = 0; i < cornerTreeNum; i++)
		{
			int treeLines = cornerGrowTrees[i].eLineIdx - cornerGrowTrees[i].sLineIdx;
			if (maxLines < treeLines)
			{
				maxLines = treeLines;
				objTreeIdx = i;
			}
		}
	}
	else
	{
		//取与线尾所在tree最近的tree作为线缝所在的生长树,计算2D距离，所以不需要区分水平和垂直扫描方式

		SVzNL3DPoint pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].startPt;
		SVzNL3DPoint pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[0].endPt;
		SVzNL3DPoint tail_end_1 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
		pt_1 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().startPt;
		pt_2 = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.back().endPt;
		SVzNL3DPoint tail_end_2 = { (pt_1.x + pt_2.x) / 2, (pt_1.y + pt_2.y) / 2 , (pt_1.z + pt_2.z) / 2 };
		double minDist = DBL_MAX;
		for (int i = 0; i < cornerTreeNum; i++)
		{
			//端点最近的距离为两个tree间的距离 
			SVzNL3DPoint end_1 = cornerGrowTrees[i].treeNodes[0].jumpPos;
			SVzNL3DPoint end_2 = cornerGrowTrees[i].treeNodes.back().jumpPos;
			int lineIdx_2 = cornerGrowTrees[i].eLineIdx;
			double dist_11 = sqrt(pow(tail_end_1.x - end_1.x, 2) + pow(tail_end_1.y - end_1.y, 2));
			double dist_12 = sqrt(pow(tail_end_1.x - end_2.x, 2) + pow(tail_end_1.y - end_2.y, 2));
			double dist_21 = sqrt(pow(tail_end_2.x - end_1.x, 2) + pow(tail_end_2.y - end_1.y, 2));
			double dist_22 = sqrt(pow(tail_end_2.x - end_2.x, 2) + pow(tail_end_2.y - end_2.y, 2));
			double dist = dist_11 < dist_12 ? dist_11 : dist_12;
			dist = dist < dist_21 ? dist : dist_21;
			dist = dist < dist_22 ? dist : dist_22;
			if (minDist > dist)
			{
				minDist = dist;
				objTreeIdx = i;
			}
		}
	}
	if(objTreeIdx < 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}
	
	//显示 
	//将原始数据的序列清0，转义使用
	for (int line = 0; line < lineNum; line++)
		for (int j = 0; j < linePtNum; j++)
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
	//置标志，用于debug
	{
		int nodeNum = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int lineIdx, ptIdx;
			if (false == scanInfo.isHorizonScan)
			{
				lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
				ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
			}
			else
			{
				lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.y;
				ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[j].jumpPos2D.x;
			}
			scanLines[lineIdx][ptIdx].nPointIdx = 1;
		}
	}
	if (threadTailTreeIdx >= 0)
	{
		int nodeNum = (int)raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes.size();
		for (int j = 0; j < nodeNum; j++)
		{
			int lineIdx, ptIdx;
			if (false == scanInfo.isHorizonScan)
			{
				lineIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
				for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
				{
					ptIdx = m;
					scanLines[lineIdx][ptIdx].nPointIdx = 2;
				}
			}
			else
			{
				ptIdx = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].lineIdx;
				for (int m = raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].startPtIdx; m <= raisedFeatureGrowTrees[threadTailTreeIdx].treeNodes[j].endPtIdx; m++)
				{
					lineIdx = m;
					scanLines[lineIdx][ptIdx].nPointIdx = 2;
				}
			}
		}
	}
	//提取针脚(提取4个）
	std::vector<SVzNLRect> stitchROIs;
	int nodeSize = (int)cornerGrowTrees[objTreeIdx].treeNodes.size();
	//提取针脚范围
	int pre_idx = -1;
	for (int i = 0; i < nodeSize; i++)
	{
		int nodeIdx = (true == scanInfo.scanFromThreadHead) ? i : (nodeSize - 1 - i);
		SSG_basicFeature1D& a_node = cornerGrowTrees[objTreeIdx].treeNodes[nodeIdx];	
		if(pre_idx >= 0)
		{
			SSG_basicFeature1D& pre_node = cornerGrowTrees[objTreeIdx].treeNodes[pre_idx];
			int line_diff = a_node.jumpPos2D.x < pre_node.jumpPos2D.x ? (pre_node.jumpPos2D.x - a_node.jumpPos2D.x) : (a_node.jumpPos2D.x - pre_node.jumpPos2D.x);
			if (line_diff > 2)
			{
				double width = (true == scanInfo.isHorizonScan)? (line_diff * ptInterval) : (line_diff * lineInterval);
				if (width > scanInfo.stitchWidth)
				{
					SVzNLRect a_stitch;
					memset(&a_stitch, 0, sizeof(SVzNLRect));
					if (a_node.jumpPos2D.x < pre_node.jumpPos2D.x)
					{
						a_stitch.left = a_node.jumpPos2D.x;
						a_stitch.right = pre_node.jumpPos2D.x;
					}
					else
					{
						a_stitch.left = pre_node.jumpPos2D.x;
						a_stitch.right = a_node.jumpPos2D.x;
					}
					if (a_node.jumpPos2D.y < pre_node.jumpPos2D.y)
					{
						a_stitch.top = a_node.jumpPos2D.y;
						a_stitch.bottom = pre_node.jumpPos2D.y;
					}
					else
					{
						a_stitch.top = pre_node.jumpPos2D.y;
						a_stitch.bottom = a_node.jumpPos2D.y;
					}
					stitchROIs.push_back(a_stitch);
					if (stitchROIs.size() >= 4)
						break;
				}
			}
		}
		pre_idx = nodeIdx;
	}
	//提取针脚位置
	if (stitchROIs.size() == 0)
	{
		*errCode = SG_ERR_ZERO_OBJECTS;
		return;
	}
	//建立node下标与扫描线序号的索引
	std::vector<int> backIndexing;
	int indexingSize = cornerGrowTrees[objTreeIdx].eLineIdx - cornerGrowTrees[objTreeIdx].sLineIdx + 1;
	backIndexing.resize(indexingSize);
	for (int i = 0; i < indexingSize; i++)
		backIndexing[i] = -1;
	for (int i = 0; i < nodeSize; i++)
	{
		int indexingIdx = cornerGrowTrees[objTreeIdx].treeNodes[i].jumpPos2D.x - cornerGrowTrees[objTreeIdx].sLineIdx;
		backIndexing[indexingIdx] = i;
	}

	int opDist_lines;
	if (false == scanInfo.isHorizonScan)  //垂直于线缝扫描
		opDist_lines = (int)(scanInfo.operateDist / lineInterval);
	else
		opDist_lines = (int)(scanInfo.operateDist / ptInterval);

	for (int i = 0, i_max = (int)stitchROIs.size(); i < i_max; i++)
	{
		//搜索Z值最高点
		SVzNLRect& a_stitch = stitchROIs[i];
		SVzNL3DPoint stitchPos = { 0, 0, -1 };
		for (int j = a_stitch.left + 1; j < a_stitch.right; j++)
		{
			SVzNL3DPoint linePeak = { 0, 0, -1 };
			for (int m = a_stitch.top; m <= a_stitch.bottom; m++)
			{
				SVzNL3DPoint a_pt;
				if (false == scanInfo.isHorizonScan)  //垂直于线缝扫描
					a_pt = scanLines[j][m].pt3D;
				else
					a_pt = scanLines[m][j].pt3D;
				if (a_pt.z > 1e-4)
				{
					if (linePeak.z < 0)
						linePeak = a_pt;
					else
					{
						if (linePeak.z > a_pt.z)
							linePeak = a_pt;
					}
				}
			}
			if (linePeak.z > 1e-4)
			{
				if (stitchPos.z < 0)
					stitchPos = linePeak;
				else
				{
					if (stitchPos.z > linePeak.z)
						stitchPos = linePeak;
				}
			}
		}
		//搜索下刀位置
		if (stitchPos.z > 1e-4)
		{
			int op_centerLine;
			int searchStart, searchEnd;
			if (true == scanInfo.scanFromThreadHead)
			{
				op_centerLine = a_stitch.right + opDist_lines;
				searchStart = a_stitch.right; 
				searchEnd = a_stitch.right + opDist_lines * 2;
				if (searchEnd > cornerGrowTrees[objTreeIdx].eLineIdx)
					searchEnd = cornerGrowTrees[objTreeIdx].eLineIdx;
			}
			else
			{
				op_centerLine = a_stitch.left - opDist_lines;
				searchEnd = a_stitch.left;
				searchStart = a_stitch.left - opDist_lines * 2;
				if (searchStart < cornerGrowTrees[objTreeIdx].sLineIdx)
					searchStart = cornerGrowTrees[objTreeIdx].sLineIdx;
			}
			//寻找最合适点
			int best_idx = -1;
			int minLineDist = INT_MAX;
			for (int j = searchStart; j <= searchEnd; j++)
			{
				int indexingIdx = j - cornerGrowTrees[objTreeIdx].sLineIdx;
				if (backIndexing[indexingIdx] >= 0)
				{
					int lineDist = j - op_centerLine;
					if (lineDist < 0)
						lineDist = -lineDist;
					if (minLineDist > lineDist)
					{
						minLineDist = lineDist;
						best_idx = backIndexing[indexingIdx];
					}
				}
			}
			if (best_idx >= 0)
			{
				int op_lineIdx, op_ptIdx;
				if (false == scanInfo.isHorizonScan)  //垂直于线缝扫描
				{
					op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x;
					op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y;
				}
				else
				{
					op_ptIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.x;
					op_lineIdx = cornerGrowTrees[objTreeIdx].treeNodes[best_idx].jumpPos2D.y;
				}
				SSX_bagThreadInfo a_stitchInfo;
				memset(&a_stitchInfo, 0, sizeof(SSX_bagThreadInfo));
				a_stitchInfo.threadPos = stitchPos;
				a_stitchInfo.operatePos = scanLines[op_lineIdx][op_ptIdx].pt3D;
				a_stitchInfo.rotateAngle = 0;
				bagThreadInfo.push_back(a_stitchInfo);
			}
		}
	}


	return;
}
#endif