#include <opencv2/opencv.hpp>
#include "SG_fireBrickAlgo.h"
#include "SG_fireBrick_Export.h"
#include "SG_errCode.h"
#include "SG_labelling.h"

#define M_PI       3.14159265358979323846   // pi
#define M_VALID_TOP_PLANE_PTNUM		1000

std::string	CSGFireBrick::m_strVersion = "1.0.0";

CSGFireBrick::CSGFireBrick()
{
	m_zHist.resize(Z_HIST_MAX_SIZE);
}

//使用RANSAC方法拟合平面，得到平面的法向，进而得到相机的旋转校正矩阵（3*3）
//poseR: 用于将得到的点云校正到相机与工作面垂直时的点云
//poseInvR：用于将得到的抓取点坐标调整到原始点云坐标。这个是因为进行手眼标定时是使用未校正的相机姿态进行的（相机使用标定板进行）
void sgGetCamPose_3DApproach(SVzNL3DLaserLine* scanData, int nLines, double* poseR, double* poseInvR)
{

}

void sgGetCamPose_2DApproach(double* poseR, double* poseInvR)
{

}

/// @brief
/// 创建实例
bool CSGFireBrick::CreateInstance(double dHistScale, ISGFireBrick** ppFireBrick)
{
	CSGFireBrick* p = new CSGFireBrick;
	if (false == p->_Init(dHistScale))
	{
		delete p;
		p = nullptr;
	}
	*ppFireBrick = p;
	return nullptr != (*ppFireBrick);
}

const char* CSGFireBrick::GetVersion()
{
	return m_strVersion.c_str();
}

void CSGFireBrick::sgCalibCamPose()
{

}

void CSGFireBrick::sgSegHistScale(double data)
{
	m_histScale = data;
	return;
}

void CSGFireBrick::sgSetPoseSortingMode(ESG_poseSortingMode mode)
{
	m_sortingMode = mode;
	return;
}

ESG_poseSortingMode CSGFireBrick::sgGetPoseSortingMode()
{
	return m_sortingMode;
}

//扫描线处理：针对每一条扫描线进行处理，在扫描的同时进行处理
//直线段提取，和基于直线段的生长
void CSGFireBrick::sgScanLineProc(SVzNL3DLaserLine* a_line, double* camPoseR, int* errCode)
{
	*errCode = 0;
	if (m_nFrameWidth < 0)
	{
		m_nFrameWidth = a_line->nPositionCnt;
		m_nFrameHeight = 1;
	}
	else
	{
		if (m_nFrameWidth != a_line->nPositionCnt)
		{
			*errCode = SG_ERR_NOT_GRID_FORMAT;
			return;
		}
		m_nFrameHeight++;
	}
	//(1)校正
	//(2)Z方向统计
	for (int i = 0; i < a_line->nPositionCnt; i++)
	{
		SVzNL3DPosition* a_pt = &a_line->p3DPosition[i];
		if (a_pt->pt3D.z > 1e-4)
		{
			double x = camPoseR[0] * a_pt->pt3D.x + camPoseR[1] * a_pt->pt3D.y + camPoseR[2] * a_pt->pt3D.z;
			double y = camPoseR[3] * a_pt->pt3D.x + camPoseR[4] * a_pt->pt3D.y + camPoseR[5] * a_pt->pt3D.z;
			double z = camPoseR[6] * a_pt->pt3D.x + camPoseR[7] * a_pt->pt3D.y + camPoseR[8] * a_pt->pt3D.z;
			a_pt->pt3D.x = x;
			a_pt->pt3D.y = y;
			a_pt->pt3D.z = z;
			//Hist
			int idx = (int)(z / m_histScale);  //0.5mm
			m_zHist[idx] = m_zHist[idx]+1;
			//统计Z范围
			if (m_zIdxRange.nMin < 0)
			{
				m_zIdxRange.nMin = idx;
				m_zIdxRange.nMax = idx;
			}
			else
			{
				if (m_zIdxRange.nMin > idx)
					m_zIdxRange.nMin = idx;
				if (m_zIdxRange.nMax < idx)
					m_zIdxRange.nMax = idx;
			}
		}
	}
}

