#include "eyemEdge1d.h"

static void findPeak(std::vector<double> vec, std::vector<int>& peaks)
{
	std::vector<int> sign;
	for (int i = 1; i < vec.size(); i++)
	{
		auto diff = vec[i] - vec[i - 1];
		if (diff < .0)
		{
			sign.push_back(-1);
		}
		else if (diff > .0)
		{
			sign.push_back(1);
		}
		else
		{
			sign.push_back(0);
		}
	}
	for (int j = 1; j < sign.size(); j++)
	{
		int diff = sign[j] - sign[j - 1];
		if (diff != 0)
		{
			peaks.push_back(j);
		}
	}
}

static cv::Mat projectMap(const cv::Mat map, int threshold)
{
	double height = .0, width = map.cols;

	cv::Mat temp;
	cv::normalize(map, temp, 0, 255, cv::NORM_MINMAX);
	cv::minMaxLoc(temp, NULL, &height, NULL, NULL);

	cv::Mat image = cv::Mat::zeros((int)height, (int)width, CV_8UC3);
	image.setTo(cv::Scalar(100, 0, 0));

	std::vector<cv::Point> rejectPoint;
	for (int i = 0; i < temp.cols; i++)
	{
		rejectPoint.push_back(cv::Point(i, (int)(height - temp.at<double>(cv::Point(i, 0)))));
	}

	cv::polylines(image, rejectPoint, false, cv::Scalar(0, 255, 0), 1, 8, 0);

	cv::line(image, cv::Point(0, (int)MAX(height - threshold, 0)), cv::Point((int)width, (int)MAX(height - threshold, 0)), cv::Scalar(0, 255, 255), 1, 8);

	return image;
}

static void interp2(cv::Mat& src, cv::Mat& dst, cv::Mat& map1, cv::Mat& map2)
{

}

