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

//将ply格式的数据恢复成扫描行的数据形式，从而方面按行进行处理
void wd_getScanLines(
	std::vector<SVzNL3DPoint>& scanData,
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	std::vector<SVzNL3DPosition> a_line;
	for (int i = 0, i_max = scanData.size(); i < i_max; i++)
	{
		SVzNL3DPoint a_pt = scanData[i];
		SVzNL3DPosition a_idxPt;
		a_idxPt.nPointIdx = 0;
		a_idxPt.pt3D = a_pt;
		if (a_line.size() == 0)
			a_line.push_back(a_idxPt);
		else
		{
			SVzNL3DPosition pre_pt = a_line.back();
			double x_diff = abs(pre_pt.pt3D.x - a_idxPt.pt3D.x);
			if (x_diff > 1e-4)
			{
				scanLines.push_back(a_line);
				a_line.clear();
				a_line.push_back(a_idxPt);
			}
			else
				a_line.push_back(a_idxPt);
		}
	}
	//last line
	if(a_line.size() > 0)
		scanLines.push_back(a_line);
	return;
}

void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT_vector(a_line, camPoseR, groundH);
}

void wd_getGloveGrabPostion(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_gloveArcParam arcPara, 
	SSG_treeGrowParam growParam,
	std::vector<SSG_peakRgnInfo>& objOps,
	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();
	bool isGridData = true;
	//垂直arc特征提取
	std::vector<std::vector<SSG_basicFeature1D>> arcFeatures;
	for(int line = 0; line < lineNum; line ++)
	{
		std::vector<SSG_basicFeature1D> lineArcFeatures;
		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
		if (linePtNum != (int)lineData.size())
			isGridData = false;

		wd_getLineGloveArcs(
			lineData,
			line,
			arcPara,
			lineArcFeatures);
		arcFeatures.push_back(lineArcFeatures);
	}

	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	//生成水平扫描
	std::vector<std::vector<SVzNL3DPosition>> hLines;
	hLines.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		hLines[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			hLines[j][line] = scanLines[line][j];
			hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	//水平arc特征提取
	std::vector<std::vector<SSG_basicFeature1D>> arcFeatures_h;
	int lineNum_h = (int)hLines.size();
	for (int line = 0; line < lineNum_h; line++)
	{
		std::vector<SSG_basicFeature1D> lineArcFeatures;
		std::vector<SVzNL3DPosition>& lineData = hLines[line];
		wd_getLineGloveArcs(
			lineData,
			line,
			arcPara,
			lineArcFeatures);
		arcFeatures_h.push_back(lineArcFeatures);
	}

	//特征生长
	//垂直方向特征生长（激光线方向）
	std::vector<SSG_featureTree> v_trees;
	for (int line = 0; line < lineNum; line++)
	{
		bool isLastLine = false;
		if (line == lineNum - 1)
			isLastLine = true;
		std::vector<SSG_basicFeature1D>& a_lineArcFeature = arcFeatures[line];
		sg_lineFeaturesGrowing(
			line,
			isLastLine,
			a_lineArcFeature,
			v_trees,
			growParam);
	}
	//水平方向特征生长（扫描运动方向）
	std::vector<SSG_featureTree> h_trees;
	for (int line = 0; line < lineNum_h; line++)
	{
		if (line == 650)
			int kkk = 1;
		bool isLastLine = false;
		if (line == lineNum_h - 1)
			isLastLine = true;
		std::vector<SSG_basicFeature1D>& a_lineArcFeature = arcFeatures_h[line];
		sg_lineFeaturesGrowing(
			line,
			isLastLine,
			a_lineArcFeature,
			h_trees,
			growParam);
	}
	//保留最大平均转角的生长树
	double maxTurn_v = 0;
	int maxTurn_v_id = -1;
	for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
	{
		int nodeSize = (int)v_trees[i].treeNodes.size();
		v_trees[i].tree_value = v_trees[i].tree_value / (double)nodeSize;
		if (maxTurn_v_id < 0)
		{
			maxTurn_v_id = i;
			maxTurn_v = v_trees[i].tree_value;
		}
		else
		{
			if (maxTurn_v < v_trees[i].tree_value)
			{
				maxTurn_v_id = i;
				maxTurn_v = v_trees[i].tree_value;
			}
		}
	}
	double maxTurn_h = 0;
	int maxTurn_h_id = -1;
	for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
	{
		int nodeSize = (int)h_trees[i].treeNodes.size();
		h_trees[i].tree_value = h_trees[i].tree_value / (double)nodeSize;
		if (maxTurn_h_id < 0)
		{
			maxTurn_h_id = i;
			maxTurn_h = h_trees[i].tree_value;
		}
		else
		{
			if (maxTurn_h < h_trees[i].tree_value)
			{
				maxTurn_h_id = i;
				maxTurn_h = h_trees[i].tree_value;
			}
		}
	}
	if ((maxTurn_v_id >= 0) && (maxTurn_h_id < 0))
	{
		SSG_featureTree bestTree = v_trees[maxTurn_v_id];
		v_trees.clear();
		v_trees.push_back(bestTree);
	}
	else if ((maxTurn_v_id < 0) && (maxTurn_h_id >= 0))
	{
		SSG_featureTree bestTree = h_trees[maxTurn_h_id];
		h_trees.clear();
		h_trees.push_back(bestTree);
	}
	else if ((maxTurn_v_id >= 0) && (maxTurn_h_id >= 0))
	{
		if (maxTurn_h <= maxTurn_v)
		{
			h_trees.clear();
			SSG_featureTree bestTree = v_trees[maxTurn_v_id];
			v_trees.clear();
			v_trees.push_back(bestTree);
		}
		else
		{
			v_trees.clear();
			SSG_featureTree bestTree = h_trees[maxTurn_h_id];
			h_trees.clear();
			h_trees.push_back(bestTree);
		}
	}

	//tree信息
	std::vector<SSG_treeInfo> allTreesInfo; //不包含边界
	SSG_treeInfo a_nullTree;
	memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
	allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
	//标记，根据起点的生长树进行标注
	int hvTreeIdx = 1;
	for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_vTree = &v_trees[i];

		//记录Tree的信息
		SSG_treeInfo a_treeInfo;
		a_treeInfo.vTreeFlag = 1;
		a_treeInfo.treeIdx = hvTreeIdx;
		a_treeInfo.treeType = a_vTree->treeType;
		a_treeInfo.sLineIdx = a_vTree->sLineIdx;
		a_treeInfo.eLineIdx = a_vTree->eLineIdx;
		a_treeInfo.roi = a_vTree->roi;
		allTreesInfo.push_back(a_treeInfo);
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
			if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16;
				if (existEdgeId == 0)
				{
					scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
					scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
					scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
				}
			}
		}
		hvTreeIdx++;
	}
	int hTreeStart = hvTreeIdx;
	////标注:水平特征
	for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_hTree = &h_trees[i];
		//记录Tree的信息
		SSG_treeInfo a_treeInfo;
		a_treeInfo.vTreeFlag = 0;
		a_treeInfo.treeIdx = hvTreeIdx;
		a_treeInfo.treeType = a_hTree->treeType;
		a_treeInfo.sLineIdx = a_hTree->sLineIdx;
		a_treeInfo.eLineIdx = a_hTree->eLineIdx;
		a_treeInfo.roi.left = a_hTree->roi.top;  //水平扫描xy是交换的
		a_treeInfo.roi.right = a_hTree->roi.bottom;
		a_treeInfo.roi.top = a_hTree->roi.left;
		a_treeInfo.roi.bottom = a_hTree->roi.right;
		allTreesInfo.push_back(a_treeInfo);
		//在原始点云上标记，同时有Mask上标记
		for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
			if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置0
			{
				int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
				if (existEdgeId == 0)
				{
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
					scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
				}
			}
		}
		hvTreeIdx++;
	}
	int hvTreeSize = hvTreeIdx;


}