#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "rodAndBarDetection_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_rodAndBarDetectionVersion(void)
{
	return m_strVersion.c_str();
}

 void sx_hexHeadScrewMeasure(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	bool isHorizonScan,  //true:激光线平行槽道；false:激光线垂直槽道
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	double rodRidius,
	std::vector<SSX_hexHeadScrewInfo>& screwInfo,
	int* errCode)
{
	*errCode = 0;
	SSX_hexHeadScrewInfo screwInfo;
	memset(&screwInfo, 0, sizeof(SSX_hexHeadScrewInfo));
	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;
	}

	std::vector< std::vector<SVzNL3DPosition>> data_lines;
	if (false == isHorizonScan)
	{
		data_lines.resize(lineNum);
		for (int line = 0; line < lineNum; line++)
		{
			data_lines[line].insert(data_lines[line].end(), scanLines[line].begin(), scanLines[line].end());
			for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
			{
				data_lines[line][j].nPointIdx = j;
				scanLines[line][j].nPointIdx = 0; //转义复用
			}
		}
	}
	else
	{
		data_lines.resize(linePtNum);
		for (int i = 0; i < linePtNum; i++)
			data_lines[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[j][line] = scanLines[line][j];
				data_lines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
				data_lines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
			}

		}

		lineNum = linePtNum;
		linePtNum = (int)data_lines[0].size();
		for (int line = 0; line < lineNum; line++)
		{
			for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
				data_lines[line][j].nPointIdx = j;
		}
	}

	std::vector<std::vector<SWD_segFeature>> arcFeatures;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 44)
			int kkk = 1;
		std::vector<SVzNL3DPosition>& lineData = data_lines[line];

		//滤波，滤除异常点
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
		std::vector<SWD_segFeature> line_ringArcs;
		int dataSize = (int)lineData.size();
		//提取Arc特征
		wd_getRingArcFeature(
			lineData,
			line, //当前扫描线序号
			cornerPara,
			rodRidius / 2, //环宽度
			line_ringArcs  //环
		);
		arcFeatures.push_back(line_ringArcs);
	}
	//特征生长
	std::vector<SWD_segFeatureTree> growTrees;
	wd_getSegFeatureGrowingTrees(
		arcFeatures,
		growTrees,
		growParam);

	if (growTrees.size() == 0)
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	int objNum = (int)growTrees.size();
	for (int i = 0; i < objNum; i++)
	{
		//空间直线拟合

		//投影

		//
	}


	std::vector<SSG_basicFeatureGap>& nodes_1 = growTrees[0].treeNodes;
	std::vector<SSG_basicFeatureGap>& nodes_2 = growTrees[1].treeNodes;
#if _OUTPUT_DEBUG_DATA
	for (int i = 0, i_max = (int)nodes_1.size(); i < i_max; i++)
	{
		int lineIdx, ptIdx;
		if (false == isHorizonScan)
		{
			lineIdx = nodes_1[i].lineIdx;
			ptIdx = nodes_1[i].gapPt_0.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 1;
			ptIdx = nodes_1[i].gapPt_1.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 2;
		}
		else
		{
			ptIdx = nodes_1[i].lineIdx;
			lineIdx = nodes_1[i].gapPt_0.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 1;
			lineIdx = nodes_1[i].gapPt_1.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 2;
		}
	}
	for (int i = 0, i_max = (int)nodes_2.size(); i < i_max; i++)
	{
		int lineIdx, ptIdx;
		if (false == isHorizonScan)
		{
			lineIdx = nodes_2[i].lineIdx;
			ptIdx = nodes_2[i].gapPt_0.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 1;
			ptIdx = nodes_2[i].gapPt_1.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 2;
		}
		else
		{
			ptIdx = nodes_2[i].lineIdx;
			lineIdx = nodes_2[i].gapPt_0.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 1;
			lineIdx = nodes_2[i].gapPt_1.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 2;
		}
	}
#endif

	//按扫描线配对
	std::vector<SSG_basicFeatureGap> line_1;  //line_1和line_2为按扫描线对应的两个槽道上的Gap特征
	std::vector<SSG_basicFeatureGap> line_2;
	int i_idx = 0;
	int j_idx = 0;
	while (1)
	{
		int line_idx1 = nodes_1[i_idx].lineIdx;
		int line_idx2 = nodes_2[j_idx].lineIdx;
		if (line_idx1 == line_idx2)
		{
			line_1.push_back(nodes_1[i_idx]);
			line_2.push_back(nodes_2[j_idx]);
			i_idx++;
			j_idx++;
		}
		else
		{
			if (line_idx1 < line_idx2)
				i_idx++;
			else
				j_idx++;
		}
		if ((i_idx >= (int)nodes_1.size()) || (j_idx >= (int)nodes_2.size()))
			break;
	}

#if _OUTPUT_DEBUG_DATA
	for (int i = 0, i_max = (int)line_1.size(); i < i_max; i++)
	{
		int lineIdx, ptIdx;
		if (false == isHorizonScan)
		{
			lineIdx = line_1[i].lineIdx;
			ptIdx = line_1[i].gapPt_0.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 3;
			ptIdx = line_1[i].gapPt_1.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 4;
		}
		else
		{
			ptIdx = line_1[i].lineIdx;
			lineIdx = line_1[i].gapPt_0.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 3;
			lineIdx = line_1[i].gapPt_1.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 4;
		}
	}
	for (int i = 0, i_max = (int)line_2.size(); i < i_max; i++)
	{
		int lineIdx, ptIdx;
		if (false == isHorizonScan)
		{
			lineIdx = line_2[i].lineIdx;
			ptIdx = line_2[i].gapPt_0.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 3;
			ptIdx = line_2[i].gapPt_1.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 4;
		}
		else
		{
			ptIdx = line_2[i].lineIdx;
			lineIdx = line_2[i].gapPt_0.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 3;
			lineIdx = line_2[i].gapPt_1.nPointIdx;
			scanLines[lineIdx][ptIdx].nPointIdx = 4;
		}
	}
#endif

	//旋转，保证扫描线与Gap垂直
	double angleSearchWin = 30;
	double angleStepping = 0.1;

	int loop = (int)(angleSearchWin / angleStepping);
	double bestSpace = 0;
	double rotateAngle = 0;
	for (int i = -loop; i <= loop; i++)
	{
		double angle = i * angleStepping;
		std::vector<SSG_basicFeatureGap> rotate_line_1;
		_XY_rotateLine(angle, line_1, rotate_line_1);
		std::vector<SSG_basicFeatureGap> rotate_line_2;
		_XY_rotateLine(angle, line_2, rotate_line_2);
		double meanSpace = _computeChannelSpace(rotate_line_1, rotate_line_2);
		if (bestSpace < meanSpace)
		{
			bestSpace = meanSpace;
			rotateAngle = angle;
		}
	}
	std::vector<SSG_basicFeatureGap> calib_line_1;
	_XY_rotateLine(rotateAngle, line_1, calib_line_1);
	std::vector<SSG_basicFeatureGap> calib_line_2;
	_XY_rotateLine(rotateAngle, line_2, calib_line_2);

	channelInfo.channelSpace = _computeChannelSpace(calib_line_1, calib_line_2);
	channelInfo.channelWidth[0] = _computeGapMeanWidth(calib_line_1);
	channelInfo.channelWidth[1] = _computeGapMeanWidth(calib_line_2);
	channelInfo.channelDepth[0] = _computeGapMeanDepth(calib_line_1, data_lines);
	channelInfo.channelDepth[1] = _computeGapMeanDepth(calib_line_2, data_lines);
	return channelInfo;
}