#include<iostream>
#include<opencv2\opencv.hpp>
#include<unordered_map>
#include<math.h>
#include <corecrt_math_defines.h>
#include "lineDetection_steger.h"

#define USING_1D_MASK	1
#if 1
using namespace std;
using namespace cv;

struct EV_VAL_VEC {
	double nx, ny;//nx、ny为最大特征值对应的特征向量
};

void SetFilterMat(Mat& Dx, Mat& Dy, Mat& Dxx, Mat& Dyy, Mat& Dxy, int size_m)//radius * 2= size 
{
	if (size_m % 2 == 0 || size_m == 1) cout << "size is not illegal !!" << endl;
	Mat m1 = Mat::zeros(Size(size_m, size_m), CV_64F);//Dx
	Mat m2 = Mat::zeros(Size(size_m, size_m), CV_64F);//Dy
	Mat m3 = Mat::zeros(Size(size_m, size_m), CV_64F);//Dxx
	Mat m4 = Mat::zeros(Size(size_m, size_m), CV_64F);//Dyy
	Mat m5 = Mat::zeros(Size(size_m, size_m), CV_64F);//Dxy

	for (int i = 0; i < size_m / 2; i++)
	{
		m1.row(i) = 1;
		m2.col(i) = 1;
		m3.row(i) = -1;
		m4.col(i) = -1;
	}
	for (int i = size_m / 2 + 1; i < size_m; i++)
	{
		m1.row(i) = -1;
		m2.col(i) = -1;
		m3.row(i) = -1;
		m4.col(i) = -1;
	}
	m3.row(size_m / 2) = (size_m / 2) * 2;
	m4.col(size_m / 2) = (size_m / 2) * 2;
	m5.row(size_m / 2) = 1;
	m5.col(size_m / 2) = 1;
	m5.at<double>(size_m / 2, size_m / 2) = -(size_m / 2) * 4;
	if (size_m == 5) m5 = (Mat_<double>(5, 5) << 0, 0, 1, 0, 0, 0, 1, 2, 1, 0, 1, 2, -16, 2, 1, 0, 1, 2, 1, 0, 0, 0, 1, 0, 0);
	Dx = m2;
	Dy = m1;
	Dxx = m4;
	Dyy = m3;
	Dxy = m5;
	cout << Dx << endl;
	cout << Dy << endl;
	cout << Dxx << endl;
	cout << Dyy << endl;
	cout << Dxy << endl;
	return;
}

#if 0
bool isMax(int i, int j, Mat& img, int dx, int dy)//在法线方向上是否为极值
{
	double val = img.at<double>(j, i);
	double max_v = max(img.at<double>(j + dy, i + dx), img.at<double>(j - dy, i - dx));
	if (val >= max_v) return true;
	else return false;
}

