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

//扫描线处理，进行垂直方向的特征提取和生长
void sg_boxCarMeasure_lineProc(SVzNL3DLaserLine* a_line, int lineIdx, int* errCode, std::vector<SSG_lineFeature>& all_vLineFeatures, std::vector<std::vector<int>>& noisePts, const SG_boxCarMeasureParam measureParam)
{
	SSG_lineFeature a_line_features;
	a_line_features.lineIdx = lineIdx;
	//滤波，滤除异常点
	std::vector<SVzNL3DPosition> filterData;
	std::vector<int> lineNoisePts;
	sg_lineDataRemoveOutlier(a_line->p3DPosition, a_line->nPositionCnt, measureParam.filterParam, filterData, lineNoisePts);
	noisePts.push_back(lineNoisePts);

	sg_getLineRigthAngleFeature(
		filterData.data(),
		filterData.size(),
		lineIdx,
		measureParam.templatePara_HF,
		measureParam.templatePara_FH,
		measureParam.templatePara_HR,
		measureParam.templatePara_RH,
		&a_line_features);
	//if ((a_line_features.features.size() > 0) || (a_line_features.endings.size() > 0))
	all_vLineFeatures.push_back(a_line_features);  //空行也加入，保证能按行号索引
	return;
}

typedef struct
{
	SSG_featureTree* hTree;
	SSG_featureTree* head_vTree;
	SSG_featureTree* tail_vTree;
	int head_vTree_linkType; //1-head, 2-tail
	int tail_vTree_linkType; //1-head, 2-tail
}boxCarEdge;

SSG_featureTree* _findNeighbouringTree(
	SVzNL3DPoint seedPt, 
	std::vector<SSG_featureTree>&trees, 
	double neighbourDistTh, 
	double lenTh,
	int* vTree_linkType)
{
	for (int i = 0, i_max = trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_tree = &trees[i];
		SVzNL3DPoint headPt = a_tree->treeNodes[0].jumpPos;
		SVzNL3DPoint tailPt = a_tree->treeNodes.back().jumpPos;
		double treeLen = sqrt(pow(tailPt.x - headPt.x, 2) +
			pow(tailPt.y - headPt.y, 2) +
			pow(tailPt.z - headPt.z, 2));
		if (treeLen > lenTh)
		{
			double dist_head = sqrt(pow(seedPt.x - headPt.x, 2) +
				pow(seedPt.y - headPt.y, 2) +
				pow(seedPt.z - headPt.z, 2));
			double dist_tail = sqrt(pow(seedPt.x - tailPt.x, 2) +
				pow(seedPt.y - tailPt.y, 2) +
				pow(seedPt.z - tailPt.z, 2));
			if (dist_head < neighbourDistTh)
			{
				*vTree_linkType = 1; //head link
				return a_tree;
			}
			else if (dist_tail < neighbourDistTh)
			{
				*vTree_linkType = 2; //tail link
				return a_tree;
			}
		}
	}
	*vTree_linkType = 0;
	return NULL;
}

void _getBoxCarDimensionInfo(boxCarEdge* carEdges, SSG_boxCarDimension* dimen)
{
	if ((carEdges->head_vTree_linkType == 0) || (carEdges->tail_vTree_linkType == 0))
		return;

	//计算端点
	SVzNL3DPoint hTree_firstNode = carEdges->hTree->treeNodes[0].jumpPos;
	SVzNL3DPoint htree_head = { hTree_firstNode.y, hTree_firstNode.x, hTree_firstNode.z };  //htree的坐标与vtree坐标的xy是相反的
	SVzNL3DPoint hTree_lastNode = carEdges->hTree->treeNodes.back().jumpPos;
	SVzNL3DPoint htree_tail = { hTree_lastNode.y, hTree_lastNode.x, hTree_lastNode.z };  //htree的坐标与vtree坐标的xy是相反的

	SVzNL3DPoint link_ending_0, link_ending_1;
	SVzNL3DPoint endings[4];
	if (carEdges->head_vTree_linkType == 1)  //head link
	{
		link_ending_0 = carEdges->head_vTree->treeNodes[0].jumpPos;
		endings[0] = carEdges->head_vTree->treeNodes.back().jumpPos;
	}
	else  //tail link
	{
		link_ending_0 = carEdges->head_vTree->treeNodes.back().jumpPos;
		endings[0] = carEdges->head_vTree->treeNodes[0].jumpPos;
	}

	if (carEdges->tail_vTree_linkType == 1)  //head link
	{
		link_ending_1 = carEdges->tail_vTree->treeNodes[0].jumpPos;
		endings[3] = carEdges->tail_vTree->treeNodes.back().jumpPos;
	}
	else  //tail link
	{
		link_ending_1 = carEdges->tail_vTree->treeNodes.back().jumpPos;
		endings[3] = carEdges->tail_vTree->treeNodes[0].jumpPos;
	}

	dimen->endings[0] = endings[0];
	dimen->endings[1] = { htree_head.x, link_ending_0.y, (htree_head.z + link_ending_0.z) / 2 };
	dimen->endings[2] = { htree_tail.x, link_ending_1.y, (htree_tail.z + link_ending_1.z) / 2 };
	dimen->endings[3] = endings[3];

	double len_1 = sqrt(pow(dimen->endings[0].x - dimen->endings[1].x, 2) + pow(dimen->endings[0].y - dimen->endings[1].y, 2));
	double len_2 = sqrt(pow(dimen->endings[2].x - dimen->endings[3].x, 2) + pow(dimen->endings[2].y - dimen->endings[3].y, 2));
	double w = sqrt(pow(dimen->endings[1].x - dimen->endings[2].x, 2) + pow(dimen->endings[1].y - dimen->endings[2].y, 2));
	dimen->L = len_1 < len_2 ? len_2 : len_1;
	dimen->W = w;
	double angle = atan((dimen->endings[0].y - dimen->endings[1].y) /(dimen->endings[0].x - dimen->endings[1].x));
	dimen->angle = angle * 180.0 / PI;
	return;
}