int	eyemEdge1dGenMeasureRect(EyemImage tpImage, EyemOcsDXY tpLineSt, EyemOcsDXY tpLineEd, int iWhRoi, const char *ccSubType, int iTransition, double dSigma, double dAmpThresh, IntPtr *hObject)
{
	cv::Mat image(tpImage.iHeight, tpImage.iWidth, CV_8UC1, tpImage.vpImage);

	if (image.empty()) {
		return FUNC_IMAGE_NOT_EXIST;
	}

	if (tpLineSt.dX < 0 || tpLineSt.dY < 0 || tpLineSt.dX>tpImage.iWidth || tpLineSt.dY>tpImage.iHeight\
		|| tpLineEd.dX<0 || tpLineEd.dY<0 || tpLineEd.dX>tpImage.iWidth || tpLineEd.dY>tpImage.iHeight)
		return FUNC_ILLEGAL_ARGUMENT;

	//主轴倾斜角
	double t = .0;

	t = atan2(tpLineEd.dY - tpLineSt.dY, tpLineEd.dX - tpLineSt.dX) * 180. / PI;

	double rotWeight = tpImage.iHeight * abs(sin(t * CV_PI / 180)) + tpImage.iWidth * abs(cos(t * CV_PI / 180));

	double rotHeight = ceil(tpImage.iWidth * abs(sin(t * CV_PI / 180)) + tpImage.iHeight * abs(cos(t * CV_PI / 180)));

	cv::RotatedRect rotRect(cv::Point2f((float)(tpLineSt.dX + tpLineEd.dX) / 2.0f, (float)(tpLineSt.dY + tpLineEd.dY) / 2.0f), \
		cv::Size2f((float)std::sqrt(std::pow(tpLineSt.dX - tpLineEd.dX, 2) + (float)std::pow(tpLineSt.dY - tpLineEd.dY, 2)), (float)iWhRoi), (float)t);

	cv::Mat rotM2d(2, 3, CV_64F);
	rotM2d = cv::getRotationMatrix2D(cv::Point2f(tpImage.iWidth / 2.0f - 0.5f, tpImage.iHeight / 2.0f - 0.5f), t, 1.0);

#define COEFF_AT(x,y) ((double *)rotM2d.data)[(y)*rotM2d.cols + (x)]

	COEFF_AT(2, 0) += (rotWeight - tpImage.iWidth) / 2.0;
	COEFF_AT(2, 1) += (rotHeight - tpImage.iHeight) / 2.0;

	cv::Point2d rotedCenter;
	rotedCenter.x = rotRect.center.x*COEFF_AT(0, 0) + rotRect.center.y*COEFF_AT(1, 0) + COEFF_AT(2, 0);
	rotedCenter.y = rotRect.center.x* COEFF_AT(0, 1) + rotRect.center.y*COEFF_AT(1, 1) + COEFF_AT(2, 1);

	cv::Mat rotImg;
	cv::warpAffine(image, rotImg, rotM2d, cv::Size((int)rotWeight, (int)rotHeight), cv::INTER_LINEAR);

	//获取Roi区域
	cv::Rect roi((int)MAX(rotedCenter.x - rotRect.size.width / 2, 0), (int)MAX(rotedCenter.y - rotRect.size.height / 2, 0), \
		(int)rotRect.size.width, (int)rotRect.size.height);

	cv::Mat filter, diffMat, one;

	diffMat = /*convert(rotImg(roi), CV_64F)*/cv::Mat();

	//计算投影
	cv::reduce(diffMat, one, 0, cv::REDUCE_AVG, CV_64F);

#ifdef _DEBUG
	cv::Mat map1 = projectMap(one, 30);
#endif

	//高斯滤波
	cv::Mat kernel = cv::getGaussianKernel(5, dSigma).t();

	cv::sepFilter2D(one, one, diffMat.depth(), kernel, cv::Mat::ones(1, 1, CV_64F));

#ifdef _DEBUG
	cv::Mat map2 = projectMap(one, 30);
#endif

	//默认过滤一半像素
	const cv::Mat whalf = (cv::Mat_<double>(1, 5) << -1, -1, 0, 1, 1);

	cv::sepFilter2D(one, filter, diffMat.depth(), whalf, cv::Mat::ones(1, 1, CV_64F));

#ifdef _DEBUG
	cv::Mat map3 = projectMap(filter, 130);
#endif

	std::vector<double> v_filter = filter.reshape(0, 1);

	std::vector<int> peeks;
	findPeak(v_filter, peeks);

	std::vector<EyemOcsDXY> *tpEdges = new std::vector<EyemOcsDXY>();
	EyemOcsDXY tpEdge;
	for (int i = 0; i < (int)peeks.size(); i++)
	{
		if (abs(v_filter[peeks[i]]) > dAmpThresh)
		{
			double a, b, c;
			a = v_filter[MAX(0, peeks[i] - 1)];
			b = v_filter[peeks[i]];
			c = v_filter[MIN((int)v_filter.size() - 1, peeks[i] + 1)];

			double offset = 0.5 * (a - c) / (a - b - b + c);

			if (abs(offset) <= 0.5)
			{
				double x = peeks[i] + rotedCenter.x - rotRect.size.width / 2 + offset + 0.5;
				double y = iWhRoi / 2 + rotedCenter.y - rotRect.size.height / 2 + 0.5;

				double a1, b1, c1, a2, b2, c2;
				a1 = COEFF_AT(0, 0); b1 = COEFF_AT(1, 0); c1 = x - COEFF_AT(2, 0);
				a2 = COEFF_AT(0, 1); b2 = COEFF_AT(1, 1); c2 = y - COEFF_AT(2, 1);

				tpEdge.dX = (c1*b2 - c2*b1) / (a1*b2 - a2*b1);
				tpEdge.dY = (c1*a2 - c2*a1) / (b1*a2 - b2*a1);

				//all
				if (iTransition == 0)
				{
					tpEdges->push_back(tpEdge);
				}
				else if (iTransition == 1)
				{
					//positive
					if (b < 0)
					{
						tpEdges->push_back(tpEdge);
					}
				}
				else if (iTransition == -1)
				{
					//negative
					if (b > 0)
					{
						tpEdges->push_back(tpEdge);
					}
				}
			}
		}
	}

	if (strcmp(ccSubType, "first") == 0)
	{
		tpEdge = tpEdges->front();
		tpEdges->clear();
		tpEdges->push_back(tpEdge);
	}
	else if (strcmp(ccSubType, "last") == 0)
	{
		tpEdge = tpEdges->back();
		tpEdges->clear();
		tpEdges->push_back(tpEdge);
	}

#ifdef _DEBUG
	std::cout << "Test 'eyemEdge1dGenRect' " << std::endl;

	cv::Mat showMat, showMat2;
	cv::cvtColor(image, showMat, cv::COLOR_GRAY2BGR);

	cv::Point2f rect[4];
	rotRect.points(rect);
	for (int j = 0; j < 4; j++)
	{
		cv::line(showMat, rect[j], rect[(j + 1) % 4], cv::Scalar(0, 255, 0), 1);
	}

	//获取ROI区域
	cv::cvtColor(rotImg.clone(), showMat2, cv::COLOR_GRAY2BGR);
	cv::rectangle(showMat2, roi, cv::Scalar(0, 255, 0), 1);

	cv::line(showMat, cv::Point((int)tpLineSt.dX, (int)tpLineSt.dY), cv::Point((int)tpLineEd.dX, (int)tpLineEd.dY), cv::Scalar(0, 255, 0), 1);

	for (int i = 0; i < tpEdges->size(); i++)
	{

		double _angle = (t + 90.)*CV_PI / 180.;
		float b = (float)cos(_angle)*0.5f;
		float a = (float)sin(_angle)*0.5f;

		cv::Point2f center((float)tpEdges->at(i).dX, (float)tpEdges->at(i).dY);

		cv::Point start((int)(center.x - b*iWhRoi + 0.5), (int)(center.y - a*iWhRoi + 0.5));

		cv::Point end((int)(center.x + b*iWhRoi + 0.5), (int)(center.y + a*iWhRoi + 0.5));

		cv::line(showMat, start, end, cv::Scalar(0, 0, 255), 1);
	}
#endif
	*hObject = reinterpret_cast<IntPtr>(tpEdges);
	return FUNC_OK;
}

