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

#define DEBUG_OUT_IMAGE	1

std::string	m_strVersion = "1.0.0";
const char* wd_sheetPositionVersion(void)
{
	return m_strVersion.c_str();
}

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara wd_sheetPosition_getBaseCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_getPlaneCalibPara2(scanLines);
}

//相机姿态调平，并去除地面
void wd_sheetPosition_lineDataR(
	std::vector< SVzNL3DPosition>& a_line,
	const double* camPoseR,
	double groundH)
{
	lineDataRT_vector(a_line, camPoseR, groundH);
}

//提取边缘封闭目标的边缘特征，输出聚类簇
void wd_computeClosedEdgeClusters(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_planeCalibPara groundCalibPara,
	const SWD_sheetAlgoParam algoParam,
	int* errCode,
	std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
	std::vector<SVzNL3DRangeD>& clustersRoi3D)
{
	int lineNum = (int)scanLines.size();
	if (lineNum == 0)
	{
		*errCode = SG_ERR_3D_DATA_NULL;
		return;
	}

	bool isGridData = true;
	int linePtNum = (int)scanLines[0].size();
	for (int i = 0; i < lineNum; i++)
	{
		if (linePtNum != (int)scanLines[i].size())
			isGridData = false;//行处理
		//调平，去除地面
		wd_sheetPosition_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
	}
	if (false == isGridData)//数据不是网格格式
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return;
	}

	//提取薄片上的圆孔
	//垂直跳变特征提取
	std::vector<std::vector<SSG_basicFeature1D>> lineJumpFeatures_v;
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 76)
			int kkk = 1;

		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
		if (linePtNum != (int)lineData.size())
			isGridData = false;

		//滤波，滤除异常点
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, algoParam.filterParam);

		std::vector<SSG_basicFeature1D> line_jumpFeatures;
		int dataSize = (int)lineData.size();
		/// 提取激光线上的Jumping特征
		sg_getLineLJumpFeature_cornerMethod(
			&lineData[0],
			dataSize,
			line,
			algoParam.cornerParam,
			true,  //取上沿
			line_jumpFeatures);
		lineJumpFeatures_v.push_back(line_jumpFeatures);
	}

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

	//生成水平扫描
	std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
	hLines_raw.resize(linePtNum);
	for (int i = 0; i < linePtNum; i++)
		hLines_raw[i].resize(lineNum);
	for (int line = 0; line < lineNum; line++)
	{
		for (int j = 0; j < linePtNum; j++)
		{
			scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
			hLines_raw[j][line] = scanLines[line][j];
			hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
			hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
		}
	}
	//水平jump特征提取
	std::vector<std::vector<SSG_basicFeature1D>> lineJumpFeatures_h;
	int lineNum_h_raw = (int)hLines_raw.size();
	for (int line = 0; line < lineNum_h_raw; line++)
	{
		if (line == 522)
			int kkk = 1;
		std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
		//滤波，滤除异常点
		int ptNum = (int)lineData.size();
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, algoParam.filterParam);

		std::vector<SSG_basicFeature1D> line_jumpFeatures;
		int dataSize = (int)lineData.size();
		/// 提取激光线上的Jumping特征
		sg_getLineLJumpFeature_cornerMethod(
			&lineData[0],
			dataSize,
			line,
			algoParam.cornerParam,
			true, //取上沿
			line_jumpFeatures);
		lineJumpFeatures_h.push_back(line_jumpFeatures);
	}
	//标注
	std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
	std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
	featureInfoMask.resize(lineNum);
	feature3DInfo.resize(lineNum);
	for (int i = 0; i < lineNum; i++)
	{
		featureInfoMask[i].resize(lineNum_h_raw);
		feature3DInfo[i].resize(lineNum_h_raw);
	}

	//垂直标注
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 390)
			int kkk = 1;
		std::vector<SSG_basicFeature1D>& a_lineJumpFeature = lineJumpFeatures_v[line];
		for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++)
		{
			int px = a_lineJumpFeature[m].jumpPos2D.x;
			int py = a_lineJumpFeature[m].jumpPos2D.y;
			SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[px][py];
			a_featureInfo.clusterID = 0;
			a_featureInfo.featurType = 1;
			a_featureInfo.featureIdx_v = m;
			a_featureInfo.featureIdx_h = 0;
			a_featureInfo.lineIdx = px;
			a_featureInfo.ptIdx = py;
			a_featureInfo.flag = 0;
			SVzNL3DPoint& a_feature3D = feature3DInfo[px][py];
			a_feature3D = a_lineJumpFeature[m].jumpPos;

			scanLines[px][py].nPointIdx = 1;
		}
	}
	//水平标注
	for (int line = 0; line < lineNum_h_raw; line++)
	{
		std::vector<SSG_basicFeature1D>& a_lineJumpFeature = lineJumpFeatures_h[line];
		for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++)
		{
			int py = a_lineJumpFeature[m].jumpPos2D.x;
			int px = a_lineJumpFeature[m].jumpPos2D.y;
			SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[px][py];
			if (a_featureInfo.featurType == 0)
			{
				a_featureInfo.clusterID = 0;
				a_featureInfo.lineIdx = px;
				a_featureInfo.ptIdx = py;
				a_featureInfo.flag = 0;
			}
			a_featureInfo.featurType += 2;
			a_featureInfo.featureIdx_h = m;
			SVzNL3DPoint& a_feature3DValue = feature3DInfo[px][py];
			a_feature3DValue = { a_lineJumpFeature[m].jumpPos.y, a_lineJumpFeature[m].jumpPos.x, a_lineJumpFeature[m].jumpPos.z };

			scanLines[px][py].nPointIdx += 2;
		}
	}

	//聚类
	//采用迭代思想，回归思路进行高效聚类
	int clusterID = 1;
	int clusterCheckWin = 3;
	for (int y = 0; y < lineNum_h_raw; y++)
	{
		for (int x = 0; x < lineNum; x++)
		{
			SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
			if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
				continue;

			SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
			SVzNL3DRangeD a_clusterRoi;
			a_clusterRoi.xRange.min = a_feature3DValue.x;
			a_clusterRoi.xRange.max = a_feature3DValue.x;
			a_clusterRoi.yRange.min = a_feature3DValue.y;
			a_clusterRoi.yRange.max = a_feature3DValue.y;
			a_clusterRoi.zRange.min = a_feature3DValue.z;
			a_clusterRoi.zRange.max = a_feature3DValue.z;

			SVzNL2DPoint a_seedPos = { x, y };
			std::vector< SVzNL2DPoint> a_cluster;
			a_cluster.push_back(a_seedPos);
			wd_gridPointClustering(
				featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
				feature3DInfo,//double,记录坐标信息
				clusterCheckWin, //搜索窗口
				algoParam.growParam,//聚类条件
				clusterID, //当前Cluster的ID
				a_cluster,  //result
				a_clusterRoi
			);
			clusters.push_back(a_cluster);
			clustersRoi3D.push_back(a_clusterRoi);
			clusterID++;
		}
	}
}


