/*********************************************************************

PSE.C - 用FFT實(shí)現(xiàn)功率譜估計(jì)

程序輸入一個(gè)加入噪聲的正弦信號, 采用周期圖法來實(shí)現(xiàn)功率譜分析

參數(shù)如下:
 slice = 每次FFT的長度,缺省為256;
 numav = FFT取平均的組數(shù),缺省為16;
 ovlap = 每次FFT運(yùn)算交叉的點(diǎn)數(shù),缺省為128;
 wtype = 選用窗函數(shù)的類型: 1--5.
 ntype = 選用噪聲信號類型, 0:無噪聲; 1:均勻噪聲; 2:高斯噪聲

使用子程序?yàn)?
fft            基2 DIF FFT 子程序
ifft           基2 DIF IFFT 子程序
log2           基2求對數(shù)函數(shù)
addnoise       在輸入信號上加入均勻分布或高斯分布的噪聲
gaussian       產(chǎn)生均值為零,單位方差的高斯分布隨機(jī)數(shù)
uniform        產(chǎn)生零均值,均勻分布的隨機(jī)數(shù)(-0.5 到 0.5 之間)
ham            Hamming 窗函數(shù)
han            Hanning 窗函數(shù)
triang         triangle 窗函數(shù)
black          Blackman 窗函數(shù)
harris         4 term Blackman-Harris 窗函數(shù)
draw_image     繪圖子程序
***********************************************************************/

#include    <math.h>
#include    <stdlib.h>
#include    <stdio.h>
#include    <string.h>
#include    <conio.h>
#include    <graphics.h>

#define MAX(a,b)    (((a) > (b)) ? (a) : (b))
#define    PI	    (4.0*atan(1.0))

/* COMPLEX STRUCTURE */
typedef struct {
    float real, imag;
} COMPLEX;

 void fft(COMPLEX *,int);
 void ifft(COMPLEX *,int);
 int  log2(unsigned int x);
 double *addnoise(double *Indata,unsigned length,int type,double nmult);
 double gaussian(void);
 double uniform(void);
 void ham(COMPLEX *x, int n);
 void han(COMPLEX *x, int n);
 void triang(COMPLEX *,int n);
 void black(COMPLEX *x, int n);
 void harris(COMPLEX *x, int n);
 void draw_image(double *x,int m,char *title1,char *title2,
		char *xdis1,char *xdis2,int dis_type);
/*********************************************************************/

int numav=16;     /* 每次FFT的長度*/
int slice=256;    /* FFT取平均的組數(shù)*/
int ovlap=128;    /* 每次FFT運(yùn)算交叉的點(diǎn)數(shù) */
int wtype=5;      /* 選用窗函數(shù)的類型: 0--5 */
int ntype=2;      /* 選用噪聲信號類型, 0:無噪聲; 1:均勻噪聲; 2:高斯噪聲 */


void main(void)
{
  int        i, j, m, insize,index;
  int        k,numests,estsize;
  double     a,*mag, *sigfloat;
  double     tempflt;
  char	     title[80],tmp[20];
  COMPLEX    *samp;

  estsize = (slice-ovlap)*(numav-1) + slice;
  printf("The size of each estimate is: %d\n", estsize);
  sigfloat = (double *) calloc(estsize,sizeof(double));
  mag = (double *) calloc(slice,sizeof(double));
  samp = (COMPLEX *) calloc(slice,sizeof(COMPLEX));
  if (!mag || !samp){
    printf("\n  Memory allocation error.\n");
    exit(1);
  }
  /* input signal data series */
  for (i=0; i<estsize; i++)
      sigfloat[i] = cos(6.0*1.0*PI*(double)i*2.0/slice);
  for (i=0; i<estsize; i++)
      sigfloat[i] = sigfloat[i]+sin(35.0*1.0*PI*(double)i*2.0/slice);

  if(ntype==1)  addnoise(sigfloat,estsize,0,0.2); /* 加入均勻噪聲 */
  else if(ntype==2) addnoise(sigfloat,estsize,1,0.2); /* 加入高斯噪聲 */

  strcpy(title,"The Input Signal Data (x[n])");
  draw_image(sigfloat,estsize,title,"The Magnitude value","0",
	     itoa(estsize,tmp,10),0);

  /* calculate the periodical signal estimation value */
  for (k=0; k<slice; k++) mag[k] = 0;
  for (j=0; j<numav; j++){
     for (k=0; k<slice; k++){
	index = j*(slice-ovlap) + k;
	samp[k].real = sigfloat[index];
	samp[k].imag = 0;
     }
     switch (wtype) {
	case 1: ham(samp,slice);
		break;
	case 2: han(samp,slice);
		break;
	case 3: triang(samp,slice);
		break;
	case 4: black(samp,slice);
		break;
	case 5: harris(samp,slice);
		break;
	default:
		break;
     }

     m = log2(slice);
     fft(samp,m);
     a = (double) slice*slice;
     for (k=0; k<slice; k++){
	tempflt  = samp[k].real * samp[k].real;
	tempflt += samp[k].imag * samp[k].imag;
	tempflt = tempflt/a;
	mag[k] += tempflt;
     }
     printf("*");
  }

/*  Take log after averaging the magnitudes.  */

  for (k=0; k<slice/2; k++){
     mag[k] = mag[k]/numav;
     mag[k] = 10*log10(MAX(mag[k],1.e-14));
  }

  strcpy(title,"The Estimation Reault (X[m])");
  draw_image(mag,slice/2,title,"The Magnitude value (dB)","0",
	     "PI",0);

  free(mag);
  free(sigfloat);
  free(samp);
}

