#include "WD_convolveGauss.h"
#include <math.h>
#include <stdlib.h>

// TODO: Auto-generated Javadoc
/**
 * The Class Normal.
 */
static double SQRT_2_PI_INV = 0.398942280401432677939946059935;

/** The Constant MAX_SIZE_MASK_0. */
static double MAX_SIZE_MASK_0 = 3.09023230616781; /* Size for Gaussian mask */

/** The Constant MAX_SIZE_MASK_1. */
static double MAX_SIZE_MASK_1 = 3.46087178201605; /* Size for 1st derivative mask */

/** The Constant MAX_SIZE_MASK_2. */
static double MAX_SIZE_MASK_2 = 3.82922419517181; /* Size for 2nd derivative mask */

/**
 * Mask size.
 *
 * @param MAX
 *            the max
 * @param sigma
 *            the sigma
 * @return the int
 */
static int MASK_SIZE(double MAX, double sigma) 
{
	return (int)ceil(MAX * sigma); /* Maximum mask index */
}

double getNormal(double x)
{

	/** The Constant SQRTPI. */
	static double SQRTPI = 1.772453850905516027;

	/** The Constant UPPERLIMIT. */
	static double UPPERLIMIT = 20.0;

	/** The Constant P10. */
	static double P10 = 242.66795523053175;

	/** The Constant P11. */
	static double P11 = 21.979261618294152;

	/** The Constant P12. */
	static double P12 = 6.9963834886191355;

	/** The Constant P13. */
	static double P13 = -.035609843701815385;

	/** The Constant Q10. */
	static double Q10 = 215.05887586986120;

	/** The Constant Q11. */
	static double Q11 = 91.164905404514901;

	/** The Constant Q12. */
	static double Q12 = 15.082797630407787;

	/** The Constant Q13. */
	static double Q13 = 1.0;

	/** The Constant P20. */
	static double P20 = 300.4592610201616005;

	/** The Constant P21. */
	static double P21 = 451.9189537118729422;

	/** The Constant P22. */
	static double P22 = 339.3208167343436870;

	/** The Constant P23. */
	static double P23 = 152.9892850469404039;

	/** The Constant P24. */
	static double P24 = 43.16222722205673530;

	/** The Constant P25. */
	static double P25 = 7.211758250883093659;

	/** The Constant P26. */
	static double P26 = .5641955174789739711;

	/** The Constant P27. */
	static double P27 = -.0000001368648573827167067;

	/** The Constant Q20. */
	static double Q20 = 300.4592609569832933;

	/** The Constant Q21. */
	static double Q21 = 790.9509253278980272;

	/** The Constant Q22. */
	static double Q22 = 931.3540948506096211;

	/** The Constant Q23. */
	static double Q23 = 638.9802644656311665;

	/** The Constant Q24. */
	static double Q24 = 277.5854447439876434;

	/** The Constant Q25. */
	static double Q25 = 77.00015293522947295;

	/** The Constant Q26. */
	static double Q26 = 12.78272731962942351;

	/** The Constant Q27. */
	static double Q27 = 1.0;

	/** The Constant P30. */
	static double P30 = -.00299610707703542174;

	/** The Constant P31. */
	static double P31 = -.0494730910623250734;

	/** The Constant P32. */
	static double P32 = -.226956593539686930;

	/** The Constant P33. */
	static double P33 = -.278661308609647788;

	/** The Constant P34. */
	static double P34 = -.0223192459734184686;

	/** The Constant Q30. */
	static double Q30 = .0106209230528467918;

	/** The Constant Q31. */
	static double Q31 = .191308926107829841;

	/** The Constant Q32. */
	static double Q32 = 1.05167510706793207;

	/** The Constant Q33. */
	static double Q33 = 1.98733201817135256;

	/** The Constant Q34. */
	static double Q34 = 1.0;

	/** The Constant SQRT2. */
	static double SQRT2 = 1.41421356237309504880;

	int sn;
	double R1, R2, y, y2, y3, y4, y5, y6, y7;
	double erf, erfc, z, z2, z3, z4;
	double phi;

	if (x < -UPPERLIMIT)
		return 0.0;
	if (x > UPPERLIMIT)
		return 1.0;

	y = x / SQRT2;
	if (y < 0) {
		y = -y;
		sn = -1;
	}
	else
		sn = 1;

	y2 = y * y;
	y4 = y2 * y2;
	y6 = y4 * y2;

	if (y < 0.46875) {
		R1 = P10 + P11 * y2 + P12 * y4 + P13 * y6;
		R2 = Q10 + Q11 * y2 + Q12 * y4 + Q13 * y6;
		erf = y * R1 / R2;
		if (sn == 1)
			phi = 0.5 + 0.5 * erf;
		else
			phi = 0.5 - 0.5 * erf;
	}
	else if (y < 4.0) {
		y3 = y2 * y;
		y5 = y4 * y;
		y7 = y6 * y;
		R1 = P20 + P21 * y + P22 * y2 + P23 * y3 + P24 * y4 + P25 * y5 + P26 * y6 + P27 * y7;
		R2 = Q20 + Q21 * y + Q22 * y2 + Q23 * y3 + Q24 * y4 + Q25 * y5 + Q26 * y6 + Q27 * y7;
		erfc = exp(-y2) * R1 / R2;
		if (sn == 1)
			phi = 1.0 - 0.5 * erfc;
		else
			phi = 0.5 * erfc;
	}
	else {
		z = y4;
		z2 = z * z;
		z3 = z2 * z;
		z4 = z2 * z2;
		R1 = P30 + P31 * z + P32 * z2 + P33 * z3 + P34 * z4;
		R2 = Q30 + Q31 * z + Q32 * z2 + Q33 * z3 + Q34 * z4;
		erfc = (exp(-y2) / y) * (1.0 / SQRTPI + R1 / (R2 * y2));
		if (sn == 1)
			phi = 1.0 - 0.5 * erfc;
		else
			phi = 0.5 * erfc;
	}

	return phi;
}

