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

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

#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 FWT is FIXED in the version*/                           
int fr[N_WAVE] ;  //the data to be transformed and the output transform
                                                    
//======================================================== 

void FWTfix16(void)
// This routine does foward transform only  
// Output and input are through the same array
//output is in sequency order -- see web links 
//Algorithm is from:
// Hugh Larson, IEEE transactions on Acoustics, Speech, and Signal
// Aug 1976, page 335-6
begin
    int a0,a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,a11,a12,a13,a14,a15;
    int b0,b1,b2,b3,b4,b5,b6,b7,b8,b9,b10,b11,b12,b13,b14,b15;
    int c0,c1,c2,c3,c4,c5,c6,c7,c8,c9,c10,c11,c12,c13,c14,c15; 
    //Stage I
    a0 = fr[0] + fr[1] ;
    a1 = fr[0] - fr[1] ; 
    a2 = fr[2] + fr[3] ;
    a3 = fr[2] - fr[3] ;
    a4 = fr[4] + fr[5] ;
    a5 = fr[4] - fr[5] ;
    a6 = fr[6] + fr[7] ;
    a7 = fr[6] - fr[7] ;
    a8 = fr[8] + fr[9] ;
    a9 = fr[8] - fr[9] ;
    a10= fr[10] + fr[11] ;
    a11= fr[10] - fr[11] ;
    a12= fr[12] + fr[13] ;
    a13= fr[12] - fr[13] ;
    a14= fr[14] + fr[15] ;
    a15= fr[14] - fr[15] ;  
    //Stage II
    b0 = a0 + a2 ;
    b1 = a1 - a3 ; 
    b2 = a0 - a2 ; 
    b3 = a1 + a3 ; 
    b4 = a4 + a6 ; 
    b5 = a5 - a7 ;
    b6 = a4 - a6 ; 
    b7 = a5 + a7 ;
    b8 = a8 + a10 ; 
    b9 = a9 - a11 ;
    b10= a8 - a10 ; 
    b11= a9 + a11 ;
    b12= a12 + a14 ; 
    b13= a13 - a15 ;
    b14= a12 - a14 ; 
    b15= a13 + a15 ; 
    //Stage III
    c0 = b0 + b4 ;
    c1 = b1 + b5 ; 
    c2 = b2 - b6 ;
    c3 = b3 - b7 ;
    c4 = b0 - b4 ;
    c5 = b1 - b5 ; 
    c6 = b2 + b6 ; 
    c7 = b3 + b7 ;
    c8 = b8 + b12 ;
    c9 = b9 + b13 ;
    c10= b10 - b14 ;
    c11= b11 - b15 ;
    c12= b8 - b12 ;
    c13= b9 - b13 ;
    c14= b10 + b14 ;
    c15= b11 + b15 ;
    //Stage IV 
    fr[0] = c0 + c8 ;
    fr[8] = c1 + c9 ;
    fr[4] = c2 + c10 ;
    fr[12]= c3 + c11 ;
    fr[2] = c4 - c12 ;
    fr[10]= c5 - c13 ;
    fr[6] = c6 - c14 ;
    fr[14]= c7 - c15 ;
    fr[1] = c0 - c8 ;
    fr[9] = c1 - c9 ;
    fr[5] = c2 - c10 ;
    fr[13]= c3 - c11 ;
    fr[3] = c4 + c12 ;
    fr[11]= c5 + c13 ;
    fr[7] = c6 + c14 ;
    fr[15]= c7 + c15 ;   
end


//========================================================        
//set up a test input and call FWT stuff

void main(void)
begin
    unsigned char v ; 
       
  //serial setop for debugging using printf, etc.     
  UCSRB = 0x18 ;
  UBRRL = 103 ;
  putsf("\r\nStarting...\r\n");  
  
    //input function for FWT  
    for (v=0; v<N_WAVE; v++)
    begin  
        //fr[v]=(v<N_WAVE/2)? 0x0100:0xff00 ; 
        //fr[v]=(v<N_WAVE/4 || (v>=N_WAVE/2 && v<N_WAVE/4*3))? 0x0100:0xff00 ;
        fr[v] = (v%2) ? 0x0100:0xff00 ; 
        //fr[v] = sin(2*pi*1*v/N_WAVE);
    end
    
    
    FWTfix16();
    
    for (v=0; v<N_WAVE; v++)
    begin
        printf(" %f\r\n", fix2float(fr[v]));
    end
       
    
    //============================================
    //time the FWT
    TCCR1B = 1 ;
    TCNT1 = 0;
    FWTfix16();
    TCCR1B = 0;
    printf("FWT16 cycles=%d\n\r",TCNT1)  ; 
    
    //============================================ 
    
  while(1);   
end