#include "eyemEdge1d.h"

static void findPeak(int iSize, float projectMap[], float fAmpThreshold, std::vector<int>& peaks)
{
	std::vector<int> sign;
	for (int i = 1; i < iSize; i++) {
		auto diff = projectMap[i] - projectMap[i - 1];
		if (diff < 0.0f) {
			sign.push_back(-1);
		}
		else if (diff > 0.0f) {
			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 && abs(projectMap[j]) > fAmpThreshold) {
			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<float>(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;
}

void  getSuperResolution(const cv::Mat &src, cv::Mat &dst, int size)
{
	dst = cv::Mat::zeros(src.size()*size, src.type());
	for (int i = 0; i < src.cols; i++)
	{
		for (int j = 0; j < src.rows; j++)
		{
			cv::Mat m(cv::Size(size, size), src.type(), cv::Scalar::all(src.at<uchar>(j, i)));
			m.copyTo(dst(cv::Rect(i*size, j*size, size, size)));
		}
	}
}

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 eyemEdge1dFindLine(EyemImage tpImage, EyemOcsDXY tpLineSt, EyemOcsDXY tpLineEd, int iCapLength, int iCapWidth, int nCalipers, int nFilterSize, int iSearchDirec, double dAmpThreshold, const char *ccTransition, IntPtr *hObject)
{
	cv::Mat image = cv::Mat(tpImage.iHeight, tpImage.iWidth, MAKETYPE(tpImage.iDepth, tpImage.iChannels), tpImage.vpImage).clone();
	if (image.empty()) {
		return FUNC_IMAGE_NOT_EXIST;
	}
	const int X = image.cols, Y = image.rows;
	//显示用
	cv::Mat cc;
	cv::cvtColor(image, cc, cv::COLOR_GRAY2BGR);
	//判断越界
	if (tpLineSt.dX < 0 || tpLineSt.dY < 0 || tpLineSt.dX>X || tpLineSt.dY>Y || tpLineEd.dX<0 || tpLineEd.dY<0 || tpLineEd.dX>X || tpLineEd.dY>Y) {
		return FUNC_ILLEGAL_ARGUMENT;
	}
	//主轴倾角
	double t;
	t = atan2(tpLineEd.dY - tpLineSt.dY, tpLineEd.dX - tpLineSt.dX);
	//直线上的坐标
	float L = (float)cv::norm(cv::Point2d(tpLineSt.dX, tpLineSt.dY) - cv::Point2d(tpLineEd.dX, tpLineEd.dY));
	//步长
	float plusStep = (L - (float)nCalipers*(float)iCapWidth) / ((float)nCalipers + 1.0f);
	//绘制profileLine
	//drawArrowedLine("", tpLineSt, tpLineEd, cv::Scalar(255, 153, 0), 2);
	//判断极性
	bool anyPolarity = strcmp("all", ccTransition) == 0;
	//默认过滤一半像素
	float *filterK = new float[2 * nFilterSize + 1]();
	//定义滤波核
	for (int n = 0; n < nFilterSize; n++) {
		filterK[n] = 1;
		filterK[2 * nFilterSize - n] = -1;
	}
	cv::Mat whalf(cv::Size(2 * nFilterSize + 1, 1), CV_32FC1, filterK);
	//线采样,采用双三次插值
	cv::Size szMap(2 * iCapLength + 1, iCapWidth + 1);
	//结果
	std::vector<EyemOcsDXY> edgePoint;
	for (int n = 1; n <= nCalipers; n++)
	{
		float *pMag = new float[szMap.width*szMap.height * sizeof(float_t)];
		for (int m = 0; m <= iCapWidth; m++)
		{
			float plusX, plusY;
			plusX = ((float)n*(plusStep + (float)iCapWidth) - (float)iCapWidth + m) * (float)cos(t);
			plusY = ((float)n*(plusStep + (float)iCapWidth) - (float)iCapWidth + m) * (float)sin(t);
			//中轴线路径上的点
			cv::Point2f pLine((float)tpLineSt.dX + plusX, (float)tpLineSt.dY + plusY);
			//drawPoint("", pLine, cv::Scalar(0, 0, 255), 4);
			for (int iR = -iCapLength; iR <= iCapLength; iR++) {
				//待插值坐标
				float _plusX, _plusY;
				_plusX = (float)iR*(float)cos(t + iSearchDirec*CV_PI / 2.0) + pLine.x;
				_plusY = (float)iR*(float)sin(t + iSearchDirec*CV_PI / 2.0) + pLine.y;
				//防止越界
				if (_plusX < 1 || _plusX >= X - 2 || _plusY < 1 || _plusY >= Y - 2) {
					pMag[(iCapLength + iR) + m*szMap.width] = -1;
					continue;
				}
				//drawPoint("", cv::Point2f(_plusX, _plusY), cv::Scalar(36, 127, 255), 1);
				//画像素
				float bb = (float)cos(t)*0.5f;
				float aa = (float)sin(t)*0.5f;

				cv::Point2f pt(_plusX, _plusY);
				cv::Point2f pts[4];
				pts[0].x = (float)(pt.x - aa - bb);
				pts[0].y = (float)(pt.y + bb - aa);
				pts[1].x = (float)(pt.x + aa - bb);
				pts[1].y = (float)(pt.y - bb - aa);
				pts[2].x = (float)(2 * pt.x - pts[0].x);
				pts[2].y = (float)(2 * pt.y - pts[0].y);
				pts[3].x = (float)(2 * pt.x - pts[1].x);
				pts[3].y = (float)(2 * pt.y - pts[1].y);
				//for (int j = 0; j < 4; j++)
				//{
				//	drawLine("", pts[j], pts[(j + 1) % 4], cv::Scalar(255, 153, 0));
				//}
				//整数部分
				int x = cvRound(_plusX), y = cvRound(_plusY);
				//小数部分
				float u = abs(_plusX - ((float)x + 0.5f));
				float v = abs(_plusY - ((float)y - 1.0f + 0.5f));
				//插值计算灰度值
				float gv = (1.0f - v)*(image.ptr<uint8_t>(y - 1)[x] * (1.0f - u) + image.ptr<uint8_t>(y - 1)[x - 1] * u)
					+ v*(image.ptr<uint8_t>(y)[x] * (1.0f - u) + image.ptr<uint8_t>(y)[x - 1] * u);
				//填入灰度值
				pMag[(iCapLength + iR) + m*szMap.width] = gv;
			}
		}
		//采样位置
		cv::Point2f midLine((float)tpLineSt.dX + ((float)n*(plusStep + (float)iCapWidth) - (float)iCapWidth + (float)iCapWidth / 2.0f) * (float)cos(t),
			(float)tpLineSt.dY + ((float)n*(plusStep + (float)iCapWidth) - (float)iCapWidth + (float)iCapWidth / 2.0f) * (float)sin(t));
		//各位置采样路径
		cv::Point2f midLineStart, midLineEnd;
		midLineStart = cv::Point2f(-iCapLength*(float)cos(t + iSearchDirec*CV_PI / 2.0) + midLine.x, -iCapLength*(float)sin(t + iSearchDirec*CV_PI / 2.0) + midLine.y);
		midLineEnd = cv::Point2f(iCapLength*(float)cos(t + iSearchDirec* CV_PI / 2) + midLine.x, iCapLength*(float)sin(t + iSearchDirec*CV_PI / 2.0) + midLine.y);
		//采样图像
		cv::Mat interMap(szMap, CV_32FC1, pMag);
		//计算投影
		cv::Mat projectedMap;
		cv::reduce(interMap, projectedMap, 0, cv::REDUCE_AVG, CV_32F);
		//差分过滤(TODO:加高斯滤波)
		float *pFilteredMap = new float[szMap.width * sizeof(float_t)];
		cv::Mat filteredMap(cv::Size(szMap.width, 1), CV_32FC1, pFilteredMap);
		cv::sepFilter2D(projectedMap, filteredMap, CV_32F, whalf, cv::Mat::ones(1, 1, CV_32F));
		//投影峰值查找
		std::vector<int> peeks;
		findPeak(szMap.width, pFilteredMap, (float)dAmpThreshold, peeks);
		//存在满足幅度值的边缘
		if (!peeks.empty()) {
			float dist = 0; bool found = false;
			//判断灰度值过滤类型
			if (anyPolarity) {
				//不分极性
				float maxDist = 0; int maxPos = 0;
				for (auto&peek : peeks) {
					if (abs(pFilteredMap[peek]) > maxDist) {
						maxDist = abs(pFilteredMap[peek]);
						maxPos = peek;
					}
				}
				found = true;
				dist = (float)maxPos;
			}
			else if (strcmp("positive", ccTransition) == 0) {
				int maxPos = 0;
				for (auto&peek : peeks) {
					if (pFilteredMap[peek] > 0) {
						maxPos = peek;
						found = true;
						break;
					}
				}
				dist = (float)maxPos;
			}
			else if (strcmp("negative", ccTransition) == 0) {
				int maxPos = 0;
				for (auto&peek : peeks) {
					if (pFilteredMap[peek] < 0) {
						maxPos = peek;
						found = true;
						break;
					}
				}
				dist = (float)maxPos;
			}
			//阈值限制,计算亚像素坐标
			if (found) {
				int l, m, r; float a, b, c, u;
				m = (int)dist;
				l = m - 1 < 0 ? m : m - 1;
				r = m + 1 >= szMap.width ? m : m + 1;

				a = pFilteredMap[l]; b = pFilteredMap[m];
				c = pFilteredMap[r];
				u = 0.5f*(a - c) / (a - b - b + c);

				//定位结果
				float dstX, dstY;
				dstX = midLineStart.x + (dist + u)*(float)cos(t + iSearchDirec* CV_PI / 2.0);
				dstY = midLineStart.y + (dist + u)*(float)sin(t + iSearchDirec* CV_PI / 2.0);
				//drawPoint("", cv::Point2f(dstX, dstY), cv::Scalar(0, 0, 255), 2);
				//edgePoint.push_back(EdgePoint(0, dstX, dstY, 0, 0, true));
			}
		}
		//作图显示用
		cv::Mat cc;
		cv::cvtColor(image, cc, cv::COLOR_GRAY2BGR);
		//drawArrowedLine("", midLineStart, midLineEnd, cv::Scalar(0, 255, 0), 2);
		cv::arrowedLine(cc, midLineStart, midLineEnd, cv::Scalar(0, 255, 0), 1);
		//画卡尺
		float bb = (float)cos(t)*0.5f;
		float aa = (float)sin(t)*0.5f;
		cv::Point2f pt(midLine.x, midLine.y);
		cv::Point2f pts[4];
		pts[0].x = (float)(pt.x - aa * szMap.width - bb * szMap.height);
		pts[0].y = (float)(pt.y + bb * szMap.width - aa * szMap.height);
		pts[1].x = (float)(pt.x + aa * szMap.width - bb * szMap.height);
		pts[1].y = (float)(pt.y - bb * szMap.width - aa * szMap.height);
		pts[2].x = (float)(2 * pt.x - pts[0].x);
		pts[2].y = (float)(2 * pt.y - pts[0].y);
		pts[3].x = (float)(2 * pt.x - pts[1].x);
		pts[3].y = (float)(2 * pt.y - pts[1].y);

		for (int j = 0; j < 4; j++)
		{
			//drawLine("", pts[j], pts[(j + 1) % 4], cv::Scalar(255, 153, 0), 2);
		}
		//释放资源
		delete[] pMag;
		pMag = NULL;
	}
	if (!edgePoint.empty()) {
		//拟合直线
		//double k, b, rms;
		//findLine(edgePoint, 1, k, b, rms);
		////计算交点
		//cv::Point2f it;
		//eyemClp2dIntersectionLineSegment(k, -1, b, 43, 25, 231, 25, it);
		//画直线y=kx+b--->kx-y+b=0
		//(0,P1)、(X,P2)、(P3,0)、(P4,Y)
		cv::Point2f p1, p2;
		//eyemClp2dIntersectionOfLineRectangle(k, -1, b, 0, 0, X, 0, X, Y, p1, p2);

		//drawLine("", p1, p2, cv::Scalar(0, 255, 0), 1);
	}
	//cv::Mat dst;
	//getSuperResolution(image, dst, 10);
	//cv::cvtColor(dst, dst, cv::COLOR_GRAY2BGR);
	//for (int i = 0; i < dstPts.size(); i++)
	//{
	//	cv::circle(dst, cv::Point(int(round(dstPts[i].x * 10)), int(round(dstPts[i].y * 10))), 1, cv::Scalar(0, 255, 0), -1);
	//}
	////释放内存(Tips:当存在越界时候在用free释放时会报错)
	//free((void *)pMap);
	//释放资源
	delete[] filterK;
	filterK = NULL;
	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;
}