void StegerLine(Mat& img0, vector<Point2d>& sub_pixel, int size_m)
{
	Mat img;
	cvtColor(img0, img0, cv::COLOR_BGR2GRAY);
	img = img0.clone();

	//高斯滤波
	img.convertTo(img, CV_64FC1);
	cv::GaussianBlur(img, img, Size(3, 3), 0.9, 0.9);
	cv::imwrite("gauss_blur_src.bmp", img);

	//ho_Image = Mat2Hobject(img);
	//Threshold(ho_Image, &ho_Region, 80, 255);
	//GetRegionPoints(ho_Region, &hv_Rows, &hv_Columns);


	//一阶偏导数
	Mat m1, m2;
	//二阶偏导数
	Mat m3, m4, m5;
	SetFilterMat(m1, m2, m3, m4, m5, size_m);

	cout << m5 << endl;

	Mat dx, dy;
	filter2D(img, dx, CV_64FC1, m1);
	filter2D(img, dy, CV_64FC1, m2);

	Mat dxx, dyy, dxy;
	filter2D(img, dxx, CV_64FC1, m3);
	filter2D(img, dyy, CV_64FC1, m4);
	filter2D(img, dxy, CV_64FC1, m5);

	//hessian矩阵
	int imgcol = img.cols;
	int imgrow = img.rows;
	vector<double> Pt;

	for (int i = 0; i < imgcol; i++)
	{
		for (int j = 0; j < imgrow; j++)
		{
			double pixel_val = img.at<double>(j, i);
			if (img.at<double>(j, i) > 50)            //需要确定ROI！！！
			{
				Mat hessian(2, 2, CV_64FC1);
				hessian.at<double>(0, 0) = dxx.at<double>(j, i);
				hessian.at<double>(0, 1) = dxy.at<double>(j, i);
				hessian.at<double>(1, 0) = dxy.at<double>(j, i);
				hessian.at<double>(1, 1) = dyy.at<double>(j, i);

				Mat eValue;
				Mat eVectors;
				eigen(hessian, eValue, eVectors);

				double nx, ny;
				double EV_max = 0;//特征值
				double EV_min = 0;
				if (fabs(eValue.at<double>(0, 0)) >= fabs(eValue.at<double>(1, 0)))  //求特征值最大时对应的特征向量
				{
					nx = eVectors.at<double>(0, 0);//大的特征值对应的特征向量
					ny = eVectors.at<double>(0, 1);
					EV_max = max(eValue.at<double>(0, 0), eValue.at<double>(1, 0));
					EV_min = min(eValue.at<double>(0, 0), eValue.at<double>(1, 0));
				}
				else
				{
					nx = eVectors.at<double>(1, 0);//大的特征值对应的特征向量
					ny = eVectors.at<double>(1, 1);
					EV_max = max(eValue.at<double>(0, 0), eValue.at<double>(1, 0));
					EV_min = min(eValue.at<double>(0, 0), eValue.at<double>(1, 0));
				}

				//t 公式 泰勒展开到二阶导数
				double a = nx * nx * dxx.at<double>(j, i) + 2 * nx * ny * dxy.at<double>(j, i) + ny * ny * dyy.at<double>(j, i);
				double b = nx * dx.at<double>(j, i) + ny * dy.at<double>(j, i);
				double t = -b / a;

				int dx;
				int dy;

				if (abs(nx) >= 2 * abs(ny)) dx = 1, dy = 0;//垂直
				else if (abs(ny) >= 2 * abs(nx)) dx = 0, dy = 1;//水平
				else if (nx > 0) dx = 1, dy = 1;
				else if (ny > 0) dx = -1, dy = -1;

				if (isMax(i, j, img, dx, dy))
					//if(EV_min<-120)
				{
					double temp1 = t * nx;
					double temp2 = t * ny;
					if (fabs(t * nx) <= 0.5 && fabs(t * ny) <= 0.5)//(x + t * Nx, y + t * Ny)为亚像素坐标
					{
						Pt.push_back(i + t * nx);
						Pt.push_back(j + t * ny);
					}
				}
			}
		}
	}

	for (int k = 0; k < Pt.size() / 2; k++)
	{
		Point2d rpt;
		rpt.x = Pt[2 * k + 0];
		rpt.y = Pt[2 * k + 1];
		sub_pixel.push_back(rpt);
	}

}

//E:/workplace/Line_Dtection/Project1/1/1-0.bmp

int main()//E:/workplace/get_line_width/两头亮二原图/两头亮二/1300us.bmp
{
	Mat src = imread("E:/workplace/Line_Dtection/Project1/1/1-9.bmp");
	vector<Point2d>sub_pixel;
	StegerLine(src, sub_pixel, 5);
	ofstream outfile_r, outfile_c;
	outfile_r.open("subpixel_row.txt");
	outfile_c.open("subpixel_col.txt");
	outfile_r << sub_pixel.size() << endl;
	outfile_c << sub_pixel.size() << endl;
	for (int i = 0; i < sub_pixel.size(); i++)
	{
		outfile_r << setprecision(15) << "2 " << sub_pixel[i].y << endl;
		outfile_c << setprecision(15) << "2 " << sub_pixel[i].x << endl;
	}
	outfile_c.close();
	outfile_r.close();
	return 0;
}
#endif
#endif

typedef struct
{
	double dx;
	double dy;
	double dxx;
	double dyy;
	double dxy;
}sPtDerivate;

//直接采样法
double gaussKernel2D(double x, double y, double sigma)
{
	double sigma2 = sigma * sigma;
	double norm = 1.0 / (2 * M_PI * sigma2);
	return (norm * exp(-(x * x + y * y) / (2 * sigma2)));
}

// 2D高斯一阶导数掩模生成
void generate_1st_2Dmask_directSample(double sigma, int radius, cv::Mat& mask_x, cv::Mat& mask_y)
{
	int size = 2 * radius + 1;
	mask_x = cv::Mat::zeros(size, size, CV_64FC1);
	mask_y = cv::Mat::zeros(size, size, CV_64FC1);
	double norm = -1.0 / (sigma * sigma);
	for (int row = 0; row < size; row++)
	{
		double y = (double)(row - radius);
		for (int col = 0; col < size; col++)
		{
			double x = (double)(col - radius);
			double dx = x * norm * gaussKernel2D(x, y, sigma);
			double dy = y * norm * gaussKernel2D(x, y, sigma);
			mask_x.at<double>(row, col) = dx;
			mask_y.at<double>(row, col) = dy;
		}
	}
	return;
}

