/*--- FftFilter.c --- ＦＦＴにより画像を大局補正する --------------------------
		（X_SIZE，Y_SIZEが２のべき乗の場合に限る）
                 井上誠喜他，1999を元に，田中が翻案，2002．
-----------------------------------------------------------------------------*/
#include	<stdio.h>
#include	<stdlib.h>
#include	<math.h>
#include	"Params.h"

#define	OPT		0	/*　OPT = 1　光学的ＤＦＴ（直流分が中央）	*/
				/*　OPT = 0　通常のＤＦＴ（直流分が左端）	*/

int fft2 (float a_rl[Y_SIZE][X_SIZE], float a_im[Y_SIZE][X_SIZE], int inv);
void fft1core(float a_rl[], float a_im[], int length, 
	int ex, float sin_tbl[], float cos_tbl[], float buf[]);
void cstb(int length, int inv, float sin_tbl[], float cos_tbl[]);
void birv(float a[], int length, int ex, float b[]);
void rvmtx1(float a[Y_SIZE][X_SIZE], float b[X_SIZE][Y_SIZE], 
	int xsize, int ysize);
void rvmtx2(float a[X_SIZE][Y_SIZE], float b[Y_SIZE][X_SIZE], 
	int xsize, int ysize);

void main( void )
{
	FILE	*fpr,*fpw;
	FILE	*fopen();
	int		image_in[Y_SIZE][X_SIZE];
	float  	*ar;		/* データ実数部（入出力兼用）*/
	float  	*ai;		/* データ虚数部（入出力兼用）*/
	float	*ff;		/* 空間周波数特性フィルタ　　*/
	float	data;
	long	i, j;

	ar = (float *)malloc((size_t)Y_SIZE*X_SIZE*sizeof(float));
	ai = (float *)malloc((size_t)Y_SIZE*X_SIZE*sizeof(float));
	ff = (float *)malloc((size_t)Y_SIZE*X_SIZE*sizeof(float));
	if ((ar == NULL) || (ai == NULL) || (ff == NULL)) {
		return(-1);
	}

	/* 原画像を読み込み，２次元ＦＦＴの入力データに変換する */
	fpw = fopen("denoised.gray","wb");
	fpr = fopen("noised.gray","rb");
	for (i = 0; i < Y_SIZE; i++) {
		for (j = 0; j < X_SIZE; j++) {
			fseek(fpr,X_SIZE*i + j,0);
			image_in[i][j] = (int)fgetc(fpr);
			ar[X_SIZE*i + j] = image_in[i][j];
			ai[X_SIZE*i + j] = 0.0;
		}
	}

	/* ２次元ＦＦＴを実行する */
	if (fft2((float (*)[X_SIZE])ar, (float (*)[X_SIZE])ai, 1) == -1) return -1;

	/* 指定周波数成分だけを通過させないフィルタを作る */
	for (i = 0; i < Y_SIZE; i++) {
		for(j = 0; j < X_SIZE; j++) {
			if ( (j==34||j==X_SIZE-34)||(i==65||i==Y_SIZE -65) )
				ff[X_SIZE*i + j] = 0.0;
			else 
				ff[X_SIZE*i + j] = 1.0;
		}
	}
	/* 原画像の周波数成分に対してフィルタ処理を行なう */
	for (i = 0; i < Y_SIZE; i++) {
		for (j = 0; j < X_SIZE; j++) {
			ar[X_SIZE*i + j] *= ff[X_SIZE*i + j];
			ai[X_SIZE*i + j] *= ff[X_SIZE*i + j];
		}
	}

	/* 逆ＦＦＴを実行し，フィルタ処理された周波数成分を画像に戻す */
	if (fft2((float (*)[X_SIZE])ar, (float (*)[X_SIZE])ai, -1) == -1)
		return(-1);

	/* 補正した画像データをファイルに格納する */
	for (i = 0; i < Y_SIZE; i++) {
		for (j = 0; j < X_SIZE; j++) {
			data = ar[X_SIZE*i + j];
			if (data > 255) data = 255;
			if (data <   0) data = 0;
			fputc(data,fpw);
		}
	}
	free(ar);
	free(ai);
	return 0;
	fclose(fpr);
	fclose(fpw);
}