/************************************************************************
fft - 基2 DIF FFT 子程序

輸入?yún)?shù):
	 COMPLEX *x : FFT 輸入和輸出數(shù)據(jù)區(qū)指針;
	      int m : FFT 長度 ( length = 2^m );
輸出參數(shù):
	 輸出數(shù)據(jù)放在 x 所指的輸入數(shù)據(jù)區(qū).
	 無輸出參數(shù).

void fft(COMPLEX *x, int m)
*************************************************************************/

void fft(COMPLEX *x,int m)
{
    static COMPLEX *w;           /* used to store the w complex array */
    static int mstore = 0;       /* stores m for future reference */
    static int n = 1;            /* length of fft stored for future */

    COMPLEX u,temp,tm;
    COMPLEX *xi,*xip,*xj,*wptr;

    int i,j,k,l,le,windex;

    double arg,w_real,w_imag,wrecur_real,wrecur_imag,wtemp_real;

    if(m != mstore) {

/* free previously allocated storage and set new m */

	if(mstore != 0) free(w);
	mstore = m;
	if(m == 0) return;       /* if m=0 then done */

/* n = 2**m = fft length */

	n = 1 << m;
	le = n/2;

/* allocate the storage for w */

	w = (COMPLEX *) calloc(le-1,sizeof(COMPLEX));
	if(!w) {
	    printf("\nUnable to allocate complex W array\n");
	    exit(1);
	}

/* calculate the w values recursively */

	arg = 4.0*atan(1.0)/le;         /* PI/le calculation */
	wrecur_real = w_real = cos(arg);
	wrecur_imag = w_imag = -sin(arg);
	xj = w;
	for (j = 1 ; j < le ; j++) {
	    xj->real = (float)wrecur_real;
	    xj->imag = (float)wrecur_imag;
	    xj++;
	    wtemp_real = wrecur_real*w_real - wrecur_imag*w_imag;
	    wrecur_imag = wrecur_real*w_imag + wrecur_imag*w_real;
	    wrecur_real = wtemp_real;
	}
    }

/* start fft */

    le = n;
    windex = 1;
    for (l = 0 ; l < m ; l++) {
	le = le/2;

/* first iteration with no multiplies */

	for(i = 0 ; i < n ; i = i + 2*le) {
	    xi = x + i;
	    xip = xi + le;
	    temp.real = xi->real + xip->real;
	    temp.imag = xi->imag + xip->imag;
	    xip->real = xi->real - xip->real;
	    xip->imag = xi->imag - xip->imag;
	    *xi = temp;
	}

/* remaining iterations use stored w */

	wptr = w + windex - 1;
	for (j = 1 ; j < le ; j++) {
	    u = *wptr;
	    for (i = j ; i < n ; i = i + 2*le) {
		xi = x + i;
		xip = xi + le;
		temp.real = xi->real + xip->real;
		temp.imag = xi->imag + xip->imag;
		tm.real = xi->real - xip->real;
		tm.imag = xi->imag - xip->imag;
		xip->real = tm.real*u.real - tm.imag*u.imag;
		xip->imag = tm.real*u.imag + tm.imag*u.real;
		*xi = temp;
	    }
	    wptr = wptr + windex;
	}
	windex = 2*windex;
    }

/* rearrange data by bit reversing */

    j = 0;
    for (i = 1 ; i < (n-1) ; i++) {
	k = n/2;
	while(k <= j) {
	    j = j - k;
	    k = k/2;
	}
	j = j + k;
	if (i < j) {
	    xi = x + i;
	    xj = x + j;
	    temp = *xj;
	    *xj = *xi;
	    *xi = temp;
	}
    }
}

/**************************************************************************
log2 - 基2求對數(shù)函數(shù)

函數(shù)返回以2為底的輸入整數(shù)的對數(shù)值的整數(shù).
如果對數(shù)值位于兩個(gè)整數(shù)之間,則返回較大值.

int log2(unsigned int x)
*************************************************************************/

int log2(x)
    unsigned int x;
{
    unsigned int mask,i;

    if(x == 0) return(-1);      /* zero is an error, return -1 */

    x--;                        /* get the max index, x-1 */

    for(mask = 1 , i = 0 ; ; mask *= 2 , i++) {
	if(x == 0) return(i);   /* return log2 if all zero */
	x = x & (~mask);        /* AND off a bit */
    }
}

/**************************************************************************
ADDNOISE  - 在輸入信號上加入均勻分布或高斯分布的噪聲

輸入?yún)?shù):
	 double  *Indata: 輸入的未加噪聲的信號序列;
	 unsigned length: 輸入信號序列的長度;
	 int      type  : 加噪聲的類型, 0:均勻  1:高斯
	 double   nmult : 加入噪聲的幅度值系數(shù);

輸出參數(shù):
	 double  *Indata: 在輸入序列上加入噪聲后,仍然將數(shù)據(jù)
			  放在 *Indata 數(shù)據(jù)區(qū)中.
*************************************************************************/

double *addnoise(double *Indata,unsigned length,int type,double nmult)
{
    int i,j;
    double gaussian(),uniform();

/* add noise to record */
    if(type == 0)
	for(i = 0 ; i < length ; i++) Indata[i] += nmult*uniform();
    else
	for(i = 0 ; i < length ; i++) Indata[i] += nmult*gaussian();
    return(Indata);
}

/**************************************************************************
gaussian - 產(chǎn)生均值為零,單位方差的高斯分布隨機(jī)數(shù)

函數(shù)返回 double型零均值,單位方差的高斯分布隨機(jī)數(shù).
使用 Box-Muller 轉(zhuǎn)換方法,將一對均勻分布的隨機(jī)變量映射為一對
高斯隨機(jī)變量.

double gaussian()
*************************************************************************/

double gaussian(void)
{