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

//version 1.0.0 : base version release to customer
//version 1.1.0 : 添加了轮眉到地面高度输出 
//version 1.2.0 : 修正了地面调平，防止法向量旋转180度 
std::string	m_strVersion = "1.2.0";
const char* wd_wheelArchHeigthMeasureVersion(void)
{
	return m_strVersion.c_str();
}

//相机水平安装计算地面调平参数。
//相机Z轴基本平行地面时，需要以地面为参照，将相机调水平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara wd_horizonCamera_getGroundCalibPara(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
	return sg_HCameraVScan_getGroundCalibPara(scanLines);
}

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

SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9])
{
	SVzNL3DPoint _r_pt;
	_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
	_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
	_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
	return _r_pt;
}

bool compareByPtSize(const SWD_clustersInfo& a, const SWD_clustersInfo& b) {
	return a.ptSize > b.ptSize;
}

//提取轮眉区域的下端点
void _getArcEndings(std::vector< std::vector<SVzNL3DPosition>>& scanLines, const int cluster_arc_id, std::vector<SVzNL2DPoint>& contourPts)
{
	int lineNum = (int)scanLines.size();
	
	for (int i = 0; i < lineNum; i++)
	{
		int ptNum = (int)scanLines[i].size();
		int lastIdx = -1;
		for (int j = 0; j < ptNum; j++)
		{
			if ((i == 380) && (j > 288))
				int kkk = 1;
			if ( (scanLines[i][j].nPointIdx == cluster_arc_id) && (scanLines[i][j].pt3D.z >1e-4))
				lastIdx = j;
		}
		if (lastIdx >= 0)
		{
			SVzNL2DPoint a_pt = { i, lastIdx};
			contourPts.push_back(a_pt);
		}
	}
}

//提取轮眉种子点，定义为y最小的端点
int _getArcEndingsCenterPos(std::vector< std::vector<SVzNL3DPosition>>& scanLines, std::vector<SVzNL2DPoint>& contourPts)
{
	double minY = DBL_MAX;
	int seedPosIdx = 0;
	for (int i = 0, i_max = (int)contourPts.size(); i < i_max; i++)
	{
		SVzNL2DPoint& a_pos = contourPts[i];
		double y = scanLines[a_pos.x][a_pos.y].pt3D.y;
		if ((minY > y) && (scanLines[a_pos.x][a_pos.y].pt3D.z > 1e-4))
		{
			seedPosIdx = i;
			minY = y;
		}
	}
	return seedPosIdx;
}

//使用端点直线，检查点到直线的距离，大于门限的分割
int computeMaxDistPos(
	int ptStartIdx, int ptEndIdx,
	std::vector< SVzNL3DPosition>& lineData)
{
	SVzNL3DPoint pt1 = lineData[ptStartIdx].pt3D;
	SVzNL3DPoint pt2 = lineData[ptEndIdx].pt3D;
	if ((pt1.z < 1e-4) || (pt2.z < 1e-4))
		return -1;
	double _a, _b, _c;
	compute2ptLine_2(
		pt1.y, pt1.z,
		pt2.y, pt2.z,
		&_a, &_b, &_c);
	//compute2ptLine(pt1, pt2, &_a, &_b, &_c);
	double denominator = sqrt(_a * _a + _b * _b);
	//归一化
	_a = _a / denominator;
	_b = _b / denominator;
	_c = _c / denominator;

	double maxDist = 0;
	int maxPos = 0;
	for (int i = ptStartIdx; i <= ptEndIdx; i++)
	{
		SVzNL3DPoint a_pt = lineData[i].pt3D;
		if (a_pt.z > 1e-4)
		{
			double dist = abs(a_pt.y * _a + a_pt.z * _b + _c);
			if (maxDist < dist)
			{
				maxDist = dist;
				maxPos = i;
			}
		}
	}
	return maxPos;
}

