#include <cmath>
#include <iostream>
#include "abaxCommon.h"


/* 寻找中值 */
static float getMidNum(float *nums, int len) {
	int i, j;
	int minIdx;
	float tmp;
	for (i = 0; i < len / 2 + 1; i++) {
		minIdx = i;
		for (j = i; j < len; j++) {
			if (*(nums+j) < *(nums+minIdx)) {
				minIdx = j;
			}
		}
		tmp = *(nums+i);
		*(nums+i) = *(nums+minIdx);
		*(nums+minIdx) = tmp;
	}
	return nums[i - 1];
}

/* 图像中值滤波 */
int midNumFilter(float *src, float *dst, int width, int height, int winSize)
{
	int border = winSize / 2;
	float *nums = new float[winSize * winSize];
	int offset, offsetw;
	int i, j, m, n, k;
	float result;
	
	for (i = border; i < height - border - 1; i++) {
		offset = i * width;
		for (j = border; j < width - border; j++) {
			k = 0;
			for (m = i - border; m < i + border + 1; m++) {
				offsetw = m * width;
				for (n = j - border; n < j + border + 1; n++) {
					*(nums+k) = *(src+offsetw + n);
					k++;
				}
			}

			result = getMidNum(nums, k);
			*(dst+offset + j) = result;
		}
	}

	for(i = 0;i<border;i++)
	{
		offset = i * width; 
		offsetw = (height -i-1) * width;
		for(j=0;j < width; j++)
		{
            *(dst+offset +j) = *(src+offset+j);
			*(dst+ offsetw +j) = *(src+offsetw+j); 
		}
		
	} 
	
	float (*pfs)[width], (*pfd)[width];
    m = height - border -1;
    pfd = (float (*)[width])(dst);
    pfs = (float (*)[width])(src);
	for(i=border;i<m;i++)
	{
		for(j=0;j<border;j++)
		{
           *(*(pfd+i)+j) = *(*(pfs +i)+j);
		   *(*(pfd+i)+width -1 -j) = *(*(pfs +i)+width -1 -j); 
		}
        
	}

	delete[] nums;
	return 0;
}


void badPicPixReplace(float *dst, double *posMap, int pixNum)
{
    int i; 
	int x,y, xr, yr;
    double (*pos)[pixNum] = (double (*)[pixNum]) posMap;
    for(i=0;i<pixNum;i++)
	{
		y = *(*(pos)+i)-1;
		x = *(*(pos +1)+i)-1;
		yr = *(*(pos +2)+i)-1;
		xr = *(*(pos +3)+i)-1; 
		*(dst+y*TOF_WIDTH+x) = *(dst+yr*TOF_WIDTH + xr);
	} 

}