// 2D高斯二阶导数掩模生成
void generate_2nd_2Dmask_directSample(double sigma, int radius, cv::Mat& mask_xx, cv::Mat& mask_yy, cv::Mat& mask_xy) 
{
	int size = 2 * radius + 1;
	mask_xx = cv::Mat::zeros(size, size, CV_64FC1);
	mask_yy = cv::Mat::zeros(size, size, CV_64FC1);
	mask_xy = cv::Mat::zeros(size, size, CV_64FC1);
	double sigma2 = sigma * sigma;
	double norm = 1.0 / (sigma2 * sigma2);
	double offset = 1.0 / sigma2;
	for (int row = 0; row < size; row++)
	{
		double y = (double)(row - radius);
		for (int col = 0; col < size; col++)
		{
			double x = (double)(col - radius);
			double dxx = (x * x * norm - offset) * gaussKernel2D(x, y, sigma);
			double dyy = (y * y * norm - offset) * gaussKernel2D(x, y, sigma);
			double dxy = x * y * norm * gaussKernel2D(x, y, sigma);
			mask_xx.at<double>(row, col) = dxx;
			mask_yy.at<double>(row, col) = dyy;
			mask_xy.at<double>(row, col) = dxy;
		}
	}
	return;
}

double gaussKernel_1D(double x, double sigma)
{
	double sigma2 = sigma * sigma;
	double norm = 1.0 / (sqrt(2 * M_PI) * sigma);
	return (norm * exp(-(x * x ) / (2 * sigma2)));
}

// 2D高斯一阶导数掩模生成
void generate_1st_1Dmask_directSample(double sigma, int radius, cv::Mat& mask_x)
{
	int size = 2 * radius + 1;
	mask_x = cv::Mat::zeros(1, size, CV_64FC1);
	double norm = -1.0 / (sigma * sigma);
	for (int col = 0; col < size; col++)
	{
		double x = (double)(col - radius);
		double dx = x * norm * gaussKernel_1D(x, sigma);
		mask_x.at<double>(0, col) = dx;
	}
	return;
}

// 2D高斯二阶导数掩模生成
void generate_2nd_1Dmask_directSample(double sigma, int radius, cv::Mat& mask_xx)
{
	int size = 2 * radius + 1;
	mask_xx = cv::Mat::zeros(1, size, CV_64FC1);
	double sigma2 = sigma * sigma;
	double norm = 1.0 / (sigma2 * sigma2);
	double offset = 1.0 / sigma2;
	for (int col = 0; col < size; col++)
	{
		double x = (double)(col - radius);
		double dxx = (x * x * norm - offset) * gaussKernel_1D(x, sigma);
		mask_xx.at<double>(0, col) = dxx;
	}
	return;
}

// 单点卷积函数
sPtDerivate pointConvolve(
	cv::Point ptPos, //点位置  
	int radius,  //卷积窗半径
	cv::Mat& img,  //double型单通道图像
	cv::Mat& mask_x, //一阶掩膜x
	cv::Mat& mask_y, //一阶掩膜y
	cv::Mat& mask_xx, //二阶掩膜xx
	cv::Mat& mask_yy, //二阶掩膜yy
	cv::Mat& mask_xy //二阶掩膜xy
)
{
	sPtDerivate result = { 0.0, 0.0, 0.0, 0.0, 0.0 };
	cv::Size imgSize = img.size(); //
	if ((ptPos.x < radius ) || (ptPos.x > (imgSize.width-radius-1)) || (ptPos.y < radius) || (ptPos.y > (imgSize.height - radius - 1)))
		return result;

	int size = 2 * radius + 1;
	for (int i = 0; i < size; i++)
	{
		int row = i - radius;
		int y = ptPos.y + row;
		for (int j = 0; j < size; j++)
		{
			int col = j - radius;
			int x = ptPos.x + col;
			double value = img.at<double>(y, x);
			result.dx  += mask_x.at<double>(i, j) * value;
			result.dy  += mask_y.at<double>(i, j) * value;
			result.dxx += mask_xx.at<double>(i, j) * value;
			result.dyy += mask_yy.at<double>(i, j) * value;
			result.dxy += mask_xy.at<double>(i, j) * value;
		}
	}
	return result;
}

