#include <Mega32.h>
//#include <delay.h> 
#include <math.h> // for sine
#include <stdio.h>

//I like these definitions
#define begin {
#define end   } 
#define countMS 62  //ticks/mSec
#define fix2float(a) ((float)(a)/256.0)
#define float2fix(a) ((int)((a)*256.0))
 
unsigned int accumulator @0x5f0;
unsigned char highbyte @0x5f1; //the HIGH byte of the accumulator variable
unsigned int increment;			
int sineTable[256] ;  
int fInput, fOutput, maxOutput ;
unsigned int i, time ;  
char amp, count; 
#define Nfreq 11
flash int fList[Nfreq] = 
        { 200, 400, 800, 900, 1000, 1100, 1200, 1300, 1600, 2500, 3200 };
//flash int fList[Nfreq] = 
 //       { 1000, 1200, 1300, 1600, 1900, 2000, 2100, 2150, 2200, 2500, 3200 };

//IIR stuff
 #pragma regalloc-
 int b1,a2,a3,a4,a5;   //filter constants
 int xn, xn_1, xn_2, xn_3, xn_4,  ; //input state
 int yn, yn_1, yn_2, yn_3, yn_4  ; //output state
 #pragma regalloc+

//========================================================   
  //========================================================   
int Butter2bandScaled(int xx)
// xx is the current input signal sample
// returns the current filtered output sample
begin
    #asm
    .macro mult_acc         ;r31:r30:r24 += r23:r22 * r21:r20
        muls r23, r21		; (signed)ah * (signed)bh
    	add	r31, r0
    	mul	r22, r20		; al * bl
    	add	r24, r0
    	adc	r30, r1
    	adc	r31, r27
    	mulsu r23, r20		; (signed)ah * bl
    	add	r30, r0
    	adc	r31, r1
    	mulsu	r21, r22	; (signed)bh * al
    	add	r30, r0
    	adc	r31, r1
    .endm 
    push r20    ;save parameter regs
    push r21
    
	clr r27             ;permanent zero
    clr r24             ;clear 24 bit result reg; msb to lsb => r31:r30:r24
    clr r30      
    clr r31
    
    lds  R22, _xn_2     ;load x(n-2) from  RAM
	lds  R23, _xn_2+1
	ld   R20, Y         ;load input parameter xx from stack
	ldd  R21, Y+1
	sub  r20, r22       ;form xx-x(n-2)
	sbc  r21, r23       
	lds  R22, _b1       ;load b1 from RAM
	lds  R23, _b1+1 
    mult_acc            ; b1*(xx-x(n-2)) 
    
    lds  R22, _a2       ;load -a2 from RAM
	lds  R23, _a2+1 
	lds  R20, _yn_1     ;load y(n-1) from  RAM
	lds  R21, _yn_1+1
    mult_acc            ; -a2*y(n-1)  
    
    lds  R22, _a3       ;load -a3 from RAM
	lds  R23, _a3+1 
	lds  R20, _yn_2     ;load y(n-2) from  RAM
	lds  R21, _yn_2+1
    mult_acc            ; -a3*y(n-2)
    
    lds  R20, _xn_1     ;load x(n-1) from  RAM
	lds  R21, _xn_1+1
	sts  _xn_2, r20      ;store x(n-2) to  RAM
	sts  _xn_2+1, R21 
	ld   R20, Y         ;load input parameter xx from stack
	ldd  R21, Y+1
	sts  _xn_1, r20     ;store x(n-1) to  RAM
	sts  _xn_1+1, R21 
	lds  R20, _yn_1     ;load y(n-1) from  RAM
	lds  R21, _yn_1+1
	sts  _yn_2, R20     ;store y(n-2) to  RAM
	sts  _yn_2+1, R21 
	asr  r31  ; divide by 16 for coeff prescale
	ror  r30
	asr  r31
	ror  r30
	asr  r31
	ror  r30 
	asr  r31
	ror  r30
	sts  _yn_1, r30     ;store new output as y(n-1) to  RAM
	sts  _yn_1+1, r31 
	
    pop r21             ;restore parameter regs
	pop r20
	
	#endasm 
	
end 

///////////////////////////////////////////////////////

interrupt [TIM1_COMPA] void Sgen(void)
begin 
	//the DDR code and scaling
	accumulator = accumulator + increment ;
	fInput = sineTable[highbyte] ;  
	fOutput = Butter2band(fInput);
	time = time + 1; 
    if (fOutput> maxOutput)  maxOutput = fOutput; 
end 
 
void main(void)
begin 
   
   //init the sine table
   for (i=0; i<256; i++)
   begin
   		sineTable[i] = float2fix(sin(6.283*((float)i)/256.0)) ;
   end  
   
   //2-pole butterworth bandpass, cutoff=[0.2 0.25]
   // NOTE that the coefficients are scaled by 16!
    b1 = float2fix(0.073*16) ;               
    a2 = float2fix(1.414*16) ;   //note that sign is negated from design input
    a3 = float2fix(-.854*16) ;  //note that sign is negated from design input
    
    
    // timer 1 interrupt every 2000 cycles to update potential 
    TCCR1B = 0b00001001 ;
    TCNT1 = 0; 
    OCR1A = 1999;
    TIMSK = 0b00010000 ; 
    #asm("sei") ;
    
   //init the UART
   UCSRB = 0x18;
   UBRRL = 103;
   printf("starting...\n\r\n\r"); 
   
   for (i=0; i<Nfreq; i++)
   begin
        time = 0;
        // compute increment 
        increment =  (int)((float)fList[i] * 8.19);
        // wait 
        while (time<1000);
        // get max amp 
        time = 0; 
        maxOutput = 0;
        while (time <1000);
        //print it
        printf("%d %f\n\r",fList[i], fix2float(maxOutput));
        
   end
   
   while(1) 
   begin  
   end // end while 
   
end  //end main