/*--- fft2 --- ２次元ＦＦＴの実行 ---------------------------------------------
		（X_SIZE，Y_SIZEが２のべき乗の場合に限る）
	a_rl:	データ実数部（入出力兼用）
	a_im:	データ虚数部（入出力兼用）
	inv:	1: ＤＦＴ，-1: 逆ＤＦＴ
-----------------------------------------------------------------------------*/
int fft2 (float a_rl[Y_SIZE][X_SIZE], float a_im[Y_SIZE][X_SIZE], int inv)
{
	float	*b_rl;		/* データ転置作業用配列（実数部）*/
	float	*b_im;		/* データ転置作業用配列（虚数部）*/
	float	*hsin_tbl;	/* 水平用SIN計算用テーブル		*/
	float	*hcos_tbl;	/* 水平用COS計算用テーブル		*/
	float	*vsin_tbl;	/* 垂直用SIN計算用テーブル		*/
	float	*vcos_tbl;	/* 垂直用COS計算用テーブル		*/
	float	*buf_x;		/* 作業用バッファ（水平方向）	*/
	float	*buf_y;		/* 作業用バッファ（垂直方向）	*/
	int		i;

	b_rl = (float *)calloc((size_t)X_SIZE*Y_SIZE, sizeof(float));
	b_im = (float *)calloc((size_t)X_SIZE*Y_SIZE, sizeof(float));
	hsin_tbl = (float *)calloc((size_t)X_SIZE, sizeof(float));
	hcos_tbl = (float *)calloc((size_t)X_SIZE, sizeof(float));
	vsin_tbl = (float *)calloc((size_t)Y_SIZE, sizeof(float));
	vcos_tbl = (float *)calloc((size_t)Y_SIZE, sizeof(float));
	buf_x = (float *)malloc((size_t)X_SIZE*sizeof(float));
	buf_y = (float *)malloc((size_t)Y_SIZE*sizeof(float));
	if ((b_rl == NULL) || (b_im == NULL)
		|| (hsin_tbl == NULL) || (hcos_tbl == NULL)
		|| (vsin_tbl == NULL) || (vcos_tbl == NULL)
		|| (buf_x == NULL) || (buf_y == NULL)) {
		return -1;
	}
	cstb(X_SIZE, inv, hsin_tbl, hcos_tbl);	/* 水平用SIN,COSテーブル作成	*/
	cstb(Y_SIZE, inv, vsin_tbl, vcos_tbl);	/* 垂直用SIN,COSテーブル作成	*/
	/* 水平方向のＦＦＴ */
	for (i = 0; i < Y_SIZE; i++) {
		fft1core(&a_rl[(long)i][0], &a_im[(long)i][0],
					X_SIZE, X_EXP, hsin_tbl, hcos_tbl, buf_x);
	}
	/* ２次元データの転置 */
	rvmtx1((float (*)[X_SIZE])a_rl, (float (*)[X_SIZE])b_rl, X_SIZE, Y_SIZE);
	rvmtx1((float (*)[X_SIZE])a_im, (float (*)[X_SIZE])b_im, X_SIZE, Y_SIZE);
	/* 垂直方向のＦＦＴ */
	for (i = 0; i < X_SIZE; i++) {
		fft1core(&b_rl[(long)Y_SIZE*i], &b_im[(long)Y_SIZE*i], 
					Y_SIZE, Y_EXP, vsin_tbl, vcos_tbl, buf_y);
	}
	/* ２次元データの転置 */
	rvmtx2((float (*)[Y_SIZE])b_rl, (float (*)[Y_SIZE])a_rl, X_SIZE, Y_SIZE);
	rvmtx2((float (*)[Y_SIZE])b_im, (float (*)[Y_SIZE])a_im, X_SIZE, Y_SIZE);
	free(b_rl);
	free(b_im);
	free(hsin_tbl);
	free(hcos_tbl);
	free(vsin_tbl);
	free(vcos_tbl);
	return 0;
}

