/**********************************************************************
ZFFT.C -   ZOOM FFT 演示程序
fft        FFT 子程序
ifft       IFFT 子程序
***********************************************************************/

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

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

#define    PI	(4.0*atan(1.0))

 void fft(COMPLEX *,int);
 void ifft(COMPLEX *,int);

/********************************************************/
void main(void)
{
  int          i,length,m,j;
  char         title[80],tmp[20],dis[40];
  double       *amp;
  double       a,tempflt;
  COMPLEX      *samp,*rsamp;

  int gdriver=DETECT, gmode,errorcode;
  int scx,scy,y0,signa,signb;
  int style, userpat;
  int start_x=60,start_y1=20,start_y2,end_x=10,end_y=40;
  long tlen;
  double ys,xs,ym;

  int N = 256;           /* FFT 點數 */
  int M = 512;           /* ZFFT 點數 */
  int A = 5;             /* ZFFT 放大倍數 */
  int start_F = 15;      /* ZFFT 開始點 */
  int B0,B;

  B = N/A/2;
  B0 = start_F+B;

 /*initializes the graphics mode */
  initgraph(&gdriver,&gmode,"");
  errorcode=graphresult();
  if (errorcode != grOk) {
     printf("Graphics error: %s\n",grapherrormsg(errorcode));
     printf("Press any key to halt!\n");
     getch();
     exit(1);
  }
  if(N>M) {
     amp = (double *) calloc(N+1,sizeof(double));
     samp = (COMPLEX *) calloc(N+1,sizeof(COMPLEX));
     rsamp = (COMPLEX *) calloc(N+1,sizeof(COMPLEX));
  }
  else {
     amp = (double *) calloc(M+1,sizeof(double));
     samp = (COMPLEX *) calloc(M+1,sizeof(COMPLEX));
     rsamp = (COMPLEX *) calloc(M+1,sizeof(COMPLEX));
  }
  if(!rsamp) {
      printf("\nUnable to allocate complex array for fft\n");
      exit(1);
  }

  scx=getmaxx();
  scy=getmaxy();
  start_y2=scy/2;

  /* draw the frame */
  setcolor(LIGHTGREEN);
  rectangle(start_x-1,start_y1-20,scx-end_x+1,start_y2-end_y+1);
  rectangle(start_x-1,start_y2-20,scx-end_x+1,scy-end_y+1);
  setcolor(GREEN);
  style=SOLID_LINE;
  userpat = 1;
  setlinestyle(style, userpat, 1);
  rectangle(start_x,start_y1,scx-end_x,start_y2-end_y);
  rectangle(start_x,start_y2,scx-end_x,scy-end_y);

  settextstyle(DEFAULT_FONT,HORIZ_DIR,1);
  setcolor(YELLOW);
  strcpy(dis,"0");
  outtextxy(start_x-10,start_y2-end_y+3,dis);
  itoa(N,dis,10);
  outtextxy(scx-end_x+5-strlen(dis)*8,start_y2-end_y+3,dis);
  strcpy(dis,"0");
  outtextxy(start_x-10,scy-end_y+3,dis);
  itoa(M,dis,10);
  outtextxy(scx-end_x+5-strlen(dis)*8,scy-end_y+3,dis);

  settextstyle(DEFAULT_FONT,VERT_DIR,1);
  strcpy(title,"The Magnitude of Spectrum");
  tlen=strlen(title);
  if ((tlen<<3)<scy) {
     setcolor(YELLOW);
     outtextxy(start_x-20,(scy-end_y-(tlen<<3))>>1,title);
  }

  settextstyle(DEFAULT_FONT,HORIZ_DIR,1);
  strcpy(title,"The Input Signal Spectrum(FFT)");
  tlen=strlen(title);
  if ((tlen<<3)<scx) {
    setcolor(LIGHTGREEN);
    outtextxy((start_x+scx-end_x-(tlen<<3))>>1,start_y1-13,title);
  }

  strcpy(title,"The Spectrum of ZFFT Result A=");
  itoa(A,dis,10);
  strcat(title,dis);
  tlen=strlen(title);
  if ((tlen<<3)<scx) {
    setcolor(LIGHTGREEN);
    outtextxy((start_x+scx-end_x-(tlen<<3))>>1,start_y2-13,title);
  }

  /* Input sampling data for processing */
  settextstyle(DEFAULT_FONT,HORIZ_DIR,1);
  strcpy(title,"Waitting for the calculation...");
  tlen=strlen(title);
  if ((tlen<<3)<scx) {
    setcolor(LIGHTRED);
    outtextxy((start_x+scx-end_x-(tlen<<3))>>1,start_y2/2,title);
  }

  /* Input signal sample */
  for (i = 0; i < N; i++)  {
     samp[i].real = 0;
     samp[i].imag = 0;
  }
  for (i = 0; i < N/8; i++)  {
      samp[i].real=1;
      samp[N/2+i].real=1;
      samp[N/4+i].real=1;
      samp[N*3/4+i].real=1;
  }

  /* Calculate the input data's spectrum */
  fft(samp,log(N)/log(2));

  for (i = 0; i < N; i++)
     amp[i] = sqrt(samp[i].real*samp[i].real+samp[i].imag*samp[i].imag);

  /* Display the spectrum curve */
  ym = 1.0e-90;
  for(i = 0; i < N; i++)
     if (amp[i] > ym)  ym = amp[i];

  ys=(double)(start_y2-end_y-start_y1)/ym;
  xs=(double)(scx - start_x - end_x)/N;
  y0=start_y2-end_y;

  setfillstyle(EMPTY_FILL,BLACK);
  bar(start_x+1,start_y1+1,scx-end_x-1,start_y2-end_y-1);

  setcolor(DARKGRAY);
  style=DASHED_LINE;
  userpat = 1;
  setlinestyle(style, userpat, 1);
  for(i=0;i<=10;i++)
    line(start_x,start_y1+(start_y2-start_y1-end_y)*i/10,scx-end_x,start_y1+(start_y2-start_y1-end_y)*i/10);
  for(i=0;i<=10;i++)
    line(start_x+(scx-start_x-end_x)*i/10,start_y1,start_x+(scx-start_x-end_x)*i/10,start_y2-end_y);

  style=SOLID_LINE;
  setlinestyle(style, userpat, 1);
  setcolor(LIGHTMAGENTA);
  for(i=0;i<N-1;i++)
    line(xs*i+start_x,y0-amp[i]*ys,xs*(i+1)+start_x,y0-amp[i+1]*ys);

  gcvt(ym,8,dis);
  outtextxy(start_x+3,start_y1-10,dis);

  setcolor(WHITE);
  setwritemode(XOR_PUT);
  line(xs*(B0-B)+start_x,start_y1,xs*(B0-B)+start_x,y0);
  line(xs*(B0+B)+start_x,start_y1,xs*(B0+B)+start_x,y0);
  setwritemode(COPY_PUT);

  /* Input resampling data for processing */
  settextstyle(DEFAULT_FONT,HORIZ_DIR,1);
  strcpy(title,"Waitting for the calculation...");
  tlen=strlen(title);
  if ((tlen<<3)<scx) {
    setcolor(LIGHTRED);
    outtextxy((start_x+scx-end_x-(tlen<<3))>>1,start_y2+start_y2/2,title);
  }

  for (i = 0; i < N; i++)  {
     rsamp[i].real = 0;
     rsamp[i].imag = 0;
  }
  for (i = B0; i < B0+B; i++) {
     rsamp[(i-B0)].real = samp[i].real;
     rsamp[(i-B0)].imag = samp[i].imag;
  }

  for (i = B0; i > B0-B; i--) {
     rsamp[(N-B0+i)].real = samp[i].real;
     rsamp[(N-B0+i)].imag = samp[i].imag;
  }

  /* Calculate the input data spectrum */
  ifft(rsamp,log(N)/log(2));

  for (i = 0; i < M; i++) {
     if (i*A < N) {
       samp[i].real = rsamp[i*A].real;
       samp[i].imag = rsamp[i*A].imag;
     }
     else {
       samp[i].real = 0;
       samp[i].imag = 0;
     }
  }

  fft(samp,log(M)/log(2));

  /* Trim the output order */
  for (i = 0; i < M/2; i++)  {
     rsamp[i].real = samp[i+M/2].real;
     rsamp[i].imag = samp[i+M/2].imag;
     rsamp[i+M/2].real = samp[i].real;
     rsamp[i+M/2].imag = samp[i].imag;
  }
  for (i = 0; i < M; i++)
     amp[i] = (sqrt(rsamp[i].real*rsamp[i].real+rsamp[i].imag*rsamp[i].imag))*A;

  ym = 1.0e-90;
  for(i = 0; i < M; i++)
     if (amp[i] > ym)  ym = amp[i];

  ys=(double)(start_y2-end_y-start_y1)/ym;
  xs=(double)(scx - start_x - end_x)/N;
  y0=start_y2-end_y;

  ym = 1.0e-90;
  for(i = 0; i < M; i++)
     if (amp[i] > ym)  ym = amp[i];

  ys=(double)(scy - end_y - start_y2)/ym;
  xs=(double)(scx - start_x - end_x)/M;
  y0=scy-end_y;

  setfillstyle(EMPTY_FILL,BLACK);
  bar(start_x+1,start_y2+1,scx-end_x-1,scy-end_y-1);

  setcolor(DARKGRAY);
  style=DASHED_LINE;
  userpat = 1;
  setlinestyle(style, userpat, 1);
  for(i=0;i<=10;i++)
    line(start_x,start_y2+(scy-start_y2-end_y)*i/10,scx-end_x,start_y2+(scy-start_y2-end_y)*i/10);
  for(i=0;i<=10;i++)
    line(start_x+(scx-start_x-end_x)*i/10,start_y2,start_x+(scx-start_x-end_x)*i/10,scy-end_y);

  style=SOLID_LINE;
  setlinestyle(style, userpat, 1);
  setcolor(LIGHTMAGENTA);
  for(i=0;i<M-1;i++)
    line(xs*i+start_x,y0-amp[i]*ys,xs*(i+1)+start_x,y0-amp[i+1]*ys);

  gcvt(ym,8,dis);
  outtextxy(start_x+3,start_y2-10,dis);

  strcpy(dis,"Press any key to quit...");
  setcolor(LIGHTRED);
  outtextxy((scx-28*8)>>1,scy-16,dis);

  getch();
  closegraph();
  free(samp);
  free(rsamp);
  free(amp);
}

/************************************************************************
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)
*************************************************************************/
void ifft(x,m)
    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 ifft 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;
    float scale;

    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 = inverse 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);  /* opposite sign from fft */
	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 inverse 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;
	}
    }

/* scale all results by 1/n */
    scale = (float)(1.0/n);
    for(i = 0 ; i < n ; i++) {
	x->real = scale*x->real;
	x->imag = scale*x->imag;
	x++;
    }
}