/**
 * Phi 0.
 *
 * @param x
 *            the x
 * @param sigma
 *            the sigma
 * @return the double
 */
 /* Integral of the Gaussian function */
double phi0(double x, double sigma) {
	return getNormal(x / sigma);
}

/**
 * Phi 1.
 *
 * @param x
 *            the x
 * @param sigma
 *            the sigma
 * @return the double
 */
 /* The Gaussian function */
double phi1(double x, double sigma) {
	double t;
	t = x / sigma;
	return SQRT_2_PI_INV / sigma * exp(-0.5 * t * t);
}

/**
 * Phi 2.
 *
 * @param x
 *            the x
 * @param sigma
 *            the sigma
 * @return the double
 */
 /* First derivative of the Gaussian function */
double phi2(double x, double sigma) {
	double t;
	t = x / sigma;
	return -x * SQRT_2_PI_INV / pow(sigma, 3.0) * exp(-0.5 * t * t);
}

/* Gaussian smoothing mask */

/**
 * Compute gauss mask 0.
 *
 * @param num
 *            the num
 * @param sigma
 *            the sigma
 * @return the double[]
 */
 /*
  * num ist eigentlich pointer - aufrufende Funkion nimmt an, dass num geändert
  * wird. Übergebe es deswegen als MutableDouble aus CommonsLang
  */
double* compute_gauss_mask_0(int* num, double sigma) {

	int i, n;
	double limit;
	
	limit = MASK_SIZE(MAX_SIZE_MASK_0, sigma); /* Error < 0.001 on each side */
	n = (int)limit;
	double* h = (double*)malloc(sizeof(double)*(2*n+1));//h = new double[2 * n + 1];
	for (i = -n + 1; i <= n - 1; i++)
		h[n + i] = phi0(-i + 0.5, sigma) - phi0(-i - 0.5, sigma);
	h[0] = 1.0 - phi0(n - 0.5, sigma);
	h[2 * n] = phi0(-n + 0.5, sigma);
	*num = n;
	return h;
}