///数据输入必须是grid格式，以进行水平方向和垂直方向的处理
///（1）寻找直角点
///（2）对LINE_FEATURE_RIGHT_ANGLE_HF和LINE_FEATURE_RIGHT_ANGLE_RH两种直角点进行生长
///（3) 确定尺寸
SSG_boxCarDimension sg_getBoxCarDimension(
	SVzNL3DLaserLine* laser3DPoints,
	int lineNum,
	std::vector<SSG_lineFeature>& all_vLineFeatures,
	std::vector<std::vector<int>>& noisePts,
	const SG_boxCarMeasureParam measureParam)
{
	//噪声去除
	for (int i = 0; i < noisePts.size(); i++)
	{
		std::vector<int>& lineNoise = noisePts[i];
		for (int j = 0; j < lineNoise.size(); j++)
		{
			int ptIdx = lineNoise[j];
			laser3DPoints[i].p3DPosition[ptIdx].pt3D.z = 0;
		}
	}

	int hLineNum = laser3DPoints->nPositionCnt;  //Grid格式，所有扫描线的点数是一样的
	//生成水平扫描数据
	std::vector<std::vector<SVzNL3DPosition>> hLines;
	hLines.resize(hLineNum);
	for (int i = 0; i < hLineNum; i++)
		hLines[i].resize(lineNum);

	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < hLineNum; j++)
		{
			hLines[j][line] = laser3DPoints[line].p3DPosition[j];
			hLines[j][line].pt3D.x = laser3DPoints[line].p3DPosition[j].pt3D.y;
			hLines[j][line].pt3D.y = laser3DPoints[line].p3DPosition[j].pt3D.x;
		}
	}

	std::vector<SSG_lineFeature> all_hLineFeatures;
	std::vector<std::vector<int>> all_hLineNoises;
	//逐行提取特征
	for (int hLine = 0; hLine < hLineNum; hLine++)
	{
if (hLine == 520)
	int kkk = 1;
		//滤波，滤除异常点
		std::vector<SVzNL3DPosition> filterData;
		std::vector<int> lineNoise;
		sg_lineDataRemoveOutlier((
			SVzNL3DPosition*)hLines[hLine].data(), 
			(int)hLines[hLine].size(), 
			measureParam.filterParam,
			filterData, 
			lineNoise);
		all_hLineNoises.push_back(lineNoise);

		std::vector<SVzNL3DPosition> smoothData;
		sg_lineDataSmoothing(filterData, 5, smoothData);
		SSG_lineFeature a_hLine_featrues;
		a_hLine_featrues.lineIdx = hLine;
		sg_getLineRigthAngleFeature(
			smoothData.data(),
			smoothData.size(),
			hLine,
			measureParam.templatePara_HF,
			measureParam.templatePara_FH,
			measureParam.templatePara_HR,
			measureParam.templatePara_RH,
			&a_hLine_featrues);
		//if ((a_hLine_featrues.features.size() > 0) || (a_hLine_featrues.endings.size() > 0))
		all_hLineFeatures.push_back(a_hLine_featrues);//空行也加入，保证能按行号索引
	}

	//噪声去除
	for (int i = 0; i < all_hLineNoises.size(); i++)
	{
		if (i == 105)
			int kkk = 1;
		std::vector<int>& lineNoise = all_hLineNoises[i];
		for (int j = 0; j < lineNoise.size(); j++)
		{
			int lineIdx = lineNoise[j];
			laser3DPoints[lineIdx].p3DPosition[i].pt3D.z = 0;
		}
	}

	//迭代处理一次，防止由于行处理时smooth将一些点无效，面垂直处理的feature点正好落在这些无效点上
	for (int i = 0; i < lineNum; i++)
	{
		//逐行检查
		if (i == 175)
			int kkk = 1;
		SSG_lineFeature& a_line = all_vLineFeatures[i];
		if (a_line.features.size() > 0)
		{
			bool reDoFlag = false;
			for (int j = 0, j_max = a_line.features.size(); j < j_max; j++)
			{
				SSG_basicFeature1D& a_feature = a_line.features[j];
				if (laser3DPoints[a_feature.jumpPos2D.x].p3DPosition[a_feature.jumpPos2D.y].pt3D.z < 1e-4)
				{
					reDoFlag = true;
					break;
				}
			}
			if (true == reDoFlag)
			{
				SSG_lineFeature a_line_features;
				sg_getLineRigthAngleFeature(
					laser3DPoints[i].p3DPosition,
					laser3DPoints[i].nPositionCnt,
					i,
					measureParam.templatePara_HF,
					measureParam.templatePara_FH,
					measureParam.templatePara_HR,
					measureParam.templatePara_RH,
					&a_line_features);
				all_vLineFeatures[i].features.clear();
				all_vLineFeatures[i].features.insert(all_vLineFeatures[i].features.end(), a_line_features.features.begin(), a_line_features.features.end());
				all_vLineFeatures[i].endings.clear();
				all_vLineFeatures[i].endings.insert(all_vLineFeatures[i].endings.end(), a_line_features.endings.begin(), a_line_features.endings.end());
			}
		}
	}

	//垂直方向特征生长（相机扫描方向）
	std::vector<SSG_featureTree> all_v_trees;
	sg_getFeatureGrowingTrees(
		all_vLineFeatures,
		all_v_trees,
		measureParam.growParam);

	//水平方向特征生长
	std::vector<SSG_featureTree> all_h_trees;
	sg_getFeatureGrowingTrees(
		all_hLineFeatures,
		all_h_trees,
		measureParam.growParam);

	//过滤出合适的Tree:只有LINE_FEATURE_RIGHT_ANGLE_HF和LINE_FEATURE_RIGHT_ANGLE_RH的tree是符合要求的tree
	std::vector<SSG_featureTree> vld_v_trees;
	for (int i = 0, i_max = all_v_trees.size(); i < i_max; i++)
	{
		if ((LINE_FEATURE_RIGHT_ANGLE_HF == all_v_trees[i].treeType) ||
			(LINE_FEATURE_RIGHT_ANGLE_RH == all_v_trees[i].treeType))
			vld_v_trees.push_back(all_v_trees[i]);
	}
	std::vector<SSG_featureTree> vld_h_trees;
	for (int i = 0, i_max = all_h_trees.size(); i < i_max; i++)
	{
		if ((LINE_FEATURE_RIGHT_ANGLE_HF == all_h_trees[i].treeType) ||
			(LINE_FEATURE_RIGHT_ANGLE_RH == all_h_trees[i].treeType))
			vld_h_trees.push_back(all_h_trees[i]);
	}

	//在搜索h_trees中搜索两端均与v_trees邻接的生长树
	double neighbourDistTh = 400.0;
	std::vector< boxCarEdge> validBoxCarEdges;
	for (int i = 0, i_max = vld_h_trees.size(); i < i_max; i++)
	{
		SSG_featureTree* a_hTree = &vld_h_trees[i];
		//取出头尾，htree的坐标与vtree坐标的xy是相反的
		SVzNL3DPoint hTree_firstNode = a_hTree->treeNodes[0].jumpPos;
		SVzNL3DPoint htree_head = { hTree_firstNode.y, hTree_firstNode.x, hTree_firstNode.z };  //htree的坐标与vtree坐标的xy是相反的
		SVzNL3DPoint hTree_lastNode = a_hTree->treeNodes.back().jumpPos;
		SVzNL3DPoint htree_tail = { hTree_lastNode.y, hTree_lastNode.x, hTree_lastNode.z };  //htree的坐标与vtree坐标的xy是相反的
		double hTreeLen = sqrt(pow(htree_tail.x - htree_head.x, 2) +
			pow(htree_tail.y - htree_head.y, 2) +
			pow(htree_tail.z - htree_head.z, 2));
		int vTree_linkType_0 = 0;
		SSG_featureTree* head_link = _findNeighbouringTree(htree_head, vld_v_trees, neighbourDistTh, hTreeLen, &vTree_linkType_0);
		int vTree_linkType_1 = 0;
		SSG_featureTree* tail_link = _findNeighbouringTree(htree_tail, vld_v_trees, neighbourDistTh, hTreeLen, &vTree_linkType_1);
		if ((NULL == head_link) || (NULL == tail_link))
			continue;
		boxCarEdge car_edge = { a_hTree , head_link , tail_link, vTree_linkType_0, vTree_linkType_1 };
		validBoxCarEdges.push_back(car_edge);
	} 
	if (validBoxCarEdges.size() == 0)
	{
		SSG_boxCarDimension result_null;
		memset(&result_null, 0, sizeof(SSG_boxCarDimension));
		return result_null;
	}

	//取出最长的组合
	int maxLen = -1;
	int max_idx = 0;
	for (int i = 0, i_max = validBoxCarEdges.size(); i < i_max; i++)
	{
		int sum_len = abs(validBoxCarEdges[i].hTree->eLineIdx - validBoxCarEdges[i].hTree->sLineIdx) +
			abs(validBoxCarEdges[i].head_vTree->eLineIdx - validBoxCarEdges[i].head_vTree->sLineIdx) +
			abs(validBoxCarEdges[i].tail_vTree->eLineIdx - validBoxCarEdges[i].tail_vTree->sLineIdx);
		if (maxLen < 0)
		{
			maxLen = sum_len;
			max_idx = i;
		}
		else if (maxLen < sum_len)
		{
			maxLen = sum_len;
			max_idx = i;
		}
	}

	//用于标注
	std::vector<SSG_featureTree> v_trees;
	std::vector<SSG_featureTree> h_trees;
