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


//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0 
SVzNL3DPoint _rotate2D(SVzNL3DPoint pt, double sinTheta, double cosTheta)
{
	SVzNL3DPoint rotatePt;
	rotatePt.x = pt.x * cosTheta - pt.y * sinTheta;
	rotatePt.y = pt.x * sinTheta + pt.y * cosTheta;
	rotatePt.z = pt.z;
	return rotatePt;
}

void _XY_rotateLine(double angle, std::vector<SSG_basicFeatureGap>& line_src, std::vector<SSG_basicFeatureGap>& rotate_dst)
{
	rotate_dst.resize(line_src.size());
	double sinTheta = sin(PI * angle / 180);
	double cosTheta = cos(PI * angle / 180);
	for (int i = 0, i_max = (int)line_src.size(); i < i_max; i++)
	{
		rotate_dst[i] = line_src[i];
		rotate_dst[i].gapPt_0.pt3D = _rotate2D(line_src[i].gapPt_0.pt3D, sinTheta, cosTheta);
		rotate_dst[i].gapPt_1.pt3D = _rotate2D(line_src[i].gapPt_1.pt3D, sinTheta, cosTheta);
	}
	return;
}

double _computeChannelSpace(
	std::vector<SSG_basicFeatureGap>& line_1, 
	std::vector<SSG_basicFeatureGap>& line_2)
{
	double meanSpace = 0;
	int dataSize = (int)line_1.size();
	if (dataSize == 0)
		return 0;
	for (int i = 0; i < dataSize; i++)
	{
		double cy_1 = (line_1[i].gapPt_0.pt3D.y + line_1[i].gapPt_1.pt3D.y) / 2;
		double cy_2 = (line_2[i].gapPt_0.pt3D.y + line_2[i].gapPt_1.pt3D.y) / 2;
		meanSpace += abs(cy_2 - cy_1);
	}
	meanSpace = meanSpace / dataSize;
	return meanSpace;
}

double _computeGapMeanWidth(std::vector<SSG_basicFeatureGap>& gap_data)
{
	double meanWidth = 0;
	int dataSize = (int)gap_data.size();
	if (dataSize == 0)
		return 0;
	for (int i = 0; i < dataSize; i++)
	{
		double width = abs(gap_data[i].gapPt_0.pt3D.y - gap_data[i].gapPt_1.pt3D.y);
		meanWidth += width;
	}
	meanWidth = meanWidth / dataSize;
	return meanWidth;
}

double _computeGapMeanDepth(std::vector<SSG_basicFeatureGap>& gap_data, std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	double meanDepth = 0;
	int dataSize = (int)gap_data.size();
	if (dataSize == 0)
		return 0;

	for (int i = 0; i < dataSize; i++)
	{
		int ptIdx_1 = gap_data[i].gapPt_0.nPointIdx;
		int ptIdx_2 = gap_data[i].gapPt_1.nPointIdx;
		int line = gap_data[i].lineIdx;
		double max_z = 0;
		for (int j = ptIdx_1; j <= ptIdx_2; j++)
		{
			if (scanLines[line][j].pt3D.z > 1e-4)
			{
				if (max_z < scanLines[line][j].pt3D.z)
					max_z = scanLines[line][j].pt3D.z;
			}
		}
		double depth = max_z - (gap_data[i].gapPt_0.pt3D.z + gap_data[i].gapPt_1.pt3D.z) / 2;
		meanDepth += depth;
	}
	meanDepth = meanDepth / dataSize;
	return meanDepth;
}

SSX_channelInfo sx_channelSpaceMeasure(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	bool isHorizonScan,  //true:激光线平行槽道；false:激光线垂直槽道
	const SSG_cornerParam cornerPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	const SSX_channelParam channelParam,
	int* errCode)
{
	*errCode = 0;
	SSX_channelInfo channelInfo;
	memset(&channelInfo, 0, sizeof(SSX_channelInfo));
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return channelInfo;
	}

	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 channelInfo;
	}

	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<SSG_basicFeatureGap>> gapFeatures;
	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<SSG_basicFeatureGap> line_gaps;
		int dataSize = (int)lineData.size();
		//提取Gap特征
		sg_getLineGapFeature(
			lineData, //扫描线
			line, //当前扫描线序号
			cornerPara,
			channelParam.channelWidthRng,
			line_gaps);
		gapFeatures.push_back(line_gaps);
	}
	//特征生长
	std::vector<SSG_gapFeatureTree> growTrees;
	for (int line = 0; line < lineNum; line++)
	{
		bool isLastLine = false;
		if (line == lineNum - 1)
			isLastLine = true;
		std::vector<SSG_basicFeatureGap>& line_gapFeatures = gapFeatures[line];

		sg_lineGapsGrowing(
			line,
			isLastLine,
			line_gapFeatures,
			growTrees,
			growParam);
	}
	if (growTrees.size() != 2)
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return channelInfo;
	}

	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;
}