/* First derivative of Gaussian smoothing mask */
/**
 * Compute gauss mask 1.
 *
 * @param num
 *            the num
 * @param sigma
 *            the sigma
 * @return the double[]
 */
 /*
  * num ist eigentlich pointer - aufrufende Funkion nimmt an, dass num geändert
  * wird. Übergebe es deswegen als MutableDouble aus CommonsLang
  */
double* compute_gauss_mask_1(int* num, double sigma) {
	int i, n;
	double limit = MASK_SIZE(MAX_SIZE_MASK_1, sigma); /* Error < 0.001 on each side */
	n = (int)limit;
	double* h = (double*)malloc(sizeof(double) * (2 * n + 1));	//h = new double[2 * n + 1];

	for (i = -n + 1; i <= n - 1; i++)
		h[n + i] = phi1(-i + 0.5, sigma) - phi1(-i - 0.5, sigma);
	h[0] = -phi1(n - 0.5, sigma);
	h[2 * n] = phi1(-n + 0.5, sigma);
	*num = n;
	return h;
}

int compute_gauss_mask_1_scale(std::vector<double>& gaussMask1st, double sigma, float scale) {
	int i, n;
	double limit = MASK_SIZE(MAX_SIZE_MASK_1, sigma) * (1.0f / scale); /* Error < 0.001 on each side */
	n = (int)limit;
	gaussMask1st.resize(n * 2 + 1);
	//double* h = (double*)malloc(sizeof(double) * (2 * n + 1));	//h = new double[2 * n + 1];
	float winSize = 0.5 * scale;
	//for (i = -n + 1; i <= n - 1; i++)
	for (i = -n; i <= n; i++)
	{
		float winCenter = i * scale;
		gaussMask1st[n + i] = phi1(-winCenter + winSize, sigma) - phi1(-winCenter - winSize, sigma);
	}
	//h[0] = -phi1( (n - 0.5)*scale, sigma);
	//h[2 * n] = phi1((-n + 0.5)*scale, sigma);
	return n;
}

/* Second derivative of Gaussian smoothing mask */
/**
 * Compute gauss mask 2.
 *
 * @param num
 *            the num
 * @param sigma
 *            the sigma
 * @return the double[]
 */
 /*
  * num ist eigentlich pointer - aufrufende Funkion nimmt an, dass num geändert
  * wird. Übergebe es deswegen als MutableDouble aus CommonsLang
  */
double* compute_gauss_mask_2(int* num, double sigma) {
	int i, n;
	double limit = MASK_SIZE(MAX_SIZE_MASK_2, sigma); /* Error < 0.001 on each side */
	n = (int)limit;
	double* h = (double*)malloc(sizeof(double) * (2 * n + 1));	//h = new double[2 * n + 1];

	for (i = -n + 1; i <= n - 1; i++)
		h[n + i] = phi2(-i + 0.5, sigma) - phi2(-i - 0.5, sigma);
	h[0] = -phi2(n - 0.5, sigma);
	h[2 * n] = phi2(-n + 0.5, sigma);
	*num = n;
	return h;
}

int compute_gauss_mask_2_scale(std::vector<double>& gaussMask2nd, double sigma, float scale) {
	int i, n;
	double limit = MASK_SIZE(MAX_SIZE_MASK_2, sigma) * (1.0f / scale); /* Error < 0.001 on each side */
	n = (int)limit;
	gaussMask2nd.resize(n * 2 + 1);
	//double* h = (double*)malloc(sizeof(double) * (2 * n + 1));	//h = new double[2 * n + 1];

	float winSize = 0.5 * scale;
	for (i = -n + 1; i <= n - 1; i++)
	{
		float winCenter = i * scale;
		gaussMask2nd[n + i] = phi2(-winCenter + winSize, sigma) - phi2(-winCenter - winSize, sigma);
	}
	gaussMask2nd[0] = -phi2( (n - 0.5)*scale, sigma);
	gaussMask2nd[2 * n] = phi2( (-n + 0.5)*scale, sigma);
	return n;
}

/**
 * Lincoor.
 *
 * @param row
 *            the row
 * @param col
 *            the col
 * @param width
 *            the width
 * @return the int
 */
 /*
  * Translate row and column coordinates of an image into an index into its
  * one-dimensional array.
  */