void _sgSearchPeaks(std::vector<int> sumHist, std::vector<SSG_RunData>& peaks)
{
	int state = 0; //0-初态；1-上升；2-下降
	int preSum = -1;
	SSG_RunData a_run = {0, 0, 0};
	for (int i = 0, i_max = sumHist.size(); i < i_max; i++)
	{
		int curSum = sumHist[i];
		if (0 == state)
		{
			if (preSum >= 0)
			{
				if (curSum > preSum)
				{
					state = 1;
					a_run.start = i;
					a_run.len = 1;
					a_run.value = curSum;
				}
				else if (curSum < preSum)
				{
					state = 2;
				}
			}
			preSum = curSum;
		}
		else if (1 == state)  //上升
		{
			if (curSum > preSum)
			{
				a_run.start = i;
				a_run.len = 1;
				a_run.value = curSum;
				if(i == i_max-1)
					peaks.push_back(a_run);
			}
			else if (curSum == preSum)
			{
				a_run.len++;
				if (i == i_max - 1)
					peaks.push_back(a_run);
			}
			else
			{
				peaks.push_back(a_run);
				a_run = { 0, 0, 0 };
				state = 2;
			}
			preSum = curSum;
		}
		else if (2 == state)  //下降
		{
			if (curSum > preSum)
			{
				state = 1;
				a_run.start = i;
				a_run.len = 1;
				a_run.value = curSum;
				if (i == i_max - 1)
					peaks.push_back(a_run);
			}
			preSum = curSum;
		}
		else  //异常
		{
			state = 0;
			continue;
		}
	}
}

bool _checkHOverlap(const SSG_Region* rgn_1, const SSG_Region* rgn_2)
{
	if ((rgn_1->roi.right > rgn_2->roi.left) && (rgn_2->roi.right > rgn_1->roi.left))
	{
		int overlap_left = rgn_1->roi.left > rgn_2->roi.left ? rgn_1->roi.left : rgn_2->roi.left;
		int overlap_right = rgn_1->roi.right < rgn_2->roi.right ? rgn_1->roi.right : rgn_2->roi.right;
		int overlap_width = overlap_right - overlap_left;
		int w_1 = rgn_1->roi.right - rgn_1->roi.left;
		int w_2 = rgn_2->roi.right - rgn_2->roi.left;
		if ((overlap_width > w_1 / 2) && (overlap_width > w_2 / 2))
			return true;
		else
			return false;
	}
	return false;
}

bool _checkVOverlap(const SSG_Region* rgn_1, const SSG_Region* rgn_2)
{
	if ((rgn_1->roi.bottom > rgn_2->roi.top) && (rgn_2->roi.bottom > rgn_1->roi.top))
	{
		int overlap_top = rgn_1->roi.top > rgn_2->roi.top ? rgn_1->roi.top : rgn_2->roi.top;
		int overlap_bottom = rgn_1->roi.bottom < rgn_2->roi.bottom ? rgn_1->roi.bottom : rgn_2->roi.bottom;
		int overlap_height = overlap_bottom - overlap_top;
		int h_1 = rgn_1->roi.bottom - rgn_1->roi.top;
		int h_2 = rgn_2->roi.bottom - rgn_2->roi.top;
		if ((overlap_height > h_1 / 2) && (overlap_height > h_2 / 2))
			return true;
		else
			return false;
	}
	return false;
}

