#include "../config.h"
int pow2(int num, int power){
int result=1;
for(;power>0;power--) result=num*result;
return result;
}
int byte_reverse(int byte_n, int old_n){
int new_n=0,i,move;
move=byte_n-1;
for(i=0;i<byte_n;i++){
if(move>=0) new_n=new_n |(old_n>>move & 0x01<<i);
else new_n=new_n |(old_n<<-move & 0x01<<i);
move=move-2;
}
return new_n;
}
void get_fft(int bias, int *wave ){
int n=FFT_N,L=FFT_POWER;
int a,b,c=0,p,q,xp,xq;
float *wr=(float *)OFFSET;
float *wi=(float *)OFFSET+sizeof(float)*FFT_N;
float *tr=(float *)OFFSET+2*sizeof(float)*FFT_N;
float *ti=(float *)OFFSET+3*sizeof(float)*FFT_N;
float max,rate;
float nsin,ncos;
// int r[(int)pow(2,FFT_POWER-1)];
print_LCD(2,0,0,n);
for(a=0;a<n;a++){
tr[byte_reverse(FFT_POWER,a)]=wave[a]-bias;
ti[a]=0;
}
// for(a=0;a<n;a++) {print_LCD(1,40*a,60,wave[a]); print_LCD(1,40*a,80,tr[a]);}
for(a=1;a<=L;a++){//a=1:L
print_LCD(2,200,40,a);
p=pow2(2,a-1);
// print_LCD(1,40*a,20,p);
// print_LCD(1,40,40,pow(2,5));
q=2*p;
for(b=0;b<n;b=b+q){
for(c=0;c<p;c++){
xp=b+c;
xq=b+c+p;
nsin=sin(PI2*c/q);
ncos=cos(PI2*c/q);
// print_LCD(1,40*c,40+20*a,r);
wr[xp]=tr[xp]+tr[xq]*ncos+ti[xq]*nsin;
wi[xp]=ti[xp]-tr[xq]*nsin+ti[xq]*ncos;
wr[xq]=tr[xp]-tr[xq]*ncos-ti[xq]*nsin;
wi[xq]=ti[xp]+tr[xq]*nsin-ti[xq]*ncos;
}
}
for(b=0;b<n;b++){
tr[b]=wr[b];
ti[b]=wi[b];
// if(b<8){print_LCD(1,40*b,40*a,tr[b]);
// print_LCD(1,40*b,20+40*a,ti[b]);}
}
}
b=0;
wave[0]=0;
for(a=1;a<n;a++){
wr[a]=sqrt(wr[a]*wr[a]+wi[a]*wi[a]);
if(a==0) max=wr[a];
else if(max<wr[a]){ max=wr[a]; p=(a<FFT_N/2)?a:FFT_N-a;}
}
rate=max/1024;
for(a=1;a<n;a++) wave[a]=wr[a]/rate;
print_LCD(1,16,20,p);
// print_LCD(1,16,40,(int)max/rate);
// if (wave[a]<0) wave[a]=9998;}
/* if(temp>65535) wave[a]=65535;
// else wave[a]=(int)temp;
*/
/* for(a=0;a<8;a++){
print_LCD(2,40*a,60,wr[a]);
print_LCD(2,40*a,80,wi[a]);
print_LCD(2,40*a,140,wave[a]);
}
for(a=8;a<16;a++){
print_LCD(2,40*a,100,wr[a]);
print_LCD(2,40*a,120,wi[a]);
print_LCD(2,40*a,160,wave[a]);
}
*/
}
void disp_fft(int* wave){
int i,y;
for(i=0;i<320;i++){
y=205-wave[i]/5;
LCD_line(0,i,y,i,205,1);
}
delay_nms(20);
get_fft(0, wave);
LCD_clear_layer(0);
for(i=0;i<FFT_N/2-1;i++){
y=205-wave[i]/5;
if (y<0) {y=0;print_LCD(2,(i/8)*8,20,wave[i]);}
LCD_line(0,i,y,i,205,1);
}
}
