#include <mega32.h> 
#include <stdio.h> 
#include <stdlib.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 FWT */
#define LOG2_N_WAVE     4     /* log2(N_WAVE) */
int fwt_t1, fwt_t2; 


flash char reorder8[8] =   {0,7,3,4,1,6,2,5}; 
flash char reorder16[16] = {0,15,7,8,3,12,4,11,1,14,6,9,2,13,5,10};
flash char reorder32[32] = {0,31,15,16,7,24,8,23,3,28,12,19,4,27,11,
                            20,1,30,14,17,6,25,9,22,2,29,13,18,5,26,10,21};
flash char reorder64[64] = {0,63,31,32,15,48,16,47,7,56,24,39,8,55,23,40,
                            3,60,28,35,12,51,19,44,4,59,27,36,11,52,20,43,
                            1,62,30,33,14,49,17,46,6,57,25,38,9,54,22,41,2,
                            61,29,34,13,50,18,45,5,58,26,37,10,53,21,42}; 
flash char reorder128[128] = {0,127,63,64,31,96,32,95,15,112,48,79,16,111,
                                47,80,7,120,56,71,24,103,39,88,8,119,55,72,
                                23,104,40,87,3,124,60,67,28,99,35,92,12,115,
                                51,76,19,108,44,83,4,123,59,68,27,100,36,91,
                                11,116,52,75,20,107,43,84,1,126,62,65,30,97,
                                33,94,14,113,49,78,17,110,46,81,6,121,57,70,
                                25,102,38,89,9,118,54,73,22,105,41,86,2,125,
                                61,66,29,98,34,93,13,114,50,77,18,109,45,82,5,
                                122,58,69,26,101,37,90,10,117,53,74,21,106,42,85};                            
                                                    
//======================================================== 

void FWTfix(int x[])
// This routine does foward transform only
//output is in dyadic order -- see web links
//use FWTreorder() to put the transform in sequency order 
begin
    unsigned char ii, k, j, i, bb, st;
    
    bb = N_WAVE/2; st = N_WAVE; 
    //
    for (i=1; i<LOG2_N_WAVE+1; i++)
    begin
        for (j=0; j<N_WAVE; j+=st)
        begin
            for (k=0; k<bb; k++)
            begin
                ii=j+k;
                fwt_t1 = x[ii] >>1 ;         //scale to keep fixed pt in range
                fwt_t2 = x[ii+bb] >>1 ;
                x[ii] = fwt_t1 + fwt_t2 ;    //
                x[ii+bb] = fwt_t1 - fwt_t2 ;
            end
        end
        bb = bb>>1 ; st = st>>1 ;
    end
end

//========================================================  
void FWTreorder(int x[], flash char r[]) 
//converts from dyadic order to sequency order -- see references
begin
    char i,j;
    int xp[N_WAVE];
    for (i=0; i<N_WAVE; i++)
    begin
        j = r[i];
        xp[j] = x[i];
    end
    for (i=0; i<N_WAVE; i++)
    begin
        x[i] = xp[i];
    end    
end
//========================================================        
//set up a test input and call FWT stuff

void main(void)
begin
    unsigned char v ; 
    int fr[N_WAVE] ;
       
  //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[i] = i;
    end
    
    
    FWTfix(fr);
    FWTreorder(fr, reorder16);
    
    for (v=0; v<N_WAVE; v++)
    begin
        printf(" %f\r\n", fix2float(fr[v]));
    end
       
    
    //============================================
    //time the FWT
    TCCR1B = 1 ;
    TCNT1 = 0;
    FWTfix(fr);
    TCCR1B = 0;
    printf("FWT cycles=%d\n\r",TCNT1)  ; 
    
    TCCR1B = 1 ;
    TCNT1 = 0;
    FWTreorder(fr, reorder16);
    TCCR1B = 0;
    printf("Reorder cycles=%d\n\r",TCNT1)  ;
    
    //============================================ 
    
  while(1);   
end