SSG_Region* _getSortedObj(std::vector<SSG_Region>& labelRgns, ESG_poseSortingMode sortingMode, int* errCode)
{
	*errCode = 0;
	SSG_Region* objRgn = nullptr;
	//按照排序挑选一个目标
	for (int i = 0, i_max = labelRgns.size(); i < i_max; i++)
	{
		if (labelRgns[i].ptCounter > M_VALID_TOP_PLANE_PTNUM)
		{
			if (nullptr == objRgn)
				objRgn = &labelRgns[i];
			else
			{
				SSG_Region* chkRgn = &labelRgns[i];
				//计算水平重叠区和垂直重叠区
				bool isVOverlap = _checkVOverlap(objRgn, chkRgn);
				bool isHOverlap = _checkHOverlap(objRgn, chkRgn);
				//默认从左到右，从上到下
				if ((keSG_PoseSorting_L2R_T2B == sortingMode) || (keSG_PoseSorting_Uknown == sortingMode))
				{
					if (true == isHOverlap)
					{
						//T2B
						if (objRgn->roi.top > chkRgn->roi.top)
							objRgn = chkRgn;
					}
					else
						if (objRgn->roi.left > chkRgn->roi.left)
							objRgn = chkRgn;
				}
				else if (keSG_PoseSorting_T2B_L2R == sortingMode)
				{
					if(true == isVOverlap)
					{
						//L2R
						if (objRgn->roi.left > chkRgn->roi.left)
							objRgn = chkRgn;
					}
					else
						if (objRgn->roi.top > chkRgn->roi.top)
							objRgn = chkRgn;
				}
				else
				{
					//不支持的排序方式
					*errCode = SG_ERR_INVLD_SORTING_MODE;
					return nullptr;
				}
			}
		}
	}
	return objRgn;
}

#define RGN_OUTER_PT		0
#define RGN_CONTOUR_PT		1
#define RGN_INNER_PT		2
#define RGN_CONTOUR_TRACKED	3

void _rgnContourTracking(cv::Mat& rgnImg, std::vector<SSG_RgnContour2D>& contours)
{
	SVzNL2DPoint scanSeq[8] = {
	{-1,-1}, {0,-1}, {1,-1}, {1,0}, {1, 1}, {0, 1}, {-1, 1}, {-1, 0} };
	//边界跟踪
	while (1)
	{
		//取种子点,选定任意一个方向
		bool foundSeed = false;
		SVzNL2DPoint seed;
		for (int i = 1; i < rgnImg.rows; i++)
		{
			for (int j = 1; j < rgnImg.cols; j++)
			{
				if (1 == rgnImg.at<uchar>(i, j))  //边界像素
				{
					int outerNum = 0;
					int innerNum = 0;
					for (int m = 0; m < 8; m++)
					{
						if (RGN_OUTER_PT == rgnImg.at<uchar>(i + scanSeq[m].y, j + scanSeq[m].x)) //外部像素计数
							outerNum++;
						else if (RGN_INNER_PT == rgnImg.at<uchar>(i + scanSeq[m].y, j + scanSeq[m].x)) //内部像素计数
							innerNum++;
					}
					if ((outerNum > 0) && (innerNum > 0))
					{
						foundSeed = true;
						seed = { j, i };
						break;
					}
				}
			}
			if (true == foundSeed)
				break;
		}
		if (false == foundSeed)
			break;

		SSG_RgnContour2D a_contour;
		a_contour.isClosed = 0;
		a_contour.contourPtList.push_back(seed);
		rgnImg.at<uchar>(seed.y, seed.x) = RGN_CONTOUR_TRACKED;  //已经被寻迹
		while (1)
		{
			//搜索 RGN_CONTOUR_PT-RGN_INNER_PT 序列。这个是搜索方向。或找到序列，由RGN_CONTOUR_PT为一个种子点
			int preState = rgnImg.at<uchar>(seed.y + scanSeq[0].y, seed.x + scanSeq[0].x);
			bool found_next = false;
			for (int m = 1; m <= 8; m++)
			{
				int idx = m % 8;
				int currState = rgnImg.at<uchar>(seed.y + scanSeq[idx].y, seed.x + scanSeq[idx].x);
				if ((RGN_CONTOUR_PT == preState) && (RGN_INNER_PT == currState))
				{
					seed = { seed.x + scanSeq[m-1].x , seed.y + scanSeq[m-1].y };
					a_contour.contourPtList.push_back(seed);
					rgnImg.at<uchar>(seed.y, seed.x) = RGN_CONTOUR_TRACKED;
					found_next = true;
					break;
				}
				else if ((RGN_CONTOUR_TRACKED == preState) && (RGN_INNER_PT == currState))
				{
					if (a_contour.contourPtList.size() > 2)
					{
						SVzNL2DPoint seed_0 = a_contour.contourPtList[0];
						SVzNL2DPoint seed_1 = a_contour.contourPtList[1];
						if ( ((seed_0.x == seed.x + scanSeq[m - 1].x) && (seed_0.y == seed.y + scanSeq[m - 1].y)) ||
							((seed_1.x == seed.x + scanSeq[m - 1].x) && (seed_1.y == seed.y + scanSeq[m - 1].y)))
						{
							a_contour.isClosed = 1;
							break;
						}
					}
				}
				preState = currState;
			}
			if (false == found_next)
				break;
		}
		contours.push_back(a_contour);
		if (1 == a_contour.isClosed)
			break;
	}
	return;
}