#if 1
	v_trees.push_back(*validBoxCarEdges[max_idx].head_vTree);
	v_trees.push_back(*validBoxCarEdges[max_idx].tail_vTree);
	h_trees.push_back(*validBoxCarEdges[max_idx].hTree);
#else
	v_trees.insert(v_trees.end(), all_v_trees.begin(), all_v_trees.end());
	h_trees.insert(h_trees.end(), all_h_trees.begin(), all_h_trees.end());
#endif
	//获取车厢信息
	SSG_boxCarDimension result_dimen;
	memset(&result_dimen, 0, sizeof(SSG_boxCarDimension));
	_getBoxCarDimensionInfo(&validBoxCarEdges[max_idx], &result_dimen);
	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 = v_trees.size(); i < i_max; i++)
	{
		if (i == 22)
			int kkk = 1;
		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 = a_vTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
			laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
			laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
			laser3DPoints[a_feature->jumpPos2D.x].p3DPosition[a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
		}
		hvTreeIdx++;
	}
	int hTreeStart = hvTreeIdx;
	////标注:水平特征
	for (int i = 0, i_max = h_trees.size(); i < i_max; i++)
	{
		if (i == 9)
			int kkk = 1;
		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 = a_hTree->treeNodes.size(); j < j_max; j++)
		{
			SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
			laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
			laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
			laser3DPoints[a_feature->jumpPos2D.y].p3DPosition[a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
		}
		hvTreeIdx++;
	}
	int hvTreeSize = hvTreeIdx;

	//统计VTree中LINE_FEATURE_RIGHT_ANGLE_HF和LINE_FEATURE_RIGHT_ANGLE_RH的数量
	std::vector<int> vscan_HF_trees;
	std::vector<int> vscan_RH_trees;
	for (int i = 0; i < v_trees.size(); i++)
	{
		if (LINE_FEATURE_RIGHT_ANGLE_HF == v_trees[i].treeType)
			vscan_HF_trees.push_back(i);
		else if (LINE_FEATURE_RIGHT_ANGLE_RH == v_trees[i].treeType)
			vscan_RH_trees.push_back(i);
	}
	//统计HTree中LINE_FEATURE_RIGHT_ANGLE_RH的数量
	std::vector<int> hscan_HF_trees;
	std::vector<int> hscan_RH_trees;
	for (int i = 0; i < h_trees.size(); i++)
	{
		if (LINE_FEATURE_RIGHT_ANGLE_HF == h_trees[i].treeType)
			hscan_HF_trees.push_back(i);
		else if (LINE_FEATURE_RIGHT_ANGLE_RH == h_trees[i].treeType)
			hscan_RH_trees.push_back(i);
	}

	return result_dimen;
}