// 单点卷积函数
sPtDerivate pointConvolve_1D(
	cv::Point ptPos, //点位置  
	int radius,  //卷积窗半径
	cv::Mat& img,  //double型单通道图像
	cv::Mat& mask_x, //一阶掩膜x
	cv::Mat& mask_xx //二阶掩膜xx
)
{
	sPtDerivate result = { 0.0, 0.0, 0.0, 0.0, 0.0 };
	cv::Size imgSize = img.size(); //
	if ((ptPos.x < radius) || (ptPos.x > (imgSize.width - radius - 1)) || (ptPos.y < radius) || (ptPos.y > (imgSize.height - radius - 1)))
		return result;

	int size = 2 * radius + 1;
	for (int j = 0; j < size; j++)
	{
		int col = j - radius;
		int x = ptPos.x + col;
		double value = img.at<double>(ptPos.y, x);
		result.dx += mask_x.at<double>(0, j) * value;
		result.dy += 0;
		result.dxx += mask_xx.at<double>(0, j) * value;
		result.dyy += 0;
		result.dxy += 0;
	}
	return result;
}

bool isMax(int i, int j, cv::Mat& img, int dx, int dy)//在法线方向上是否为极值
{
	double val = img.at<double>(j, i);
	double max_v = std::max(img.at<double>(j + dy, i + dx), img.at<double>(j - dy, i - dx));
	if (val >= max_v) return true;
	else return false;
}

//计算Hessian得到亚像素
cv::Point2f computeHessianSubpix(
	cv::Point ptPos, //点位置
	cv::Mat& img,  //double型单通道图像
	sPtDerivate _ptDerivate
) 
{
	double subTh = 0.7;
	cv::Mat hessian(2, 2, CV_64FC1);
	hessian.at<double>(0, 0) = _ptDerivate.dxx;
	hessian.at<double>(0, 1) = _ptDerivate.dxy;
	hessian.at<double>(1, 0) = _ptDerivate.dxy;
	hessian.at<double>(1, 1) = _ptDerivate.dyy;

	cv::Mat eigenValue, eigenVector;
	cv::eigen(hessian, eigenValue, eigenVector);

	double nx, ny;
	double vnx, vny;
	double EV_max = 0;//特征值
	double EV_min = 0;
	if (fabs(eigenValue.at<double>(0, 0)) >= fabs(eigenValue.at<double>(1, 0)))  //求特征值最大时对应的特征向量
	{
		nx = eigenVector.at<double>(0, 0);//大的特征值对应的特征向量
		ny = eigenVector.at<double>(0, 1);
		vnx = eigenVector.at<double>(1, 0);//小的特征值对应的特征向量
		vny = eigenVector.at<double>(1, 1);
		EV_max = std::max(eigenValue.at<double>(0, 0), eigenValue.at<double>(1, 0));
		EV_min = std::min(eigenValue.at<double>(0, 0), eigenValue.at<double>(1, 0));
	}
	else
	{
		nx = eigenVector.at<double>(1, 0);//大的特征值对应的特征向量
		ny = eigenVector.at<double>(1, 1);
		vnx = eigenVector.at<double>(0, 0);//小的特征值对应的特征向量
		vny = eigenVector.at<double>(0, 1);
		EV_max = std::max(eigenValue.at<double>(0, 0), eigenValue.at<double>(1, 0));
		EV_min = std::min(eigenValue.at<double>(0, 0), eigenValue.at<double>(1, 0));
	}

	cv::Point2f subPix = { -1.0f, -1.0f };  //初始化成无效亚像素值
	//t 公式 泰勒展开到二阶导数
	double a = nx * nx * _ptDerivate.dxx + 2 * nx * ny * _ptDerivate.dxy + ny * ny * _ptDerivate.dyy;
	double b = nx * _ptDerivate.dx + ny * _ptDerivate.dy;
	if (a != 0.0)
	{
		double t = b / a; // -b / a;
		int dx;
		int dy;

		if (abs(nx) >= 2 * abs(ny)) dx = 1, dy = 0;//垂直
		else if (abs(ny) >= 2 * abs(nx)) dx = 0, dy = 1;//水平
		else if (nx > 0) dx = 1, dy = 1;
		else if (ny > 0) dx = -1, dy = -1;

		if (isMax(ptPos.x, ptPos.y, img, dx, dy))
			//if(EV_min<-120)
		{
			double temp1 = t * nx;
			double temp2 = t * ny;
			if (fabs(t * nx) <= subTh && fabs(t * ny) <= subTh)//(x + t * Nx, y + t * Ny)为亚像素坐标
			{
				subPix.x = (float)(ptPos.x + t * nx);
				subPix.y = (float)(ptPos.y + t * ny);
			}
		}
	}
	return subPix;
}