void _extractArcFittingPts(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines, 
	std::vector<SVzNL2DPoint>& contourPts, 
	double chkWin, 
	int seedPosIdx, 
	std::vector< SVzNL2DPoint>& arcFittingPos)
{
	int size = (int)contourPts.size();
	arcFittingPos.push_back(contourPts[seedPosIdx]);
	SVzNL3DPoint seedPt = scanLines[contourPts[seedPosIdx].x][contourPts[seedPosIdx].y].pt3D;
	for (int i = seedPosIdx - 1; i >= 0; i--)
	{
		SVzNL2DPoint chkPos = contourPts[i];
		SVzNL3DPoint chkPt = scanLines[chkPos.x][chkPos.y].pt3D;
		double len = sqrt(pow(chkPt.x - seedPt.x, 2) + pow(chkPt.y - seedPt.y, 2));
		if (len > chkWin)
			break;
		arcFittingPos.insert(arcFittingPos.begin(), chkPos);
	}
	for (int i = seedPosIdx + 1; i < size; i++)
	{
		SVzNL2DPoint chkPos = contourPts[i];
		SVzNL3DPoint chkPt = scanLines[chkPos.x][chkPos.y].pt3D;
		double len = sqrt(pow(chkPt.x - seedPt.x, 2) + pow(chkPt.y - seedPt.y, 2));
		if (len > chkWin)
			break;
		arcFittingPos.push_back(chkPos);
	}

	//检查正确的端点，检查20mm
	double chkLen = 20;
	for (int i = 0, i_max = (int)arcFittingPos.size(); i < i_max; i++)
	{
		SVzNL2DPoint chkPos = arcFittingPos[i];
		SVzNL3DPoint chkPt = scanLines[chkPos.x][chkPos.y].pt3D;
		int endIdx = chkPos.y;
		int startIdx = endIdx;
		for (int m = endIdx - 1; m >= 0; m--)
		{
			SVzNL3DPoint a_pt = scanLines[chkPos.x][m].pt3D;
			if (a_pt.z > 1e-4)
			{
				double len = sqrt(pow(a_pt.y - chkPt.y, 2) + pow(a_pt.z - chkPt.z, 2));
				if (len > chkLen)
					break;
				startIdx = m;
			}
		}
		if (startIdx != endIdx)
		{
			int maxPos = computeMaxDistPos(startIdx, endIdx, scanLines[chkPos.x]);
			SVzNL3DPoint max_pt = scanLines[chkPos.x][maxPos].pt3D;
			double len1 = sqrt(pow(max_pt.y - chkPt.y, 2) + pow(max_pt.z - chkPt.z, 2));
			//构建等腰三角形，重新寻找点到底边的距离最高点
			for (int m = maxPos - 1; m >= 0; m--)
			{
				SVzNL3DPoint a_pt = scanLines[chkPos.x][m].pt3D;
				if (a_pt.z > 1e-4)
				{
					double len = sqrt(pow(a_pt.y - max_pt.y, 2) + pow(a_pt.z - max_pt.z, 2));
					if (len > len1)
						break;
					startIdx = m;
				}
			}
			maxPos = computeMaxDistPos(startIdx, endIdx, scanLines[chkPos.x]);
			arcFittingPos[i].y = maxPos;
		}
	}

}

SVzNL3DPoint _getWheelArcFittingPoint(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const int maskID,
	std::vector< SVzNL2DPoint>& fittingPos,
	SVzNL2DPoint& fittingPosition)
{
	//upWheel, 在XY平面生成拟合点
	std::vector<cv::Point2d> fittingPoints;
	for (int i = 0, i_max = (int)fittingPos.size(); i < i_max; i++)
	{
		int lineIdx = fittingPos[i].x;
		int ptIdx = fittingPos[i].y;
		cv::Point2d a_pt = { scanLines[lineIdx][ptIdx].pt3D.x,scanLines[lineIdx][ptIdx].pt3D.y };
		scanLines[lineIdx][ptIdx].nPointIdx = maskID;
		fittingPoints.push_back(a_pt);
	}

	double a, b, c, mse, max_err;
	bool fitResult = leastSquareParabolaFitEigen(
		fittingPoints, a, b, c, mse, max_err);
	//计算轮毂上顶点
	SVzNL3DPoint fitPt;
	fitPt.x = -b / (2 * a);
	fitPt.y = (4 * a * c - b * b) / (4 * a);
	//在轮廓点中寻找最近的点
	SVzNL2DPoint subOptiPtPos = { 0,0 };
	double minLen = DBL_MAX;
	for (int i = 0, i_max = (int)fittingPos.size(); i < i_max; i++)
	{
		int lineIdx = fittingPos[i].x;
		int ptIdx = fittingPos[i].y;
		cv::Point2d a_pt = { scanLines[lineIdx][ptIdx].pt3D.x,scanLines[lineIdx][ptIdx].pt3D.y };
		double len = sqrt(pow(fitPt.x - a_pt.x, 2) + pow(fitPt.y - a_pt.y, 2));
		if (minLen > len)
		{
			subOptiPtPos = fittingPos[i];
			minLen = len;
		}
	}
	fittingPosition = subOptiPtPos;
	fitPt.z = scanLines[subOptiPtPos.x][subOptiPtPos.y].pt3D.z;
	return fitPt;
}