void _getRgnContourData(SVzNL3DLaserLine* scanData, cv::Mat& labImg, const SSG_Region* objRgn, std::vector<SSG_RgnContour3D>& contours3D)
{
	int rgnH = objRgn->roi.bottom - objRgn->roi.top + 1;
	int rgnW = objRgn->roi.right - objRgn->roi.left + 1;
	cv::Mat objImg = cv::Mat::zeros(rgnH+2, rgnW+2, CV_8UC1);  //扩大一圈，保证8连接处理时避开边界处理
	cv::Mat objInvImg = cv::Mat::ones(rgnH + 2, rgnW + 2, CV_8UC1);  //扩大一圈，保证8连接处理时避开边界处理
	for (int i = 0; i < rgnH; i++)
	{
		for (int j = 0; j < rgnW; j++)
		{
			int gi = i + objRgn->roi.top;
			int gj = j + objRgn->roi.left;
			if (objRgn->labelID == labImg.at<int>(gi, gj))
			{
				objImg.at<uchar>(i + 1, j + 1) = RGN_CONTOUR_PT;
				objInvImg.at<uchar>(i + 1, j + 1) = 0;
			}
		}
	}
	//在寻找边界前填充所有的孔洞
	//对objInvImg进行连通域标注
	cv::Mat invLabImg = cv::Mat::zeros(rgnH + 2, rgnW + 2, CV_32SC1);//rows, cols, 
	std::vector<SSG_Region> holeRgns;
	SG_TwoPassLabel(objInvImg, invLabImg, holeRgns);
	cv::Mat holeFillingImg = objImg.clone();
	for (int rgnId = 0, rgnId_max = holeRgns.size(); rgnId < rgnId_max; rgnId++)
	{
		SSG_Region* a_hole = &holeRgns[rgnId];
		if ((1 >= a_hole->roi.left) || (1 >= a_hole->roi.top) ||
			(invLabImg.rows-2 <= a_hole->roi.bottom) || (invLabImg.cols-2 <= a_hole->roi.right))
			continue;
		//填充
		for (int m = a_hole->roi.top; m <= a_hole->roi.bottom; m++)
		{
			for (int n = a_hole->roi.left; n <= a_hole->roi.right; n++)
			{
				int kkk = invLabImg.at<int>(m, n);
				if (a_hole->labelID == invLabImg.at<int>(m, n))
					holeFillingImg.at<uchar>(m, n) = RGN_CONTOUR_PT;
			}
		}
	}
#if ENABLE_OUTPUT_DEBUG_IMAGE
	cv::Mat dbg_holeFillingImg; // = bwImg.clone();
	holeFillingImg.convertTo(dbg_holeFillingImg, CV_8UC3, 255);
	cv::imwrite("top_plane_holeFilling.png", dbg_holeFillingImg);
#endif
	//边缘像素提取
	cv::Mat contourImg = holeFillingImg.clone();
	for (int i = 1; i <= rgnH; i++)
	{
		for (int j = 1; j <= rgnW; j++)
		{
			if (holeFillingImg.at<uchar>(i, j) > 0)
			{
				uchar sum = holeFillingImg.at<uchar>(i - 1, j - 1) + holeFillingImg.at<uchar>(i, j - 1) + holeFillingImg.at<uchar>(i + 1, j - 1) +
					holeFillingImg.at<uchar>(i - 1, j) + holeFillingImg.at<uchar>(i + 1, j) +
					holeFillingImg.at<uchar>(i - 1, j + 1) + holeFillingImg.at<uchar>(i, j + 1) + holeFillingImg.at<uchar>(i + 1, j + 1);
				if (sum == 8)
					contourImg.at<uchar>(i, j) = RGN_INNER_PT;  //内部像素
			}
		}
	}
	std::vector<SSG_RgnContour2D> contours;
	_rgnContourTracking(contourImg, contours);
	//生成边界点序列（3D点）
	for (int i = 0; i < contours.size(); i++)
	{
		//从栅格格式生成三维点序列
		SSG_RgnContour3D a_contour3D;
		a_contour3D.isClosed = contours[i].isClosed;
		for (int n = 0; n < contours[i].contourPtList.size(); n++)
		{
			SSG_gridPt3D a_pt;
			a_pt.gridPos = { contours[i].contourPtList[n].x - 3, contours[i].contourPtList[n].y - 3 };
			a_pt.pt3D = scanData[a_pt.gridPos.y].p3DPosition[a_pt.gridPos.x].pt3D;
			a_contour3D.contourPtList.push_back(a_pt);
		}
		contours3D.push_back(a_contour3D);
	}
#if ENABLE_OUTPUT_DEBUG_IMAGE
	cv::Mat dbg_contourImg; // = bwImg.clone();
	contourImg.convertTo(dbg_contourImg, CV_8UC3, 128);
	cv::imwrite("top_plane_contour.png", dbg_contourImg);
#endif
}