int LINCOOR(int row, int col, int width) {
	return row * width + col;
}

/**
 * Br.
 *
 * @param row
 *            the row
 * @param height
 *            the height
 * @return the int
 */
 /*
  * Mirror the row coordinate at the borders of the image; height must be a
  * defined variable in the calling function containing the image height.
  */
int BR(int row, int height) {
	return ((row) < 0 ? -(row) : (row) >= height ? height - (row)+height - 2 : (row));
}

/**
 * Bc.
 *
 * @param col
 *            the col
 * @param width
 *            the width
 * @return the int
 */
 /*
  * Mirror the column coordinate at the borders of the image; width must be a
  * defined variable in the calling function containing the image width.
  */
int BC(int col, int width) {
	return ((col) < 0 ? -(col) : (col) >= width ? width - (col)+width - 2 : (col));
}

/*
 * Convolve an image with the derivatives of a Gaussian smoothing kernel. Since
 * all of the masks are separable, this is done in two steps in the function
 * convolve_gauss. Firstly, the rows of the image are convolved by an
 * appropriate one-dimensional mask in convolve_rows_gauss, yielding an
 * intermediate float-image h. Then the columns of this image are convolved by
 * another appropriate mask in convolve_cols_gauss to yield the final result k.
 * At the border of the image the gray values are mirrored.
 */

 /**
  * Convolve rows gauss.
  *
  * @param image
  *            the image
  * @param mask
  *            the mask
  * @param n
  *            the n
  * @param h
  *            the h
  * @param width
  *            the width
  * @param height
  *            the height
  */
  /* Convolve the rows of an image with the derivatives of a Gaussian. */
void convolve_rows_gauss(float* image, double* mask, int n, float* h, int width, int height) 
{
	int j, r, c, l;
	double sum;

	/* Inner region */
	for (r = n; r < height - n; r++) 
	{
		for (c = 0; c < width; c++) 
		{
			l = LINCOOR(r, c, width);
			sum = 0.0;
			for (j = -n; j <= n; j++)
				sum += (double)(image[(l + j * width)]) * mask[(j + n)];
			h[l] = (float)sum;
		}
	}
	/* Border regions */
	for (r = 0; r < n; r++) 
	{
		for (c = 0; c < width; c++) 
		{
			l = LINCOOR(r, c, width);
			sum = 0.0;
			for (j = -n; j <= n; j++)
				sum += (double)(image[LINCOOR(BR(r + j, height), c, width)]) * mask[(j + n)];
			h[l] = (float)sum;
		}
	}
	for (r = height - n; r < height; r++) 
	{
		for (c = 0; c < width; c++) 
		{
			l = LINCOOR(r, c, width);
			sum = 0.0;
			for (j = -n; j <= n; j++)
				sum += (double)(image[LINCOOR(BR(r + j, height), c, width)]) * mask[(j + n)];
			h[l] = (float)sum;
		}
	}
}

/**
 * Convolve cols gauss.
 *
 * @param h
 *            the h
 * @param mask
 *            the mask
 * @param n
 *            the n
 * @param k
 *            the k
 * @param width
 *            the width
 * @param height
 *            the height
 */
 /* Convolve the columns of an image with the derivatives of a Gaussian. */
void convolve_cols_gauss(float* h, double* mask, int n, float* k, int width, int height) {
	int j, r, c, l;
	double sum;

	/* Inner region */
	for (r = 0; r < height; r++) {
		for (c = n; c < width - n; c++) {
			l = LINCOOR(r, c, width);
			sum = 0.0;
			for (j = -n; j <= n; j++)
				sum += h[(l + j)] * mask[(j + n)];
			k[l] = (float)sum;
		}
	}
	/* Border regions */
	for (r = 0; r < height; r++) {
		for (c = 0; c < n; c++) {
			l = LINCOOR(r, c, width);
			sum = 0.0;
			for (j = -n; j <= n; j++)
				sum += h[LINCOOR(r, BC(c + j, width), width)] * mask[(j + n)];
			k[l] = (float)sum;
		}
	}
	for (r = 0; r < height; r++) {
		for (c = width - n; c < width; c++) {
			l = LINCOOR(r, c, width);
			sum = 0.0;
			for (j = -n; j <= n; j++)
				sum += h[LINCOOR(r, BC(c + j, width), width)] * mask[(j + n)];
			k[l] = (float)sum;
		}
	}
}