//薄片定位
//使用语义模块匹配：（1）提取特征（2）基于特征匹配薄片
void wd_YouJiang_getSheetPosition(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SWD_sheetTemplateParam templateParam,
	const SSG_planeCalibPara groundCalibPara,
	const SWD_sheetAlgoParam algoParam,
	SWD_gripState* opState,  //操作状态机。指示当前状态
	int* errCode,
	std::vector<SWD_sheetGrasper>& resultObjPositions)
{
	std::vector<std::vector< SVzNL2DPoint>> clusters;//只记录位置
	std::vector<SVzNL3DRangeD> clustersRoi3D;
	//提取边缘封闭目标的边缘特征，输出聚类簇
	wd_computeClosedEdgeClusters(
		scanLines,
		groundCalibPara,
		algoParam,
		errCode,
		clusters, //只记录位置
		clustersRoi3D);

	int clusterNum = (int)clusters.size();
	//投影和标注。投影的目的是用于模板匹配时的特征处理。以1mm为单位.记录clusterID
	SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
	double projectionScale = 1.0;  //以1mm为投影尺度
	int projecction_xw = (int)((roi3D.xRange.max - roi3D.xRange.min) / projectionScale) + 1;
	int projecction_yh = (int)((roi3D.yRange.max - roi3D.yRange.min) / projectionScale) + 1;
	std::vector<std::vector<int>> projectionClusters;
	projectionClusters.resize(projecction_xw);
	for (int i = 0; i < projecction_yh; i++)
		projectionClusters[i].resize(projecction_yh);
	for (int i = 0; i < clusterNum; i++)
	{
		std::vector< SVzNL2DPoint>& a_cluster = clusters[i];
		for (int j = 0; j < (int)a_cluster.size(); j++)
		{
			SVzNL2DPoint& a_pt = a_cluster[j];
			SVzNL3DPosition& a_pt3d = scanLines[a_pt.x][a_pt.y];
			int px = (int)((a_pt3d.pt3D.x - roi3D.xRange.min) / projectionScale);
			int py = (int)((a_pt3d.pt3D.y - roi3D.yRange.min) / projectionScale);
			if ((px < 0) || (px >= projecction_xw) || (py < 0) || (py >= projecction_yh))
				int kkk = 1;
			projectionClusters[px][py] = i + 1; //clusterID = index + 1
		}
	}

	//聚类结果分析
	//根据大小确定圆孔
	std::vector<SVzNL3DPoint> clustersCenter;
	clustersCenter.resize(clusters.size());

	std::vector<int> validIndice;
	double diameter_th = templateParam.holeDiameter * 0.1;
	for (int i = 0; i < clusterNum; i++)
	{
		double xw = clustersRoi3D[i].xRange.max - clustersRoi3D[i].xRange.min;
		double yh = clustersRoi3D[i].yRange.max - clustersRoi3D[i].yRange.min;
		double xw_diff = abs(xw - templateParam.holeDiameter);
		double yh_diff = abs(yh - templateParam.holeDiameter);
		if ((xw_diff < diameter_th) && (yh_diff < diameter_th))
		{
			validIndice.push_back(i);
			//计算中心点
			
		}
	}
	//圆孔配对
	


	//确定正反和抓取点



}


