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

//读版本号
//version 1.0.0 : base version release to customer
//version 1.1.0 : 改进了算法，顶面点提取更准确
std::string	m_strVersion = "1.1.0";
const char* wd_gasFillingPortPositionVersion(void)
{
	return m_strVersion.c_str();
}

//提取加气口中心位姿
SSG_6DOF wd_getGasFillingPortPosition(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSX_gasFillingPortPara gasFillingPortPara,
	const SSG_lineSegParam lineSegPara,
	const SSG_outlierFilterParam filterParam,
	SSG_treeGrowParam growParam,
	int* errCode)
{
	*errCode = 0;
	SSG_6DOF resultPose;
	memset(&resultPose, 0, sizeof(SSG_6DOF));

	int lineNum = (int)scanLines.size();
	int linePtNum = (int)scanLines[0].size();
	bool isGridData = true;
	//提取直线段特征
	//垂直跳变特征提取
	SVzNLRangeD lineLenRange;
	lineLenRange.max = 1.2 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD);
	lineLenRange.min = 0.5 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD) / 2;
	std::vector<std::vector<SSG_featureSemiCircle>> lineFeatures_v_raw;
	std::vector<std::vector<SSG_featureSemiCircle>> invalidFeatures_v_raw;  //确定非直线特征，用于防止在水平处理时被认为是有效点
	for (int line = 0; line < lineNum; line++)
	{
		if (line == 310)
			int kkk = 1;

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

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

		std::vector<SSG_featureSemiCircle> line_features;
		std::vector<SSG_featureSemiCircle> invalid_features;
		int dataSize = (int)lineData.size();
		wd_surfaceLineSegment(
			lineData,
			line,
			lineLenRange,
			lineSegPara,
			line_features,
			invalid_features);
		lineFeatures_v_raw.push_back(line_features);
		invalidFeatures_v_raw.push_back(invalid_features);
	}

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

	//生成水平扫描
	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;
		}
	}
	//水平arc特征提取
	std::vector<std::vector<SSG_featureSemiCircle>> lineFeatures_h_raw;
	std::vector<std::vector<SSG_featureSemiCircle>> invalidFeatures_h_raw;  //确定非直线特征，用于防止在垂直处理时被认为是有效点
	int lineNum_h_raw = (int)hLines_raw.size();
	for (int line = 0; line < lineNum_h_raw; line++)
	{
		if (line == 974)
			int kkk = 1;
		std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
		//滤波，滤除异常点
		int ptNum = (int)lineData.size();
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);

		std::vector<SSG_featureSemiCircle> line_features;
		std::vector<SSG_featureSemiCircle> invalid_features;
		int dataSize = (int)lineData.size();
		wd_surfaceLineSegment(
			lineData,
			line,
			lineLenRange,
			lineSegPara,
			line_features,
			invalid_features);
		 lineFeatures_h_raw.push_back(line_features);
		 invalidFeatures_h_raw.push_back(invalid_features);
	}

	//标注
	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_featureSemiCircle>& a_lineJumpFeature = lineFeatures_v_raw[line];
		for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++)
		{
			int px = a_lineJumpFeature[m].lineIdx;
			int py = a_lineJumpFeature[m].midPtIdx;
			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].midPt;
		}
	}
	//水平标注
	for (int line = 0; line < lineNum_h_raw; line++)
	{
		std::vector<SSG_featureSemiCircle>& a_lineJumpFeature = lineFeatures_h_raw[line];
		for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++)
		{
			int py = a_lineJumpFeature[m].lineIdx;
			int px = a_lineJumpFeature[m].midPtIdx;
			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].midPt.y, a_lineJumpFeature[m].midPt.x, a_lineJumpFeature[m].midPt.z };
		}
	}
	//聚类
	//采用迭代思想，回归思路进行高效聚类
	std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
	std::vector<SVzNL3DRangeD> clustersRoi3D;
	int clusterID = 1;
	int clusterCheckWin = 5;
	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_pointClustering2D(
				featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
				feature3DInfo,//double,记录坐标信息
				clusterCheckWin, //搜索窗口
				growParam,//聚类条件
				clusterID, //当前Cluster的ID
				a_cluster,  //result
				a_clusterRoi
			);
			clusters.push_back(a_cluster);
			clustersRoi3D.push_back(a_clusterRoi);
			clusterID++;
		}
	}
	//聚类结果分析
	//取最前面的一个聚类
	int clusterSize = (int)clusters.size();
	int bestClusterIdx = -1;
	double minZ = -1;
	double wTh1 = gasFillingPortPara.innerD * 0.8;
	double wTh2 = gasFillingPortPara.outerD * 1.1;
	for (int i = 0; i < clusterSize; i++)
	{
		SVzNL3DRangeD& a_roi = clustersRoi3D[i];
		double xWidth = a_roi.xRange.max - a_roi.xRange.min;
		double yWidth = a_roi.yRange.max - a_roi.yRange.min;
		double meanZ = (a_roi.zRange.min + a_roi.zRange.max) / 2;
		if ((xWidth > wTh1) && (xWidth < wTh2) && (yWidth > wTh1) && (yWidth < wTh2))
		{
			if (minZ < 0)
			{
				minZ = meanZ;
				bestClusterIdx = i;
			}
			else
			{
				if (minZ > meanZ)
				{
					minZ = meanZ;
					bestClusterIdx = i;
				}
			}
		}
	}
	if (minZ < 0)
	{
		*errCode = SG_ERR_NOT_GRID_FORMAT;
		return resultPose;
	}

	std::vector< SVzNL2DPoint>& obj_cluster = clusters[bestClusterIdx];
	//提取端面上的点
	//首先将无效点置标志
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SSG_featureSemiCircle>& line_invalidFeature = invalidFeatures_v_raw[line];
		for (int n = 0; n < line_invalidFeature.size(); n++)
		{
			SSG_featureSemiCircle& a_invalidFeature = line_invalidFeature[n];
			int lineIdx = a_invalidFeature.lineIdx;
			for (int m = a_invalidFeature.startPtIdx; m <= a_invalidFeature.endPtIdx; m++)
			{
				SVzNL3DPosition& a_pt3d = scanLines[lineIdx][m];
				a_pt3d.nPointIdx = 2;
			}
		}
	}
	for (int line = 0; line < lineNum_h_raw; line++)
	{
		std::vector<SSG_featureSemiCircle>& line_invalidFeature = invalidFeatures_h_raw[line];
		for (int m = 0, m_max = (int)line_invalidFeature.size(); m < m_max; m++)
		{
			for (int n = 0; n < line_invalidFeature.size(); n++)
			{
				SSG_featureSemiCircle& a_invalidFeature = line_invalidFeature[n];
				int ptIdx = a_invalidFeature.lineIdx;
				for (int m = a_invalidFeature.startPtIdx; m <= a_invalidFeature.endPtIdx; m++)
				{
					SVzNL3DPosition& a_pt3d = scanLines[m][ptIdx];
					a_pt3d.nPointIdx = 3;
				}
			}
		}
	}

	//提取端面点
	std::vector< cv::Point3f> planePts;
	for (int i = 0, i_max = (int)obj_cluster.size(); i < i_max; i++)
	{
		SVzNL2DPoint a_pos = obj_cluster[i];
		SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[a_pos.x][a_pos.y];
		int type_v = a_featureInfo.featurType & 0x01;
		int type_h = a_featureInfo.featurType & 0x02;
		if (type_v > 0)
		{
			int lineIdx = a_featureInfo.lineIdx;
			int featureIdx = a_featureInfo.featureIdx_v;
			SSG_featureSemiCircle& a_feature = lineFeatures_v_raw[lineIdx][featureIdx];
			for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
			{
				SVzNL3DPosition& a_pt3d = scanLines[lineIdx][m];
				if ((a_pt3d.nPointIdx == 0) && (a_pt3d.pt3D.z > 1e-4))
				{
					a_pt3d.nPointIdx = 1;
					cv::Point3f a_planePt = { (float)a_pt3d.pt3D.x,  (float)a_pt3d.pt3D.y, (float)a_pt3d.pt3D.z };
					planePts.push_back(a_planePt);
				}
			}
		}
		if (type_h > 0)
		{
			int ptIdx = a_featureInfo.ptIdx;
			int featureIdx = a_featureInfo.featureIdx_h;
			SSG_featureSemiCircle& a_feature = lineFeatures_h_raw[ptIdx][featureIdx];
			for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++)
			{
				SVzNL3DPosition& a_pt3d = scanLines[m][ptIdx];
				if ( (a_pt3d.nPointIdx == 0) && (a_pt3d.pt3D.z >1e-4))
				{
					a_pt3d.nPointIdx = 1;
					cv::Point3f a_planePt = { (float)a_pt3d.pt3D.x,  (float)a_pt3d.pt3D.y, (float)a_pt3d.pt3D.z };
					planePts.push_back(a_planePt);
				}
			}
		}
	}
	// 拟合平面，计算法向
	//计算面参数: z = Ax + By + C
	//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
	std::vector<double> res;
	vzCaculateLaserPlane(planePts, res);
	//将平面调整为水平平面
	SSG_planeCalibPara calibPara = adjustPlaneToXYPlane(
		res[0], res[1], res[2]  //平面法向量
	);

	//投影
	std::vector< SVzNL3DPoint> projectionPts;
	double calibMeanZ = 0;
	int planePtSize = (int)planePts.size();
	for(int i = 0; i < planePtSize; i ++)
	{
		SVzNL3DPoint a_calibPt;
		a_calibPt.x = (float)(planePts[i].x * calibPara.planeCalib[0] + planePts[i].y * calibPara.planeCalib[1] + planePts[i].z * calibPara.planeCalib[2]);
		a_calibPt.y = (float)(planePts[i].x * calibPara.planeCalib[3] + planePts[i].y * calibPara.planeCalib[4] + planePts[i].z * calibPara.planeCalib[5]);
		a_calibPt.z = (float)(planePts[i].x * calibPara.planeCalib[6] + planePts[i].y * calibPara.planeCalib[7] + planePts[i].z * calibPara.planeCalib[8]);
		calibMeanZ += a_calibPt.z;
		projectionPts.push_back(a_calibPt);
	}
	calibMeanZ = calibMeanZ / planePtSize;
	//圆最小二乘拟合
	SVzNL3DPoint calibCenter;
	double radius = 0;
	double fittingErr = fitCircleByLeastSquare(
		projectionPts,
		calibCenter,
		radius);
	calibCenter.z = calibMeanZ;

	//center点旋转回去
	SVzNL3DPoint center;
	center.x = (float)(calibCenter.x * calibPara.invRMatrix[0] + calibCenter.y * calibPara.invRMatrix[1] + calibCenter.z * calibPara.invRMatrix[2]);
	center.y = (float)(calibCenter.x * calibPara.invRMatrix[3] + calibCenter.y * calibPara.invRMatrix[4] + calibCenter.z * calibPara.invRMatrix[5]);
	center.z = (float)(calibCenter.x * calibPara.invRMatrix[6] + calibCenter.y * calibPara.invRMatrix[7] + calibCenter.z * calibPara.invRMatrix[8]);

	resultPose.x = center.x;
	resultPose.y = center.y;
	resultPose.z = center.z;
	resultPose.x_roll = res[0];
	resultPose.y_pitch = res[1];
	resultPose.z_yaw = res[2];

	return resultPose;

}