#include "ImgDenoise.h"

#define FILTER_PI 3.14159  
#pragma once


/*====================================================================
函数名  ：SurfaceFilter
功能    ：降噪-模糊滤波
输入参数说明：
srcImgData：输入数据
dstImgData：输出数据
imgwidth：图像宽
imgheight：图像高
channels：图像通道数
radius：滤波半径
threshold：阈值
====================================================================*/
#define MIN2(a, b) ((a) < (b) ? (a) : (b))  
#define MAX2(a, b) ((a) > (b) ? (a) : (b))  
#define CLIP3(x, a, b) MIN2(MAX2(a,x), b)  
extern "C" __declspec(dllexport) bool UltrasoundImageFilter(const char* srcImgData, char* dstImgData, int imgwidth, int imgheight, int channels, int radius, int threshold)
{
	try
	{
		// 检查通道数
		if (channels != 3 && channels != 4)
		{
			return false;
		}
		// 检查图像尺寸
		if (imgwidth <= 0 || imgheight <= 0)
		{
			return false;
		}

		Mat src, dst;
		if (channels == 4)
		{
			src.create(imgheight, imgwidth, CV_8UC4);
			memcpy(src.data, srcImgData, imgwidth * imgheight * channels);

			dst = src.clone();

			float sumr = 0, sumrw = 0, sumg = 0, sumgw = 0, sumb = 0, sumbw = 0, k = 0;

			for (int j = 0; j < src.rows; j++)
			{
				for (int i = 0; i < src.cols; i++)
				{
					sumr = sumrw = sumg = sumgw = sumb = sumbw = 0;
					for (int n = -radius; n <= radius; n++)
					{
						for (int m = -radius; m <= radius; m++)
						{
							int x = CLIP3(i + m, 0, imgwidth - 1);
							int y = CLIP3(j + n, 0, imgheight - 1);

							k = 1.0f - abs(src.at<Vec4b>(y, x)[0] - src.at<Vec4b>(j, i)[0]) / (2.5f * threshold);
							if (k <= 0)
								k = 0.01;
							sumb += k * src.at<Vec4b>(y, x)[0];
							sumbw += k;

							k = 1.0f - abs(src.at<Vec4b>(y, x)[1] - src.at<Vec4b>(j, i)[1]) / (2.5f * threshold);
							if (k <= 0)
								k = 0.01;
							sumg += k * src.at<Vec4b>(y, x)[1];
							sumgw += k;

							k = 1.0f - abs(src.at<Vec4b>(y, x)[2] - src.at<Vec4b>(j, i)[2]) / (2.5f * threshold);
							if (k <= 0)
								k = 0.01;
							sumr += k * src.at<Vec4b>(y, x)[2];
							sumrw += k;
						}
					}

					dst.at<Vec4b>(j, i)[0] = sumbw == 0 ? src.at<Vec4b>(j, i)[0] : CLIP3(sumb / sumbw, 0, 255);
					dst.at<Vec4b>(j, i)[1] = sumgw == 0 ? src.at<Vec4b>(j, i)[1] : CLIP3(sumg / sumgw, 0, 255);
					dst.at<Vec4b>(j, i)[2] = sumrw == 0 ? src.at<Vec4b>(j, i)[2] : CLIP3(sumr / sumrw, 0, 255);
				}
			}

		}
		else
		{
			src.create(imgheight, imgwidth, CV_8UC3);
			memcpy(src.data, srcImgData, imgwidth * imgheight * channels);

			dst = src.clone();

			float sumr = 0, sumrw = 0, sumg = 0, sumgw = 0, sumb = 0, sumbw = 0, k = 0;

			for (int j = 0; j < src.rows; j++)
			{
				for (int i = 0; i < src.cols; i++)
				{
					sumr = sumrw = sumg = sumgw = sumb = sumbw = 0;
					for (int n = -radius; n <= radius; n++)
					{
						for (int m = -radius; m <= radius; m++)
						{
							int x = CLIP3(i + m, 0, imgwidth - 1);
							int y = CLIP3(j + n, 0, imgheight - 1);

							k = 1.0f - abs(src.at<Vec3b>(y, x)[0] - src.at<Vec3b>(j, i)[0]) / (2.5f * threshold);
							if (k <= 0)
								k = 0.01;
							sumb += k * src.at<Vec3b>(y, x)[0];
							sumbw += k;

							k = 1.0f - abs(src.at<Vec3b>(y, x)[1] - src.at<Vec3b>(j, i)[1]) / (2.5f * threshold);
							if (k <= 0)
								k = 0.01;
							sumg += k * src.at<Vec3b>(y, x)[1];
							sumgw += k;

							k = 1.0f - abs(src.at<Vec3b>(y, x)[2] - src.at<Vec3b>(j, i)[2]) / (2.5f * threshold);
							if (k <= 0)
								k = 0.01;
							sumr += k * src.at<Vec3b>(y, x)[2];
							sumrw += k;
						}
					}

					dst.at<Vec3b>(j, i)[0] = sumbw == 0 ? src.at<Vec3b>(j, i)[0] : CLIP3(sumb / sumbw, 0, 255);
					dst.at<Vec3b>(j, i)[1] = sumgw == 0 ? src.at<Vec3b>(j, i)[1] : CLIP3(sumg / sumgw, 0, 255);
					dst.at<Vec3b>(j, i)[2] = sumrw == 0 ? src.at<Vec3b>(j, i)[2] : CLIP3(sumr / sumrw, 0, 255);
				}
			}
		}

		memcpy(dstImgData, dst.data, imgwidth * imgheight * channels);

		return true;
	}
	catch (...)
	{
		return false;
	}
}