int eyemEdge1dGenPosRect(EyemImage tpImage, EyemOcsDXY tpLineSt, EyemOcsDXY tpLineEd, int iWhRoi, int iTransition, double dSigma, double dAmpThresh, IntPtr *hObject)
{
	cv::Mat image(tpImage.iHeight, tpImage.iWidth, CV_8UC1, tpImage.vpImage);

	if (image.empty()) {
		return FUNC_IMAGE_NOT_EXIST;
	}

	if (tpLineSt.dX < 0 || tpLineSt.dY < 0 || tpLineSt.dX>tpImage.iWidth || tpLineSt.dY>tpImage.iHeight\
		|| tpLineEd.dX<0 || tpLineEd.dY<0 || tpLineEd.dX>tpImage.iWidth || tpLineEd.dY>tpImage.iHeight)
		return FUNC_ILLEGAL_ARGUMENT;

	//主轴倾角
	double t = .0;

	t = atan2(tpLineEd.dY - tpLineSt.dY, tpLineEd.dX - tpLineSt.dX) * 180. / PI;

	double rotWeight = tpImage.iHeight * abs(sin(t * CV_PI / 180)) + tpImage.iWidth * abs(cos(t * CV_PI / 180));

	double rotHeight = ceil(tpImage.iWidth * abs(sin(t * CV_PI / 180)) + tpImage.iHeight * abs(cos(t * CV_PI / 180)));

	cv::RotatedRect rotRect(cv::Point2f((float)(tpLineSt.dX + tpLineEd.dX) / 2.0f, (float)(tpLineSt.dY + tpLineEd.dY) / 2.0f), \
		cv::Size2f((float)std::sqrt(std::pow(tpLineSt.dX - tpLineEd.dX, 2) + (float)std::pow(tpLineSt.dY - tpLineEd.dY, 2)), (float)iWhRoi), (float)t);

	cv::Mat rotM2d(2, 3, CV_64F);
	rotM2d = cv::getRotationMatrix2D(cv::Point2f(tpImage.iWidth / 2.0f - 0.5f, tpImage.iHeight / 2.0f - 0.5f), t, 1.0);

#define COEFF_AT(x,y) ((double *)rotM2d.data)[(y)*rotM2d.cols + (x)]

	COEFF_AT(2, 0) += (rotWeight - tpImage.iWidth) / 2.0;
	COEFF_AT(2, 1) += (rotHeight - tpImage.iHeight) / 2.0;

	cv::Point2d rotedCenter;
	rotedCenter.x = rotRect.center.x*COEFF_AT(0, 0) + rotRect.center.y*COEFF_AT(1, 0) + COEFF_AT(2, 0);
	rotedCenter.y = rotRect.center.x* COEFF_AT(0, 1) + rotRect.center.y*COEFF_AT(1, 1) + COEFF_AT(2, 1);

	cv::Mat rotImg;
	cv::warpAffine(image, rotImg, rotM2d, cv::Size((int)rotWeight, (int)rotHeight), cv::INTER_LINEAR);

	//获取Roi区域
	cv::Rect roi((int)MAX(rotedCenter.x - rotRect.size.width / 2, 0), (int)MAX(rotedCenter.y - rotRect.size.height / 2, 0), \
		(int)rotRect.size.width, (int)rotRect.size.height);

	cv::Mat F, G;

	//TODO:增加处理接口
	F = /*convert(rotImg(roi), CV_64F)*/cv::Mat();

	//高斯滤波
	cv::GaussianBlur(F, G, cv::Size(3, 3), dSigma, dSigma);

	//偏导
	cv::Mat dx, dy;
	spatialGradient(G, dx, dy);

	//梯度幅值
	cv::Mat mag;
	cv::magnitude(dx, dy, mag);

#define FELEM_AT(x,y) ((double *)F.data)[(y)*F.cols + (x)]
#define FXELEM_AT(x,y) ((double *)dx.data)[(y)*dx.cols + (x)]
#define FYELEM_AT(x,y) ((double *)dy.data)[(y)*dy.cols + (x)]
#define MAGELEM_AT(x,y) ((double *)mag.data)[(y)*mag.cols + (x)]

	//提取边缘
	std::vector<cv::Point> edgePixel;
	cv::parallel_for_(cv::Range(5, F.rows - 4), [&](const cv::Range& range) -> void {
		for (int r = range.start; r < range.end; r++)
		{
			for (int c = 5; c < mag.cols - 4; c++)
			{
				if (MAGELEM_AT(c, r) > dAmpThresh)
				{
					if (abs(FYELEM_AT(c, r)) >= abs(FXELEM_AT(c, r)))
					{
						if (abs(FYELEM_AT(c, r)) >= abs(FYELEM_AT(c, r - 1))\
							&&abs(FYELEM_AT(c, r)) >= abs(FYELEM_AT(c, r + 1)))
						{
							edgePixel.push_back(cv::Point(c, r));
						}
					}
					else
					{
						if (abs(FXELEM_AT(c, r)) >= abs(FXELEM_AT(c - 1, r))\
							&&abs(FXELEM_AT(c, r)) >= abs(FXELEM_AT(c + 1, r)))
						{
							edgePixel.push_back(cv::Point(c, r));
						}
					}
				}
			}
		}
	});


	cv::Mat showMat3;
	cv::cvtColor(/*convert(F, CV_8U)*/cv::Mat(), showMat3, cv::COLOR_GRAY2BGR);

	EyemOcsDXY tpEdge;
	std::vector<EyemOcsDXY> *tpEdges = new std::vector<EyemOcsDXY>();

	for (int i = 0; i < (int)edgePixel.size(); i++)
	{
		double x = edgePixel[i].x + rotedCenter.x - rotRect.size.width / 2 + 0.5;
		double y = edgePixel[i].y + rotedCenter.y - rotRect.size.height / 2 + 0.5;

		double a1, b1, c1, a2, b2, c2;
		a1 = COEFF_AT(0, 0); b1 = COEFF_AT(1, 0); c1 = x - COEFF_AT(2, 0);
		a2 = COEFF_AT(0, 1); b2 = COEFF_AT(1, 1); c2 = y - COEFF_AT(2, 1);

		tpEdge.dX = (c1*b2 - c2*b1) / (a1*b2 - a2*b1);
		tpEdge.dY = (c1*a2 - c2*a1) / (b1*a2 - b2*a1);

		tpEdges->push_back(tpEdge);
	}


#ifdef _DEBUG
	std::cout << "Test 'eyemEdge1dGenRect' " << std::endl;

	cv::Mat showMat, showMat2;
	cv::cvtColor(image, showMat, cv::COLOR_GRAY2BGR);

	cv::Point2f rect[4];
	rotRect.points(rect);
	for (int j = 0; j < 4; j++)
	{
		cv::line(showMat, rect[j], rect[(j + 1) % 4], cv::Scalar(0, 255, 0), 1);
	}

	//获取ROI区域
	cv::cvtColor(rotImg.clone(), showMat2, cv::COLOR_GRAY2BGR);
	cv::rectangle(showMat2, roi, cv::Scalar(0, 255, 0), 1);

	cv::line(showMat, cv::Point((int)tpLineSt.dX, (int)tpLineSt.dY), cv::Point((int)tpLineEd.dX, (int)tpLineEd.dY), cv::Scalar(0, 255, 0), 1);

	//for (int i = 0; i < edges.size(); i++)
	//{
	//	showMat.at<cv::Vec3b>(cv::Point(edges[i].dX, edges[i].dY)) = cv::Vec3b(0, 0, 255);
	//	//cv::line(showMat, start, end, cv::Scalar(0, 0, 255), 1);
	//}
#endif
	*hObject = reinterpret_cast<IntPtr>(tpEdges);
	return FUNC_OK;
}

