#include "eyemMatchShapes.h"

shape_based_matching::~shape_based_matching(void) {}

int shape_based_matching::create_shape_model(cv::Mat tpTemplate, double maxContrast, double minContrast, double apertureSize, bool L2gradient)
{
	int incn = tpTemplate.channels();
	if (incn != 1) {
		cv::cvtColor(tpTemplate, tpTemplate, cv::COLOR_BGR2GRAY);
	}
	//获取边缘
	cv::Mat edges;
	cv::Canny(tpTemplate, edges, minContrast, maxContrast, (int)apertureSize, L2gradient);

	auto contours = std::vector<std::vector<cv::Point>>();
	cv::findContours(edges, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

	//计算偏导
	cv::Mat gx, gy;
	cv::Sobel(tpTemplate, gx, CV_64F, 1, 0, 3);
	cv::Sobel(tpTemplate, gy, CV_64F, 0, 1, 3);

	//计算梯度以及方向
	cv::Mat magnitude, direction;
	cv::cartToPolar(gx, gy, magnitude, direction);

	auto sum = cv::Point2d(0, 0);
	for (int i = 0; i < contours.size(); i++)
	{
		for (int j = 0; j < contours[i].size(); j++)
		{
			auto cur = contours[i][j];
			auto fdx = gx.ptr<double>(cur.y)[cur.x];
			auto fdy = gy.ptr<double>(cur.y)[cur.x];
			auto der = cv::Point2d(fdx, fdy);
			auto mag = magnitude.ptr<double>(cur.y)[cur.x];
			auto dir = direction.ptr<double>(cur.y)[cur.x];
			results.push_back(PointInfo(cur, der, mag, dir));
			sum.x += cur.x; sum.y += cur.y;
		}
	}
	//模板尺寸
	templSize = cv::Size(tpTemplate.cols, tpTemplate.rows);
	//中心
	auto Center = cv::Point2d(sum.x / results.size(), sum.y / results.size());
	//更新位置
	for (auto&item : results) {
		item.update(Center);
	}
	return FUNC_OK;
}

double shape_based_matching::find_shape_model(cv::Mat tpImage, double minScore, double greediness, cv::Point& resultPoint)
{
	int incn = tpImage.channels();
	if (incn != 1) {
		cv::cvtColor(tpImage, tpImage, cv::COLOR_BGR2GRAY);
	}
	//尺寸信息
	const int X = tpImage.cols, Y = tpImage.rows;

	//用于显示
	cv::Mat showMat;
	cv::cvtColor(tpImage, showMat, cv::COLOR_GRAY2BGR);

	//计算偏导
	cv::Mat gx, gy;
	cv::Sobel(tpImage, gx, CV_64F, 1, 0, 3);
	cv::Sobel(tpImage, gy, CV_64F, 0, 1, 3);

	//计算梯度以及方向
	cv::Mat magnitude, direction;
	cv::cartToPolar(gx, gy, magnitude, direction);

	cv::Mat scoreMap(cv::Size(tpImage.cols, tpImage.rows), CV_64FC1, cv::Scalar(0));

	//基于ncc匹配
	long noOfCordinates = static_cast<int>(results.size());
	double normMinScore = minScore / noOfCordinates;
	double normGreediness = (1 - greediness*minScore) / ((1.0 - greediness) / noOfCordinates);
	double partialScore = .0, resultScore = .0;
	for (int i = 0; i < tpImage.rows; i++) {
		for (int j = 0; j < tpImage.cols; j++) {
			double partialSum = .0;
			for (int m = 0; m < noOfCordinates; m++)
			{
				auto item = results[m];
				auto curX = (int)(j + item.Offset.x);
				auto curY = (int)(i + item.Offset.y);
				auto iTx = item.Derivative.x;
				auto iTy = item.Derivative.y;
				if (curX < 0 || curY < 0 || curY > tpImage.rows - 1 || curX > tpImage.cols - 1)
					continue;

				auto iSx = gx.ptr<double>(curY)[curX];
				auto iSy = gy.ptr<double>(curY)[curX];

				if ((iSx != 0 || iSy != 0) && (iTx != 0 || iTy != 0))
				{
					auto mag = magnitude.ptr<double>(curY)[curX];
					auto matGradMag = (mag == 0) ? 0 : 1.0 / mag;
					partialSum += ((iSx * iTx) + (iSy * iTy)) * (item.Magnitude * matGradMag);

					auto sumOfCoords = m + 1;
					partialScore = partialSum / sumOfCoords;

					if (partialScore < MIN((minScore - 1) + normGreediness * sumOfCoords, normMinScore * sumOfCoords))
						break;
				}
			}
			scoreMap.ptr<double>(i)[j] = partialScore;
		}
	}
	//归一化
	cv::normalize(scoreMap, scoreMap, 1.0, 0, cv::NORM_MINMAX);
	//计算结果非极大值抑制处理
	std::vector<int> indices;
	std::vector<cv::Rect> bboxes; std::vector<float> scoreIndex;
	for (int y = 0; y < Y; y++)
	{
		for (int x = 0; x < X; x++)
		{
			if (scoreMap.ptr<double>(y)[x]>minScore) {
				bboxes.push_back(cv::Rect(x - templSize.width / 2, y - templSize.height / 2, templSize.width, templSize.height));
				scoreIndex.push_back((float)scoreMap.ptr<double>(y)[x]);
			}
		}
	}

	cv::dnn::NMSBoxes(bboxes, scoreIndex, (float)minScore, 0.25f, indices);

	for (int i = 0; i < indices.size(); i++) {
		resultPoint = (bboxes[indices[i]].tl() + bboxes[indices[i]].br()) / 2;
		//画图显示
		auto contours = std::vector<cv::Point>();
		for (int i = 0; i < results.size(); i++)
		{
			contours.push_back(results[i].Offset + cv::Point2d(resultPoint));
		}
		draw_match_shapes(showMat, resultPoint, cv::Scalar(151, 243, 121), 1);
	}

	cv::imwrite("result.png", showMat);
	return 0;
}

void shape_based_matching::draw_match_shapes(cv::Mat &showMat, cv::Point cog, cv::Scalar color, int linew)
{
	//绘图显示
	cv::circle(showMat, cog, 2, cv::Scalar(0, 0, 255), -1);
	auto contours = std::vector<cv::Point>();
	for (int i = 0; i < results.size(); i++)
	{
		contours.push_back(results[i].Offset + cv::Point2d(cog));
	}
	for (auto&point : contours) {
		showMat.at<cv::Vec3b>(point) = cv::Vec3b((uchar)color[0], (uchar)color[1], (uchar)color[2]);
	}
}

int eyemMakeShapeModel(EyemImage tpImage, double dContrast, double dMinContrast, int iApertureSize, bool bL2gradient)
{
	CV_Assert(NULL != tpImage.vpImage);

	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;
	//创建模板
	sbm.create_shape_model(image, dContrast, dMinContrast, iApertureSize, bL2gradient);
	return FUNC_OK;
}

int eyemFindShapeModel(EyemImage tpImage, int iNumLevels, double dAngleStart, double dAngleExtent, double dAngleStep, double dMinContrast, double dContrast, double dGreediness, double dMinScore, bool bOptimization)
{
	CV_Assert(NULL != tpImage.vpImage);

	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;

	//匹配模板
	cv::Point result;
	sbm.find_shape_model(image, dMinScore, dGreediness, result);
	return FUNC_OK;
}