/**
 * Convolve gauss.
 *
 * @param image
 *            the image
 * @param k
 *            the k
 * @param width
 *            the width
 * @param height
 *            the height
 * @param sigma
 *            the sigma
 * @param deriv_type
 *            the deriv type
 */
 /* Convolve an image with a derivative of the Gaussian. */
void convolve_gauss(float* image, float* k, int width, int height, double sigma, DERIV_TYPE deriv_type) {
	double *hr = NULL, *hc = NULL;
	double* maskr=NULL, *maskc=NULL;
	int nr=0, nc=0;
	float* h;

	h = (float*)malloc(sizeof(float)*width * height);

	switch (deriv_type) {
	case DERIV_R:
		hr = compute_gauss_mask_1(&nr, sigma);
		hc = compute_gauss_mask_0(&nc, sigma);
		break;
	case DERIV_C:
		hr = compute_gauss_mask_0(&nr, sigma);
		hc = compute_gauss_mask_1(&nc, sigma);
		break;
	case DERIV_RR:
		hr = compute_gauss_mask_2(&nr, sigma);
		hc = compute_gauss_mask_0(&nc, sigma);
		break;
	case DERIV_RC:
		hr = compute_gauss_mask_1(&nr, sigma);
		hc = compute_gauss_mask_1(&nc, sigma);
		break;
	case DERIV_CC:
		hr = compute_gauss_mask_0(&nr, sigma);
		hc = compute_gauss_mask_2(&nc, sigma);
		break;
	}

	maskr = hr;// + nr; Wird ersetzt in den eigentlichen Funktionen, indem ich z.B. in
				// convolve_rows_gauss immer beim Zugriff auf mask n dazuaddiere
	maskc = hc;// + nc;

	convolve_rows_gauss(image, maskr, nr, h, width, height);
	convolve_cols_gauss(h, maskc, nc, k, width, height);

}


/**
 * Convolve cols gauss.
 *
 * @param laserLine
 *            the source laser line
 * @param mask
 *            the mask
 * @param n
 *            the n
 * @param result
 *            the result
 * @param width
 *            the width
 * @param height
 *            the height
 */
 /* Convolve the columns of an image with the derivatives of a Gaussian. */
void convolve_laserLine_gauss(
	std::vector<SVzNL3DPosition>& a_line, 
	int startId, int endId, 
	std::vector<double>&mask, int maskSize, 
	std::vector<double>& convolveResult) 
{
	convolveResult.resize(a_line.size());
	int width = endId - startId + 1;//a_line->m_3DPointCount;
	std::vector<SVzNL3DPosition> dataBuff;
	dataBuff.insert(dataBuff.end(), a_line.begin() + startId, a_line.begin() + endId+1);  //范围是左闭右开
	double* result = &convolveResult[startId];
	/* Inner region */
	for (int c = maskSize; c < width - maskSize; c++)
	{
		double sum = 0.0;
		for (int j = -maskSize; j <= maskSize; j++)
			sum += dataBuff[(c + j)].pt3D.z * mask[(j + maskSize)];
		result[c] = (float)sum;
	}

	/* Border regions */
	for (int c = 0; c < maskSize; c++) 
	{
		double sum = 0.0;
		for (int j = -maskSize; j <= maskSize; j++)
		{
			int mirror = BC(c + j, width);
			sum += dataBuff[mirror].pt3D.z * mask[(j + maskSize)];
		}	
		result[c] = (float)sum;
	}
	for (int c = width - maskSize; c < width; c++) {

		double sum = 0.0;
		for (int j = -maskSize; j <= maskSize; j++)
		{
			int mirror = BC(c + j, width);
			sum += dataBuff[mirror].pt3D.z * mask[(j + maskSize)];
		}
		result[c] = sum;
	}
	return;
}


