#include "Steger.h"

#include <math.h>
#include <vector>
#include <chrono>

const double PI = 3.1415926535897932;

typedef CvPoint2D64f Point2d;
std::vector<cv::Point2d> g_contour;

static CvMat* M;
static CvMat* E;
static CvMat* I;

double *fhx(double zita, int m)
{
	int size = 2 * m + 1;
	double *pData = new double[size*size];
	for (int i = -m; i <= m; i++) {
		for (int j = -m; j <= m; j++) {
			pData[(i + m)*size + (j + m)] = -j / (zita*zita)*1.0 / (2.0*PI*zita*zita)*exp(-(i*i + j*j) / (2.0*zita*zita));
		}
	}

	return pData;
}

double *fhy(double zita, int m)
{
	int size = 2 * m + 1;
	double *pData = new double[size*size];
	for (int i = -m; i <= m; i++) {
		for (int j = -m; j <= m; j++) {
			pData[(i + m)*size + (j + m)] = -i / (zita*zita)*1.0 / (2.0*PI*zita*zita)*exp(-(i*i + j*j) / (2.0*zita*zita));
		}
	}

	return pData;
}
double *fhxx(double zita, int m)
{
	int size = 2 * m + 1;
	double *pData = new double[size*size];
	for (int i = -m; i <= m; i++) {
		for (int j = -m; j <= m; j++) {
			pData[(i + m)*size + (j + m)] = (j*j / pow(zita, 4) - 1.0 / (zita*zita))*1.0 / (2.0*PI*zita*zita)*exp(-(i*i + j*j) / (2.0*zita*zita));
		}
	}

	return pData;
}
double *fhyy(double zita, int m)
{
	int size = 2 * m + 1;
	double *pData = new double[size*size];
	for (int i = -m; i <= m; i++) {
		for (int j = -m; j <= m; j++) {
			pData[(i + m)*size + (j + m)] = (i*i / pow(zita, 4) - 1.0 / (zita*zita))*1.0 / (2.0*PI*zita*zita)*exp(-(i*i + j*j) / (2.0*zita*zita));
		}
	}

	return pData;
}
double *fhxy(double zita, int m)
{
	int size = 2 * m + 1;
	double *pData = new double[size*size];
	for (int i = -m; i <= m; i++) {
		for (int j = -m; j <= m; j++) {
			pData[(i + m)*size + (j + m)] = i*j / pow(zita, 4) * 1.0 / (2.0*PI*zita*zita)*exp(-(i*i + j*j) / (2.0*zita*zita));
		}
	}

	return pData;
}

void getSubdata(unsigned char *pData, int width, int h, int w, int m, unsigned char *tData)
{
	int size = 2 * m + 1;
	for (int i = -m; i <= m; i++) {
		memcpy(tData + (i + m)*size, pData + (h + i)*width + w - m, size);
	}
}

double sum(unsigned char *pData, int width, int h, int w, double *mat, int m)
{
	double s = 0.0;
	int size = (2 * m + 1)*(2 * m + 1);
	double *p = mat;
	unsigned char *q = pData;
	for (int i = size - 1; i >= 0; i--)
		s += (*p++) * (*q++);

	return s;
}

CSteger::CSteger(int width, int height)
{
	m_width = width;
	m_height = height;
	m_pData = NULL;

	zita = 6 / sqrt(3.0); //6
	vThresh = 50;
	ftt_max2 = -1;

	h_size = (int)(2 * ceil(3 * zita - 1) + 1);
	h_N1 = (h_size - 1) / 2;

	hx = fhx(zita, h_N1);
	hy = fhy(zita, h_N1);
	hxx = fhxx(zita, h_N1);
	hxy = fhxy(zita, h_N1);
	hyy = fhyy(zita, h_N1);

	M = cvCreateMat(2, 2, CV_64FC1);
	E = cvCreateMat(2, 2, CV_64FC1);
	I = cvCreateMat(2, 1, CV_64FC1);
}


CSteger::~CSteger()
{
	delete[] hx;
	delete[] hy;
	delete[] hxx;
	delete[] hyy;
	delete[] hxy;
}


void CSteger::Clear()
{
	m_width = 0;
	m_height = 0;
	if (m_pData){
		delete[] m_pData;
		m_pData = NULL;
	}
}


