#include <mega32.h> 
#include <stdio.h> 
#include <stdlib.h> 
#include <math.h>

//I like these definitions
#define begin {
#define end   } 

#define int2fix(a)   (((int)(a))<<8) 
#define float2fix(a) ((int)((a)*256.0)) 
#define fix2float(a) ((float)(a)/256.0)  

#define N_WAVE          16    /* size of FFT */
#define LOG2_N_WAVE     4     /* log2(N_WAVE) */

//fft variables in registers
int wr,wi; 
int rr,ii,tr,ti; 
//sine table
int Sinewave[N_WAVE];          // table of sines for the FFT
int fr[N_WAVE], fi[N_WAVE];   //  fft in/out;

//==Fast fixed multiply=================================
#pragma warn-
int multfft(int a, int b)
begin
    #asm
    ;push r20
    ;push r21
    mov  r24,r20
    mov  r25,r21
    
    LDD  R22,Y+2  ;load a
	LDD  R23,Y+3 
	
	ld  r20,Y   ;load b
	ldd r21,Y+1 
	
	muls	r23, r21		; (signed)ah * (signed)bh
	mov		r31, r0         ;
	mul		r22, r20		; al * bl
	mov     r30, r1         ;
	                        ;mov     r16, r0
	mulsu	r23, r20		; (signed)ah * bl
	add		r30, r0         ;
	adc		r31, r1         ;
	mulsu	r21, r22		; (signed)bh * al
	add		r30, r0         ;
	adc		r31, r1         ;
	
	mov r20,r24
	mov r21,r25
	;pop r21
	;pop r20
    #endasm
end
#pragma warn+    

      
//======================================================== 

void FFTfix(int fr[], int fi[])
//Adapted from code by:
//Tom Roberts 11/8/89 and Malcolm Slaney 12/15/94 malcolm@interval.com
//fr[n],fi[n] are real,imaginary arrays, INPUT AND RESULT.
//size of data = 2**m 
// This routine does foward transform only
begin
    int ti;
    unsigned char m,mr,i,j,L,k,istep; 
    m = LOG2_N_WAVE;
    mr = 0;
    //decimation in time - re-order data 
    for(m=1; m<=N_WAVE-1; ++m) 
    begin
        L = N_WAVE;
        do L >>= 1; while(mr+L > N_WAVE-1);
        mr = (mr & (L-1)) + L;
        if(mr <= m) continue;
        tr = fr[m];
        fr[m] = fr[mr];
        fr[mr] = tr;
        fi[m] = 0;      //optimized for real inputs ONLY
    end
    
    L = 1;
    k = LOG2_N_WAVE-1;
    while(L < N_WAVE) 
    begin
        istep = L << 1;
        for(m=0; m<L; ++m) 
        begin
            j = m << k;
            wr =  Sinewave[j+N_WAVE/4] ;
            wi = -Sinewave[j] ;
            
            for(i=m; i<N_WAVE; i+=istep) 
            begin
                j = i + L; 
                rr=fr[j]; ii=fi[j];
                
                // get rid of multiply by zero in:
               // tr = multfix(wr,rr) - multfix(wi,ii);
               // ti = multfix(wr,ii) + multfix(wi,rr); 
                if (rr==0) 
                begin
                    tr = 0;
                    ti = 0;
                end
                else 
                begin
                    tr = multfft(wr,rr); 
                    ti = multfft(wi,rr);
                end
                //
                if (ii!=0) 
                begin
                    tr = tr - multfft(wi,ii);
                    ti = ti + multfft(wr,ii);
                end 
                 
                rr = fr[i] ;//>> 1;
                ii = fi[i] ;//>> 1;     
                fr[j] = rr - tr;
                fi[j] = ii - ti;
                fr[i] = rr + tr;
                fi[i] = ii + ti;   
            end
        end
        --k;
        L = istep;
    end
end

//========================================================         

void main(void)
begin 
  unsigned char i ;  
  //serial setop for debugging using printf, etc.     
  UCSRB = 0x18 ;
  UBRRL = 103 ;
  putsf("\r\nStarting...\r\n");  
  
  //===FFT setup===============================
  //set up one cycle sine table for FFT
  //and init bitrev
  for (i=0; i<N_WAVE; i++)
  begin
    Sinewave[i] = float2fix(sin(6.283*((float)i)/N_WAVE)/1.0);
  end
  //============================================
      
  while(1)
  begin
    
    //input function for FFT  
    for (i=0; i<N_WAVE; i++)
    begin
        fr[i]=float2fix(sin(6.283*4*((float)i)/N_WAVE)); fi[i]=0;
    end
    
    FFTfix(fr, fi);
    
    for (i=0; i<N_WAVE; i++)
    begin
        printf("%4x %f %f\r\n", 
            (Sinewave[i]), fix2float(fr[i]), fix2float(fi[i]));
    end
   
    
    //============================================
    //time the FFT
    TCCR1B = 1 ;
    TCNT1 = 0;
    FFTfix(fr, fi);
    TCCR1B = 0;
    printf("FFT cycles=%d\n\r",TCNT1)  ;
    
    //============================================
  while(1);   
  end
end