//轮眉高度测量
WD_wheelArchInfo wd_wheelArchHeigthMeasure(
	std::vector< std::vector<SVzNL3DPosition>>& scanLines,
	const SSG_cornerParam cornerPara,
	const SSG_lineSegParam lineSegPara,
	const SSG_outlierFilterParam filterParam,
	const SSG_treeGrowParam growParam,
	const SSG_planeCalibPara groundCalibPara,
	int* errCode)
{
	*errCode = 0;
	WD_wheelArchInfo result;
	memset(&result, 0, sizeof(WD_wheelArchInfo));

	int lineNum = (int)scanLines.size();
	int linePtNum = (int)scanLines[0].size();
	bool isGridData = true;
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
		if (linePtNum != (int)lineData.size())
			isGridData = false;
		//滤波，滤除异常点
		sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
	}

	//生成水平扫描
	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特征提取
	for (int line = 0; line < linePtNum; 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<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(linePtNum);
		feature3DInfo[i].resize(linePtNum);
	}
	//生成Mask
	for (int line = 0; line < lineNum; line++)
	{
		std::vector<SVzNL3DPosition>& lineData = scanLines[line];
		for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
		{
			if (scanLines[line][ptIdx].pt3D.z > 1e-4)
			{
				SSG_featureClusteringInfo a_mask;
				memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
				a_mask.featurType = 1;
				a_mask.lineIdx = line;
				a_mask.ptIdx = ptIdx;
				featureInfoMask[line][ptIdx] = a_mask;
				feature3DInfo[line][ptIdx] = scanLines[line][ptIdx].pt3D;
			}
		}
	}
	//聚类
	//采用迭代思想，回归思路进行高效聚类
	std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
	std::vector<SWD_clustersInfo> clustersInfo;
	int clusterID = 1;
	int clusterCheckWin = 5;
	for (int y = 0; y < linePtNum; 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, //搜索窗口
				growParam,//聚类条件
				clusterID, //当前Cluster的ID
				a_cluster,  //result
				a_clusterRoi
			);
			clusters.push_back(a_cluster);
			SWD_clustersInfo a_info;
			a_info.clusterIdx = clusterID;
			a_info.ptSize = (int)a_cluster.size();
			a_info.roi3D = a_clusterRoi;
			clustersInfo.push_back(a_info);
			clusterID++;
		}
	}
	//聚类结果分析
	//取最大的两个聚类。上面的聚类是前盖板，下面的是车轮
	std::sort(clustersInfo.begin(), clustersInfo.end(), compareByPtSize);
	int cluseter_wheel_id, cluster_arc_id;
	if (clustersInfo[0].roi3D.yRange.max > clustersInfo[1].roi3D.yRange.max)
	{
		cluseter_wheel_id = clustersInfo[0].clusterIdx;
		cluster_arc_id = clustersInfo[1].clusterIdx;
	}
	else
	{
		cluseter_wheel_id = clustersInfo[1].clusterIdx;
		cluster_arc_id = clustersInfo[0].clusterIdx;
	}
	std::vector< SVzNL2DPoint>& cluster_wheel = clusters[cluseter_wheel_id - 1];
	std::vector< SVzNL2DPoint>& cluster_arc = clusters[cluster_arc_id - 1];
	for (int i = 0, i_max = (int)cluster_wheel.size(); i < i_max; i++)
	{
		int lineIdx = cluster_wheel[i].x;
		int ptIdx = cluster_wheel[i].y;
		scanLines[lineIdx][ptIdx].nPointIdx = 1;
	}
	for (int i = 0, i_max = (int)cluster_arc.size(); i < i_max; i++)
	{
		int lineIdx = cluster_arc[i].x;
		int ptIdx = cluster_arc[i].y;
		scanLines[lineIdx][ptIdx].nPointIdx = 2;
	}

	//寻找轮眉点
	//(1)快速寻找下端点（2）取中间区域，精确确定端点（3）抛物线拟合（4）计算轮眉点（最高点）
	std::vector<SVzNL2DPoint> arcEndings;
	_getArcEndings(scanLines, 2, arcEndings); //轮眉的ID是2
	if (arcEndings.size() == 0)
	{
		*errCode = SX_ERR_INVALID_ARC;
		return result;
	}
	int arcMidPos = _getArcEndingsCenterPos(scanLines, arcEndings);
	//取两侧固定长度（100mm内的点，进行精确边界提取，拟合）
	double chkWin = 100.0;
	std::vector< SVzNL2DPoint> arcFittingPos;
	_extractArcFittingPts(
		scanLines,
		arcEndings,
		chkWin,
		arcMidPos,
		arcFittingPos);
	//在XY平面生成拟合点
	double outLineLen = 200;
	SVzNL2DPoint arcPtPos;
	SVzNL3DPoint arcPt = _getWheelArcFittingPoint(scanLines, 3, arcFittingPos, arcPtPos);
	result.wheelArchPos = arcPt;
	result.arcLine[0] = { arcPt.x - outLineLen, arcPt.y, arcPt.z };
	result.arcLine[1] = { arcPt.x + outLineLen, arcPt.y, arcPt.z };
	
	//提取轮毂特征：V型槽。连长6mm，角度最大的V型槽
	std::vector< SVzNL2DPoint> upWheelPos;
	std::vector< SVzNL2DPoint> downWheelPos;
	int startLine = arcFittingPos[0].x;
	int endLine = arcFittingPos.back().x;
	SSG_cornerParam wheelCornerPara;
	memset(&wheelCornerPara, 0, sizeof(SSG_cornerParam));
	wheelCornerPara.scale = 6.0;
	wheelCornerPara.cornerTh = 75.0;
	for (int line = startLine; line <= endLine; line++)
	{
		std::vector<SSG_basicFeature1D> cornerFeatures;
		sg_maskData_getLineCornerFeature(
			scanLines[line],
			line,
			1, //车轮的ID为1 
			wheelCornerPara, //scale通常取bagH的1/4
			cornerFeatures);
		//取第一个为up
		if (cornerFeatures.size() >= 2)
		{
			upWheelPos.push_back(cornerFeatures[0].jumpPos2D);
			downWheelPos.push_back(cornerFeatures.back().jumpPos2D);
		}
	}
	SVzNL2DPoint upPos;
	SVzNL3DPoint upWheelPt = _getWheelArcFittingPoint(scanLines, 4, upWheelPos, upPos);
	result.wheelUpPos = upWheelPt;
	result.upLine[0] = { upWheelPt.x - outLineLen, upWheelPt.y, upWheelPt.z };
	result.upLine[1] = { upWheelPt.x + outLineLen, upWheelPt.y, upWheelPt.z };
	SVzNL2DPoint downPos;
	SVzNL3DPoint downWheelPt = _getWheelArcFittingPoint(scanLines, 5, downWheelPos, downPos);
	result.wheelDownPos = downWheelPt;
	result.downLine[0] = { downWheelPt.x - outLineLen, downWheelPt.y, downWheelPt.z };
	result.downLine[1] = { downWheelPt.x + outLineLen, downWheelPt.y, downWheelPt.z };
	double centerY = (upWheelPt.y + downWheelPt.y) / 2;
	int searchLine = (upPos.x + downPos.x) / 2;
	double minDist = DBL_MAX;
	int minPtIdx = 0;
	for (int i = 0; i < (int)scanLines[searchLine].size(); i++)
	{
		if (scanLines[searchLine][i].pt3D.z > 1e-4)
		{
			double dist = abs(scanLines[searchLine][i].pt3D.y - centerY);
			if (minDist > dist)
			{
				minDist = dist;
				minPtIdx = i;
			}
		}
	}
	result.centerLine[0] = { downWheelPt.x - outLineLen, centerY, scanLines[searchLine][minPtIdx].pt3D.z };
	result.centerLine[1] = { downWheelPt.x + outLineLen, centerY, scanLines[searchLine][minPtIdx].pt3D.z };
	result.archToCenterHeigth = centerY - arcPt.y;
	result.archToGroundHeigth = groundCalibPara.planeHeight - arcPt.y;
	//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
	for (int i = 0; i < lineNum; i++)
		lineDataRT_vector(scanLines[i], groundCalibPara.invRMatrix, -1); 
	//将检测结果重新投射回原来的坐标系
	result.wheelArchPos = _ptRotate(result.wheelArchPos, groundCalibPara.invRMatrix);
	result.wheelUpPos = _ptRotate(result.wheelUpPos, groundCalibPara.invRMatrix);
	result.wheelDownPos = _ptRotate(result.wheelDownPos, groundCalibPara.invRMatrix);
	for (int i = 0; i < 2; i++)
	{
		result.arcLine[i] = _ptRotate(result.arcLine[i], groundCalibPara.invRMatrix);
		result.centerLine[i] = _ptRotate(result.centerLine[i], groundCalibPara.invRMatrix);
		result.downLine[i] = _ptRotate(result.downLine[i], groundCalibPara.invRMatrix);
		result.upLine[i] = _ptRotate(result.upLine[i], groundCalibPara.invRMatrix);
	}
	return result;
}