void convolve_laserLine_gauss_scale(
	std::vector<SVzNL3DPosition>& a_line, 
	int startId, int endId, 
	std::vector<double>& mask, int maskSize, 
	float scale, 
	std::vector<double>& conResult)
{
	int ptNum = endId - startId + 1; //a_line->m_3DPointCount;
	std::vector<SVzNL3DPosition> dataBuff;
	dataBuff.insert(dataBuff.end(), a_line.begin() + startId, a_line.begin() + endId + 1);
	
	conResult.resize(a_line.size());
	double* result = &conResult[startId];

	float halfWidth = (float)maskSize * scale;
	SVzNL3DPoint startPt = dataBuff[0].pt3D;
	SVzNL3DPoint endPt = dataBuff[ptNum-1].pt3D;
	
	for (int i = 0; i < ptNum; i++)
	{
		if ((i == 200) || (i == 280) || (i == 360) || (i == 850) ||(i==950) || (i == 1050))
			int kkk = 1;
		SVzNL3DPoint currPt = dataBuff[i].pt3D;
		double convolveResult = mask[maskSize] * currPt.z;
		//向前chkWidth/2
		int id = i;
		double dist = 0;
		SVzNL3DPoint preConvolvePt = currPt;
		int preMaskPos = maskSize;
		while (1)
		{
			id--;
			int ptId = id < 0 ? -id : id;
			SVzNL3DPoint convolvePt = dataBuff[ptId].pt3D;
			dist = dist + fabs(convolvePt.y - preConvolvePt.y);
			if (dist > halfWidth)
				break;
			int maskPos = (halfWidth - dist) / scale;
			int diffId = abs(preMaskPos - maskPos);
			convolveResult += mask[maskPos] * convolvePt.z * (double)diffId;

			preConvolvePt = convolvePt;
			preMaskPos = maskPos;
		}

		//向后chkWidth/2
		id = i;
		dist = 0;
		preMaskPos = maskSize;
		preConvolvePt = currPt;
		while (1)
		{
			id++;
			int ptId = id >= ptNum ? (ptNum * 2 - 2 - id) : id;
			SVzNL3DPoint convolvePt = dataBuff[ptId].pt3D;
			dist = dist + fabs(convolvePt.y - preConvolvePt.y);
			if (dist > halfWidth)
				break;
			int maskPos = (halfWidth + dist) / scale;
			int diffId = abs(preMaskPos - maskPos);
			convolveResult += mask[maskPos] * convolvePt.z * (double)diffId;

			preConvolvePt = convolvePt;
			preMaskPos = maskPos;
		}

		result[i] = convolveResult;
	}
	return;
}

void wd_convolveGauss_laserLine(
	std::vector<SVzNL3DPosition>& a_line, 
	int startId, int endId, 
	std::vector<double>& gaussConvolve_1st, 
	std::vector<double>& gaussConvolve_2nd, 
	float chkWidth, float scale)
{
	double sigma = chkWidth / 1.7;
	std::vector<double> mask_hc;
	int mask_hc_size = compute_gauss_mask_1_scale(mask_hc, sigma, scale);
	if (mask_hc_size == 0)
		return;

	std::vector<double> mask_hcc;
	int mask_hcc_size = compute_gauss_mask_2_scale(mask_hcc, sigma, scale);
	if (mask_hcc_size == 0)
		return;

#if 1
	convolve_laserLine_gauss(a_line, startId, endId, mask_hc, mask_hc_size, gaussConvolve_1st);
	convolve_laserLine_gauss(a_line, startId, endId, mask_hcc, mask_hcc_size, gaussConvolve_2nd);
#else
	convolve_laserLine_gauss_scale(a_line, startId, endId, mask_hc, mask_hc_size, scale, gaussConvolve_1st);
	convolve_laserLine_gauss_scale(a_line, startId, endId, mask_hcc, mask_hcc_size, scale, gaussConvolve_2nd);
#endif

	return;
}