void CSteger::SetSize(int width, int height, unsigned char *pData)
{
	if (width != m_width || m_height != height) {
		Clear();

		m_width = width;
		m_height = height;
		m_pData = new unsigned char[m_width*m_height];
	}

	memcpy(m_pData, pData, m_width*m_height);
}

std::vector<cv::Point2d> CSteger::StripCenter(cv::Mat image)
{


    auto t1=std::chrono::steady_clock::now();

	SetSize(image.cols, image.rows, (unsigned char *)image.data);

	unsigned char *tData = new unsigned char[h_size*h_size];

	int i, j;
	double gx, gy, gxx, gyy, gxy;

	cvZero(E);
	cvZero(I);
	int acc1 = 0, acc2 = 0;
	double nx, ny, fttmax, t;
	cv::Point2d pt;
	
	g_contour.clear();

	for (i = h_N1; i < m_height - h_N1; i++) {
		for (j = h_N1; j < m_width - h_N1; j++) {
			if (m_pData[i*m_width + j] > vThresh) {
				getSubdata(m_pData, m_width, i, j, h_N1, tData);
				gx = sum(tData, m_width, i, j, hx, h_N1);
				gy = sum(tData, m_width, i, j, hy, h_N1);
				gxx = sum(tData, m_width, i, j, hxx, h_N1);
				gxy = sum(tData, m_width, i, j, hxy, h_N1);
				gyy = sum(tData, m_width, i, j, hyy, h_N1);

				cvmSet(M, 0, 0, gxx);
				cvmSet(M, 0, 1, gxy);
				cvmSet(M, 1, 0, gxy);
				cvmSet(M, 1, 1, gyy);

				cvEigenVV(M, E, I);

				if (fabs(cvmGet(I, 0, 0)) > fabs(cvmGet(I, 1, 0))){
					nx = cvmGet(E, 0, 0);
					ny = cvmGet(E, 0, 1);
					fttmax = cvmGet(I, 0, 0);
				}
				else {
					nx = cvmGet(E, 1, 0);
					ny = cvmGet(E, 1, 1);
					fttmax = cvmGet(I, 1, 0);
				}

				t = -(nx*gx + ny*gy) / (nx*nx*gxx + 2 * nx*ny*gxy + ny*ny*gyy);
				if (fabs(t*nx) <= 0.5 && fabs(t*ny) <= 0.5 && fttmax<ftt_max2){
					pt.y = i + t*nx;
					pt.x = j + t*ny;
					//if(pt.x>400 && pt.x<2000 && pt.y>50 && pt.y<1900) {
						g_contour.push_back(pt);
						acc1++;
					//}
				}
				acc2++;
			}
		}
	}
	delete[] tData;

    auto t2=std::chrono::steady_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t2 - t1);
    double tm=double(duration.count()) * std::chrono::milliseconds::period::num /
            std::chrono::milliseconds::period::den;
    printf("triangle time :%lfs\n",tm);

	return g_contour;
}

void CSteger::SaveImage(char *filename)
{
	cv::Mat src = cv::Mat(cv::Size(m_width, m_height), 8, CV_8UC1);
	memcpy(src.data, m_pData, m_width*m_height);

	/*FILE *fp = fopen(".\\5.txt", "w");
	fprintf(fp, "%d\n", g_contour.size());
	int h, w;
	for (vector<cv::Point2d>::iterator it = g_contour.begin(); it != g_contour.end(); it++)
	{
		h = (int)((*it).x + 0.5);
		w = (int)((*it).y + 0.5);

		cvSet2D(src, h, w, cvScalar(0));
		fprintf(fp, "%lf\t%lf\n", (*it).x, (*it).y);
	}
	fclose(fp);
	*/
	cv::imwrite(filename, src);
}

void CSteger::test(cv::Mat image, cv::Mat& out)
{

	std::vector<cv::Point2d> points = StripCenter(image);

    cv::cvtColor(image,out,cv::COLOR_GRAY2RGB);
	for (int i = 0; i < points.size(); i++)
	{
		int x = int(points[i].x + 0.5);
		int y = int(points[i].y + 0.5);
		if (x < out.cols&&y < out.rows)
            cv::circle(out, points[i], 0.3, cv::Scalar(0, 0, 200));
	}

}