typedef struct
{
	cv::RotatedRect rect;
	cv::Point2f vertices[4];
}_RectParam;
//薄片按键高度测量
void wd_computeSheetKeyHeigth(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SWD_sheetKeyParam sheetKeyParam,
	const SSG_planeCalibPara groundCalibPara,
	const SWD_sheetAlgoParam algoParam,
	SWD_gripState* opState,  //操作状态机。指示当前状态
	int* errCode,
	std::vector<SWD_sheetGrasper>& resultObjPositions)
{
	std::vector<std::vector< SVzNL2DPoint>> clusters;//只记录位置
	std::vector<SVzNL3DRangeD> clustersRoi3D;
	//提取边缘封闭目标的边缘特征，输出聚类簇
	wd_computeClosedEdgeClusters(
		scanLines,
		groundCalibPara,
		algoParam,
		errCode,
		clusters, //只记录位置
		clustersRoi3D);
	int clusterNum = (int)clusters.size();
	
	//聚类结果分析
	//根据大小确定薄片
	const double sameSizeTh_w = sheetKeyParam.sheetSize.width * 0.05;
	const double sameSizeTh_h = sheetKeyParam.sheetSize.height * 0.05;
	const double zAreaWin = 5;

	std::vector<_RectParam> clustersRect;
	clustersRect.resize(clusters.size());
#if 1
	std::vector<int> validIndice;
	
	for (int i = 0; i < clusterNum; i++)
	{
		std::vector< SVzNL2DPoint>& a_cluster = clusters[i];
		if (a_cluster.size() < 4)
			continue;
		//计算最小外接矩
		std::vector<cv::Point2f> points;
		SVzNLRect roi = { -1,-1,-1,-1 };
		for (int j = 0; j < (int)a_cluster.size(); j++)
		{
			SVzNL2DPoint& a_pt = a_cluster[j];
			SVzNL3DPosition& a_pt3d = scanLines[a_pt.x][a_pt.y];
			//生成ROI
			if (roi.left < 0)
				roi = { a_pt.x ,a_pt.x , a_pt.y, a_pt.y };
			else
			{
				if (roi.left > a_pt.x)
					roi.left = a_pt.x;
				if (roi.right < a_pt.x)
					roi.right = a_pt.x;
				if (roi.top > a_pt.y)
					roi.top = a_pt.y;
				if (roi.bottom < a_pt.y)
					roi.bottom = a_pt.y;
			}
			float x = (float)a_pt3d.pt3D.x;
			float y = (float)a_pt3d.pt3D.y;
			points.push_back(cv::Point2f(x, y));
		}
		if (points.size() == 0)
			continue;

		// 最小外接矩形
		cv::RotatedRect rect = minAreaRect(points);
		cv::Point2f vertices[4];
		rect.points(vertices);
		double width = rect.size.width;  //投影的宽和高， 对应袋子的长和宽
		double height = rect.size.height;
		if (width < height)
		{
			double tmp = height;
			height = width;
			width = tmp;
		}

		int cx = (roi.left + roi.right) / 2;
		int cy = (roi.top + roi.bottom) / 2;
		//判别
		double w_diff = abs(width - sheetKeyParam.sheetSize.width);
		double h_diff = abs(height - sheetKeyParam.sheetSize.height);
		if ((w_diff < sameSizeTh_w) && (h_diff < sameSizeTh_h))
		{
			//寻找到目标
			//取中心点，以中心点附近区域计算Z
			int cx = (roi.left + roi.right) / 2;
			int cy = (roi.top + roi.bottom) / 2;
			SVzNLRect meanZRoi = { cx - zAreaWin , cx + zAreaWin , cy - zAreaWin , cy + zAreaWin };
			double meanZ = computeROIMeanZ(scanLines, meanZRoi);
			//以Z值作为门限进行切割
			double cutZ = meanZ - sheetKeyParam.nominalKeyHeight * 0.75;
			int labelID = i + 100;  //ID从100开始
			computeROIZCutLabel(scanLines, roi, cutZ, labelID);
		}
	}
#endif
	

}