void _trackingRgnContour()
{

}

void _getTrackingCorers()
{

}

SSG_meanVar _computeMeanVar(double* data, int size, bool skipZeroData)
{
	double sum_x = 0;
	double sum_square = 0;
	int num = 0;
	for (int i = 0; i < size; i++)
	{
		if ((skipZeroData == true) && (data[i] < 1e-4))
			continue;
		sum_x += data[i];
		sum_square += data[i] * data[i];
		num++;
	}
	SSG_meanVar a_meanVar = { 0, 0 };
	if (num > 0)
	{
		a_meanVar.mean = sum_x / num;
		a_meanVar.var = sum_square / num - a_meanVar.mean * a_meanVar.mean;
		if (a_meanVar.var < 0)
			a_meanVar.var = 0;
		else
			a_meanVar.var = sqrt(a_meanVar.var);
	}
	return a_meanVar;
}

void _getLineDataMeanVar(std::vector<double>& lineData, const int meanVarWinSize, std::vector<SSG_meanVar>& meanVarData)
{
	int halfWin = meanVarWinSize / 2;
	int size = lineData.size();
	for (int i = 0; i < size; i++)
	{
		SSG_meanVar a_meanVar = { 0, 0 };
		if ((i >= halfWin) && (i < size - halfWin))
			SSG_meanVar a_meanVar = _computeMeanVar(&lineData[i - halfWin], meanVarWinSize, true);
		meanVarData.push_back(a_meanVar);
	}
	return;
}

void _getMeanVarAbnormalPt(std::vector<SSG_meanVar>& meanVarData, std::vector< SSG_meanVarAbnormalPt> abnormaPts)
{


}

void _rgnLineSegmentation(SVzNL3DLaserLine* scanData, cv::Mat& labImg, SSG_Region* objRgn)
{
	///通过均值和方差分析，判断可能的凹点
	for (int y = objRgn->roi.top; y <= objRgn->roi.bottom; y++)
	{
		std::vector<double> lineData;
		for (int x = objRgn->roi.left; x <= objRgn->roi.right; x++)
			lineData.push_back(scanData[y - 2].p3DPosition[x - 2].pt3D.z);
		//均值和方差处理，寻找凹点

	}
	//水平处理
	//垂直处理
}