int eyemEdge1dFitLine(IntPtr hObject, EyemOcsDABC *tpLine)
{
	//根据导数符号确定由暗到明和由明到暗方向，执行圆拟合与直线拟合
	//std::vector<EyemOcsDXY>  *tpEdges = reinterpret_cast<std::vector<EyemOcsDXY>*>(hObject);
	return FUNC_OK;
}

int eyemEdge1dFindLine(EyemImage tpImage, EyemOcsDXY tpLineSt, EyemOcsDXY tpLineEd, int iWhRoi, int iTransition, IntPtr *hObject)
{
	cv::Mat image(tpImage.iHeight, tpImage.iWidth, CV_8UC1, tpImage.vpImage);

	if (image.empty()) {
		return FUNC_IMAGE_NOT_EXIST;
	}

	if (tpLineSt.dX < 0 || tpLineSt.dY < 0 || tpLineSt.dX>tpImage.iWidth || tpLineSt.dY>tpImage.iHeight\
		|| tpLineEd.dX<0 || tpLineEd.dY<0 || tpLineEd.dX>tpImage.iWidth || tpLineEd.dY>tpImage.iHeight)
		return FUNC_ILLEGAL_ARGUMENT;

	//主轴倾斜角
	double t = .0, a = .0, b = .0, c = .0;
	t = atan2(-tpLineEd.dX + tpLineSt.dX, tpLineEd.dY - tpLineSt.dY) /** 180. / PI*/;

	//主轴直线方程ax+by+c=0
	a = -(tpLineSt.dY - tpLineEd.dY);
	b = (tpLineSt.dX - tpLineEd.dX);
	c = tpLineEd.dX*tpLineSt.dY - tpLineSt.dX*tpLineEd.dY;

	double x = (tpLineSt.dX + tpLineEd.dX) / 2;
	double y = -(a*x + c) / b;

	double *dpWhalf = new double[2 * iWhRoi + 1];
	for (int i = -iWhRoi, j = 0; i <= iWhRoi, j < 2 * iWhRoi + 1; i++, j++)
	{
		dpWhalf[j] = i;
	}

	cv::Mat whalf(1, 2 * iWhRoi + 1, CV_64F, dpWhalf);
	cv::Mat map1, map2;
	//沿法线方向采样
	map1 = whalf*cos(t);
	map2 = whalf*sin(t);
	map1 += cv::Mat::ones(1, 31, CV_64F)*x;
	map2 += cv::Mat::ones(1, 31, CV_64F)*y;

	cv::Mat dst;
	interp2(image, dst, map1, map2);


	cv::Mat cc;
	cv::cvtColor(image, cc, cv::COLOR_GRAY2BGR);
	for (int i = 0; i < 31; i++)
	{
		cc.at<cv::Vec3b>(cv::Point(cvRound(map1.at<double>(0, i)), cvRound(map2.at<double>(0, i)))) = cv::Vec3b(0, 255, 0);
	}

	return FUNC_OK;
}

int eyemEdge1dFitCircle(IntPtr hObject, int iClippingEndPoints, int iMaxIterations, double dRobustCoef, EyemOcsDXYR *tpCircle)
{

	return FUNC_OK;
}

int eyemEdge1dGenArc(EyemImage tpImage, EyemOcsDXY tpLineSt, EyemOcsDXY tpLineEd, int iWhRoi, int iEdgeDirec, EyemOcsDXY *tpEdge)
{

	return FUNC_OK;
}

bool eyemEdge1dGenMeasureFree(IntPtr hObject)
{
	std::vector<EyemOcsDXY>  *tpEdges = reinterpret_cast<std::vector<EyemOcsDXY>*>(hObject);
	delete tpEdges;
	tpEdges = NULL;
	return true;
}