/*--- fft1core --- １次元ＦＦＴの計算の核になる部分 ---------------------------
	a_rl:	データ実数部（入出力兼用）
	a_im:	データ虚数部（入出力兼用）
	ex:		データ個数を２のべき乗で与える(データ個数 = 2のex乗個）
	sin_tbl:	SIN計算用テーブル
	cos_tbl:	COS計算用テーブル
-----------------------------------------------------------------------------*/
void fft1core(float a_rl[], float a_im[], int length, 
	int ex, float sin_tbl[], float cos_tbl[], float buf[])
{
	int		i, j, k, w, j1, j2;
	int		numb, lenb, timb;
	float   xr, xi, yr, yi, nrml;

	if (OPT == 1) {
		for (i = 1; i < length; i+=2) {
			a_rl[i] = -a_rl[i];
			a_im[i] = -a_im[i];
		}
	}
	numb = 1;
	lenb = length;
	for (i = 0; i < ex; i++) {
		lenb /= 2;
		timb = 0;
		for (j = 0; j < numb; j++) {
			w = 0;
			for (k = 0; k < lenb; k++) {
				j1 = timb + k;
				j2 = j1 + lenb;
				xr = a_rl[j1];
				xi = a_im[j1];
				yr = a_rl[j2];
				yi = a_im[j2];
				a_rl[j1] = xr + yr;
				a_im[j1] = xi + yi;
				xr = xr - yr;
				xi = xi - yi;
				a_rl[j2] = xr*cos_tbl[w] - xi*sin_tbl[w];
				a_im[j2] = xr*sin_tbl[w] + xi*cos_tbl[w];
				w += numb;
			}
			timb += (2*lenb);
		}
		numb *= 2;
	}
	birv(a_rl, length, ex, buf);		/*　実数データの並べ換え　*/
	birv(a_im, length, ex, buf);		/*　虚数データの並べ換え　*/
	if (OPT == 1) {
		for (i = 1; i < length; i+=2) {
			a_rl[i] = -a_rl[i];
			a_im[i] = -a_im[i];
		}
	}
	nrml = (float)(1.0 / sqrt((float)length));
	for (i = 0; i < length; i++) {
		a_rl[i] *= nrml;
		a_im[i] *= nrml;
	}
}

/*--- cstb --- SIN,COSテーブル作成 --------------------------------------------
	length:		データ個数
	inv:		1: ＤＦＴ, -1: 逆ＤＦＴ
	sin_tbl:	SIN計算用テーブル
	cos_tbl:	COS計算用テーブル
-----------------------------------------------------------------------------*/
void cstb(int length, int inv, float sin_tbl[], float cos_tbl[])
{
	int		i;
	float  	xx, arg;

	xx = (float)(((-PI) * 2.0) / (float)length);
	if (inv < 0) xx = -xx;
	for (i = 0; i < length; i++) {
		arg = (float)i * xx;
		sin_tbl[i] = (float)sin(arg);
		cos_tbl[i] = (float)cos(arg);
	}
}

/*--- birv --- データの並べ換え -----------------------------------------------
	a:		データの配列
	length:	データ個数
	ex:		データ個数を２のべき乗で与える(length = 2のex乗個）
	b:		作業用バッファ
-----------------------------------------------------------------------------*/
void birv(float a[], int length, int ex, float b[])
{
	int	i, ii, k, bit;

	for (i = 0; i < length; i++) {
		for (k = 0, ii=i, bit=0; ; bit<<=1, ii>>=1) {
			bit = (ii & 1) | bit;
			if (++k == ex) break;
		}
		b[i] = a[bit];
	}
	for (i = 0; i < length; i++) 
		a[i] = b[i];
}

/*--- rvmtx1 --- ２次元データの転置 -------------------------------------------
	a:		２次元入力データ
	b:		２次元出力データ
	xsize:	水平データ個数
	ysize:	垂直データ個数
-----------------------------------------------------------------------------*/
void rvmtx1(float a[Y_SIZE][X_SIZE], float b[X_SIZE][Y_SIZE], 
	int xsize, int ysize)
{
	int  i, j;

	for (i = 0; i < ysize; i++) {
		for (j = 0; j < xsize; j++)
			b[j][i] = a[i][j];
	}
}

/*--- rvmtx2 --- ２次元データの転置 -------------------------------------------
	a:		２次元入力データ
	b:		２次元出力データ
	xsize:	水平データ個数
	ysize:	垂直データ個数
-----------------------------------------------------------------------------*/
void rvmtx2(float a[X_SIZE][Y_SIZE], float b[Y_SIZE][X_SIZE], 
	int xsize, int ysize)
{
	int i, j;

	for (i = 0; i < ysize; i++) {
		for (j = 0; j < xsize; j++)
			b[i][j] = a[j][i];
	}
}