//计算1D得到亚像素
cv::Point2f compute1DSubpix(
	cv::Point ptPos, //点位置
	cv::Mat& img,  //double型单通道图像
	sPtDerivate _ptDerivate
)
{
	double subTh = 0.7;
	cv::Point2f subPix = { -1.0f, -1.0f };  //初始化成无效亚像素值
	//t 公式 泰勒展开到二阶导数
	double a = _ptDerivate.dxx;
	double b = _ptDerivate.dx;
	if (a != 0.0)
	{
		double t = b / a; // -b / a;
		if (fabs(t) <= subTh )//(x + t * Nx, y + t * Ny)为亚像素坐标
		{
			subPix.x = (float)(ptPos.x + t);
			subPix.y = (float)(ptPos.y);
		}
	}
	return subPix;
}

// 从点的整数坐标计算亚像素
void computePointSubpix(
	cv::Mat& inputImg,  //uchar型单通道输入图像（灰度图像）
	std::vector<cv::Point>&pos,  //峰值像素的整数坐标。
	int gaussWin,  //使用steger算法时指定的窗口宽度。此宽度需要与激光线的宽度基本吻合
	std::vector<cv::Point2f>& posSubpix  //计算出的亚像素坐标
)
{
	cv::Mat mask_x, mask_y, mask_xx, mask_yy, mask_xy;
	double sigma = (double)gaussWin / sqrt(3);
	//double sigma = 1.0;
#if USING_1D_MASK
	//SetFilterMat(mask_x, mask_y, mask_xx, mask_yy, mask_xy, gaussWin*2+1);
	generate_1st_1Dmask_directSample(sigma, gaussWin, mask_x);
	generate_2nd_1Dmask_directSample(sigma, gaussWin, mask_xx);
#else
	generate_1st_2Dmask_directSample(sigma, gaussWin, mask_x, mask_y);
	generate_2nd_2Dmask_directSample(sigma, gaussWin, mask_xx, mask_yy, mask_xy);
#endif	

#if 0
	std::cout << "mask::" << std::endl;
	std::cout << mask_x << std::endl;
	std::cout << mask_y << std::endl;
	std::cout << mask_xx << std::endl;
	std::cout << mask_yy << std::endl;
	std::cout << mask_xy << std::endl;
#endif
	//高斯滤波
	cv::Mat img;
	inputImg.convertTo(img, CV_64FC1);
	cv::GaussianBlur(img, img, cv::Size(5, 5), 1.2, 1.2);
	//cv::imwrite("gauss_blur_src.bmp", img);

	for (int i = 0, i_max = (int)pos.size(); i < i_max; i++)
	{
		cv::Point ptPos = pos[i];
		int doSubPix = 0;
		while (doSubPix >= 0)
		{
#if USING_1D_MASK
			sPtDerivate a_ptDerivate = pointConvolve_1D(
				ptPos, //点位置  
				gaussWin,  //卷积窗半径
				img,  //double型单通道图像
				mask_x, //一阶掩膜x
				mask_xx //二阶掩膜xx
			);
			cv::Point2f subpix = compute1DSubpix(
				ptPos, //点位置
				img,  //double型单通道图像
				a_ptDerivate
			);
#else
			sPtDerivate a_ptDerivate = pointConvolve(
				ptPos, //点位置  
				gaussWin,  //卷积窗半径
				img,  //double型单通道图像
				mask_x, //一阶掩膜x
				mask_y, //一阶掩膜y
				mask_xx, //二阶掩膜xx
				mask_yy, //二阶掩膜yy
				mask_xy //二阶掩膜xy
			);
			cv::Point2f subpix = computeHessianSubpix(
				ptPos, //点位置
				img,  //double型单通道图像
				a_ptDerivate
			);
#endif
			if ((subpix.x >= 0) && (subpix.y >= 0))
			{
				subpix.x += 0.5; //图像中的像素位置表示的是像素是左下角，而此处像素坐标是指向像素中间位置，所以有0.5像素差
				subpix.y -= 0.5;  //此处图像的Y坐标系方向与通常坐标系的方向相反
				posSubpix.push_back(subpix);
				doSubPix = -1;
			}
			else
			{
				if (0 == doSubPix)
				{
					ptPos.x = pos[i].x - 1;
					doSubPix = 1;
				}
				else if (1 == doSubPix)
				{
					ptPos.x = pos[i].x + 1;
					doSubPix = 2;
				}
				else
				{
					doSubPix = -1;
				}
			}
		}
	}
	return;
}