/***********************************************************************
FASTCOR.C - 使用FFT實現快速相關

該程序利用FFT實現快速相關運算.
輸入序列為兩個具有相關性的信號,一個輸入信號
為兩個不同頻率的信號迭加而成,另一個輸入信號
為其中一個頻率的單頻信號.若兩個輸入信號相同
則為自相關.

使用子程序為:
fft            基2 DIF FFT 子程序
ifft           基2 DIF IFFT 子程序
log2           基2求對數函數
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);
 void draw_image(double *x,int m,char *title1,char *title2,
		char *xdis1,char *xdis2,int dis_type);
/********************************************************/
void main(void)
{
  unsigned int i, length1,length2,m,fft_length;
  char	       title[80],tmp[20];
  double       *signal;
  double       a,tempflt;
  COMPLEX      *samp, *filt;

/* Set the input data specified by the user */

  length1 = 300;     /* 信號1 長度 */
  length2 = 50;      /* 信號2 長度 */
  m = log2(2*MAX(length1,length2));
  fft_length = 1<<m;
  signal = (double *) calloc(fft_length,sizeof(double));
  if(!signal){
    printf("\n  Could not allocate sample memory.\n");
    exit(1);
  }

  for (i = 0; i < length1; i++) {
      signal[i] = cos(6.0*1.0*PI*(double)i*2.0/length1);
      printf("*");
  }
  for (i = 0; i < length1; i++) {
     signal[i] = signal[i]+cos(138.0*PI*(double)i*2.0/length1);
     printf("*");
  }

  strcpy(title,"The Input Signal Data (x1[n])");
  draw_image(signal,length1,title,"The Magnitude value","0",
	     itoa(length1,tmp,10),0);

  samp = (COMPLEX *) calloc(fft_length, sizeof(COMPLEX));
  if(!samp){
    printf("\n  Could not allocate sample memory.\n");
    exit(1);
  }

  for (i = 0; i < length1; i++) samp[i].real = signal[i];

  fft(samp,m);

/* Display the log magnitude of the FFT */
  a = (double)length1*length1;
  for (i = 0; i < fft_length; i++) {
    tempflt  = samp[i].real * samp[i].real;
    tempflt += samp[i].imag * samp[i].imag;
    tempflt = tempflt/a;
    signal[i] = 10 * log10(MAX(tempflt,1.e-10));
  }

  strcpy(title,"The Input Data FFT Result X1[m]");
  draw_image(signal,fft_length,title,"The Magnitude (dB)","0",
	     itoa(fft_length,tmp,10),0);

/* Zero fill the filter to the sequence length */
  filt = (COMPLEX *) calloc(fft_length, sizeof(COMPLEX));
  if(!filt){
    printf("\n  Could not allocate filter memory.\n");
    exit(1);
  }

  for (i = 0; i < length2; i++) signal[i] = 0;
  for (i = 0; i < length2; i++) {
      signal[i] = cos(1.0*PI*(double)i*2.0/length2);
      filt[i].real = signal[i];
      printf("*");
  }

  strcpy(title,"The Input Data (x2[n])");
  draw_image(signal,length2,title,"The Magnitude value","0",
	     itoa(length2,tmp,10),0);

/* FFT the zero filled filter impulse response */

  fft(filt,m);

  a = (double)length2*length2;
  for (i = 0; i < fft_length; i++) {
    tempflt  = filt[i].real * filt[i].real;
    tempflt += filt[i].imag * filt[i].imag;
    tempflt = tempflt/a;
    signal[i] = 10 * log10(MAX(tempflt,1e-10));
  }

  strcpy(title,"The Input Data FFT Result X2[m]");
  draw_image(signal,fft_length,title,"The Magnitude (dB)","0",
	     itoa(fft_length,tmp,10),0);

/* Multiply the two transformed sequences */

  for (i = 0; i < fft_length; i++) {
    tempflt = samp[i].real * filt[i].real
			   + samp[i].imag * filt[i].imag;
    samp[i].imag = samp[i].imag * filt[i].real
			   - samp[i].real * filt[i].imag;
    samp[i].real = tempflt;
  }

/* Inverse fft the multiplied sequences */

  ifft(samp,m);

/* Display the result */
  for (i = 0; i < length1+length2; i++) signal[i] = samp[i].real;

  strcpy(title,"The Result of Correlation");
  draw_image(signal,length1+length2,title,"The Magnitude Value","0",
	     itoa(length1+length2,tmp,10),0);
  free(signal);
  free(samp);
  free(filt);
}

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

輸入參數:
	 COMPLEX *x : FFT 輸入和輸出數據區指針;
	      int m : FFT 長度 ( length = 2^m );
輸出參數:
	 輸出數據放在 x 所指的輸入數據區.
	 無輸出參數.

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;
	}
    }
}

/************************************************************************
ifft - 基2 DIF IFFT 子程序

輸入參數:
	 COMPLEX *x : IFFT 輸入和輸出數據區指針;
	      int m : IFFT 長度 ( length = 2^m );
輸出參數:
	 輸出數據放在 x 所指的輸入數據區.
	 無輸出參數.

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