// Test fixed arithmetic macros      
             
#include <mega32.h> 
#include <stdio.h> 
#include <stdlib.h> 
//for square root only
#include <math.h> 

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

//===The fixed macros=========================================
#define int2fix(a)   (((int)(a))<<8)            //Convert char to fix  
#define fix2intSlow(a)  ((signed char)((a)>>8))    //Convert fix to char
#define float2fix(a) ((int)((a)*256.0))         //Convert float to fix
#define fix2float(a) ((float)(a)/256.0)         //Convert fix to float

#define multfixSlow(a,b) ((int)((((long)(a))*((long)(b)))>>8)) //multiply two fixed # 
//see below for the fast version
#define divfixslow(a,b)  ((int)((((long)(a))<<8)/((long)(b)))) //divide two fixed #  

//lsqrt is in math.h
#define sqrtfixSlow(a)   (lsqrt(((long)(a))<<8))   //square root  
 
//== fixed to char ================================= 
char fix2int(int a)
begin
    #asm
    ldd r30, Y+1 ;moves the high byte of the input to low byte
    #endasm
end	 
//==Fast fixed multiply=================================

      int multfix(int a,int b)
begin
    #asm
  ;******************************************************************************
;*
;* FUNCTION
;*	muls16x16_24
;* DECRIPTION
;*	Signed multiply of two 16bits numbers with 24bits result.
;* USAGE
;*	r31:r30:rxx = r23:r22 * r21:r20

;******************************************************************************
    ;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         ;r18, r0
	mul		r22, r20		; al * bl
	mov     r30, r1         ;movw	r17:r16, r1:r0
	;mov     r16, r0
	mulsu	r23, r20		; (signed)ah * bl
	add		r30, r0         ;r17, r0
	adc		r31, r1         ;r18, r1
	mulsu	r21, r22		; (signed)bh * al
	add		r30, r0         ;r17, r0
	adc		r31, r1         ;r18, r1
	
	;pop r21
	;pop r20  
	mov r20, r24
	mov r21, r25
    #endasm
end
 
 //========================================================
int reciprocalfix( int dd)
begin
    int x, d ;
    signed char count, neg ;
    count = 0;
    neg = 0 ; 
    d = dd ;
    
    // only works with + numbers
    if (d & 0x8000)
    begin
        neg = 1;
        d = -d ;
    end
    
    // range reduction
    while (d>0x0100)
    begin
        --count ;
        d >>= 1 ;
    end
    
    while (d<0x0080)
    begin
        ++count ;
        d <<= 1 ;
    end
     
    // Newton interation
    x = 0x02ea - (d<<1) ;
    x = multfix(x, 0x0200-multfix(d,x));
    //x = multfix(x, 0x0200-multfix(d,x));  
    
    // range expansion
    if (count>0)  x = x<<count ;
    else if (count<0) x = x>>(-count) ;  
    
    // fix sign
    if (neg==1) x=-x; 
     
    return x ;  
end
//========================================================
int divfix(int nn, int dd)
begin
    int x, d ;
    signed char count, neg ;
    count = 0;
    neg = 0 ; 
    d = dd ;
    
    // only works with + numbers
    if (d & 0x8000)
    begin
        neg = 1;
        d = -d ;
    end
    
    // range reduction
    while (d>0x0100)
    begin
        --count ;
        d >>= 1 ;
    end
    
    while (d<0x0080)
    begin
        ++count ;
        d <<= 1 ;
    end
     
    // Newton interation
    x = 0x02ea - (d<<1) ;
    x = multfix(x, 0x0200-multfix(d,x));
    //x = multfix(x, 0x0200-multfix(d,x));  
    
    // range expansion
    if (count>0)  x = x<<count ;
    else if (count<0) x = x>>(-count) ;  
    
    // fix sign
    if (neg==1) x=-x;
    
    //form ratio
    x = multfix(x,nn) ;  
     
    return x ;  
end
  


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

signed char cIn; //an input integer
float fIn, fIn2 ;       //an input float
char fInString[16];

signed int fInFix2, fInFix, prod, ratio ;    
char i;

           
//==========================================================
void main(void)
begin  
  //serial setop for debugging using printf, etc.     
  UCSRB = 0x18 ;
  UBRRL = 103 ;
  putsf("\r\nStarting...\r\n");  
  
  while(1)
  begin
    
    //test float2fix
    printf("\n\r enter a float:") ;
    scanf("%s",fInString); fIn = atof(fInString);
    printf("%f",fIn);
    fInFix = float2fix(fIn);
    printf(" fixed rep:%04x\r\n",fInFix);   
    
    //test float2fix
    printf("\n\r enter a float:") ;
    scanf("%s",fInString); fIn2 = atof(fInString);
    printf("%f",fIn2);
    fInFix2 = float2fix(fIn2);
    printf(" fixed rep:%04x\r\n",fInFix2); 
    
    //test reciprocal
    ratio = reciprocalfix(fInFix);
    printf("reciprocal(%f)=%f",fIn,fix2float(ratio)) ;
    printf(" fixed rep:%04x\r\n",ratio); 
       
    //test divfix
    ratio = divfix(fInFix2,fInFix);
    printf("ratio(%f/%f)=%f", fIn2,fIn,fix2float(ratio)) ;
    printf(" fixed rep:%04x\r\n",ratio);       
    
    
    
    //============================================   
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = reciprocalfix(fInFix) ;
    TCCR1B = 0;
    printf("reciprocal cycles=%d\n\r",TCNT1)  ;
    
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = divfix(fInFix2,fInFix) ;
    TCCR1B = 0;
    printf("divfix cycles=%d\n\r",TCNT1)  ;
    
    TCCR1B = 1 ;
    TCNT1 = 0;
    fIn = fIn2/(fIn) ;
    TCCR1B = 0;
    printf(" ratio float cycles=%d\n\r",TCNT1)  ;  
    
    //============================================
     
   
  end
end