/// @brief
/// 获取耐火砖抓取姿态
/// 两段式算法：第一段为扫描线处理，在接收扫描线数据后便立即处理；第二段为本处理
void CSGFireBrick::sgGetBrickPose(SVzNL3DLaserLine* scanData, int nLines, std::vector<SSG_6AxisAttitude>& brickPoses, int* errCode, SVzNL3DLaserLine* resultData)
{
	*errCode = 0;
	if (m_nFrameHeight < 10)  //扫描线过少
	{
		*errCode = SG_ERR_3D_DATA_INVLD;
		return;
	}

	//算法流程: (1)校正（2）Z方向统计 （3）取Z最小的一个峰值作为顶面（4）针对顶面进行分割
	//(1)和(2)已经在sgScanLineProc（）中完成
	std::vector<int> sumHist;
	const int sumWin = 5;
	for (int i = m_zIdxRange.nMin; i <= m_zIdxRange.nMax; i++)
	{
		//窗口
		int sumValue = 0;
		for (int j = -sumWin; j <= sumWin; j++)
		{
			int chkIdx = i + j;
			if ((chkIdx >= 0) && (chkIdx < Z_HIST_MAX_SIZE))
				sumValue += m_zHist[chkIdx];
		}
		sumHist.push_back(sumValue);
	}
	//搜索第一个峰值，作为耐火砖的最顶面
	std::vector<SSG_RunData> peaks;
	_sgSearchPeaks(sumHist, peaks);
	if (0 == peaks.size())
	{
		*errCode = SG_ERR_FOUND_NO_TOP_PLANE;
		return;
	}
	SSG_RunData* top_plane = nullptr;
	for (int i = 0, i_max = peaks.size(); i < i_max; i++)
	{
		if (peaks[i].value < M_VALID_TOP_PLANE_PTNUM)
			continue;

		top_plane = &peaks[i];
		break;
	}
	if (nullptr == top_plane)
	{
		*errCode = SG_ERR_FOUND_NO_TOP_PLANE;
		return;
	}

	//取顶面数据
	SVzNLRangeD zTopRng;
	zTopRng.min = (top_plane->start + m_zIdxRange.nMin) * m_histScale - m_planeThick /2;
	zTopRng.max = (top_plane->start + top_plane->len -1 + m_zIdxRange.nMin) * m_histScale + m_planeThick / 2;
	cv::Mat bwImg = cv::Mat::zeros(m_nFrameHeight+4, m_nFrameWidth+4, CV_8UC1);//rows, cols, 
	for (int i = 0; i < m_nFrameHeight; i++)
	{
		for (int j = 0; j < m_nFrameWidth; j++)
		{
			double z = scanData[i].p3DPosition[j].pt3D.z;
			if ((z >= zTopRng.min) && (z <= zTopRng.max))
				bwImg.at<uchar>(i+2,j+2) = 1;
		}

	}

#if ENABLE_OUTPUT_DEBUG_IMAGE
	cv::Mat grayImg; // = bwImg.clone();
	bwImg.convertTo(grayImg, CV_8UC3, 255);
	cv::imwrite("top_plane.png", grayImg);
#endif
	//
	while (1)  //采用迭代思想处理。智能的核心之一是迭代
	{
		//标注分类
		cv::Mat labImg = cv::Mat::zeros(m_nFrameHeight, m_nFrameWidth, CV_32SC1);//rows, cols, 
		std::vector<SSG_Region>labelRgns;
		SG_TwoPassLabel(bwImg, labImg, labelRgns);
		//按照排序要求取一个目标
		SSG_Region* objRgn = _getSortedObj(labelRgns, m_sortingMode, errCode);
		if (*errCode != 0)
			return;

		//进行均值方差分析，提取线段边界点，进一步分割
		_rgnLineSegmentation();



		//获取轮廓点序列，根据序列计算角点
		std::vector<SSG_RgnContour3D> contours3D;
		_getRgnContourData(scanData, labImg, objRgn, contours3D);
#if ENABLE_OUTPUT_DEBUG_IMAGE
		//输出边界3D点

#endif
		_trackingRgnContour();
		//检查角点，确定是否要细分

	}


}

void CSGFireBrick::sgSortBrickPoses(std::vector<SSG_6AxisAttitude>& brickPoses, ESG_poseSortingMode sortingMode)
{

}

/// @brief
/// 初始化
bool CSGFireBrick::_Init(double dHistScale)
{
	m_histScale = dHistScale;
	return true;
}

bool SGCreateFireBrick(double dHistScale, ISGFireBrick** ppFireBrick)
{
	return CSGFireBrick::CreateInstance(dHistScale, ppFireBrick);
}