// 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 divfix(a,b)  ((int)((((long)(a))<<8)/((long)(b)))) //divide two fixed #  

//==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
   
    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
    #endasm
end
//========================================================

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

signed int cInFix, fInFix, prod, ratio, root, s, c;    
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 int2fix
    printf("\r\nenter a signed char:") ;
    scanf("%d",&cIn);printf("%d",cIn);
    cInFix =  int2fix(cIn);
    printf(" fixed rep:%04x\r\n",cInFix); 
    
    //test float2fix
    printf("enter a float:") ;
    scanf("%s",fInString); fIn = atof(fInString);
    printf("%f",fIn);
    fInFix = float2fix(fIn);
    printf(" fixed rep:%04x\r\n",fInFix); 
    
    //test multiply and fix2float and fix2int
    prod = multfix(fInFix, cInFix) ;
    printf("%d*%f=%f", cIn, fIn, fix2float(prod)) ;
    printf(" fixed rep:%04x\r\n", prod) ;    
    
    //test multiply and fix2float and fix2int
    prod = multfix(cInFix, fInFix) ;
    printf("%d*%f=%f", cIn, fIn, fix2float(prod)) ;
    printf(" fixed rep:%04x\r\n", prod) ;
    
    //test multiply and fix2float and fix2int
    prod = multfixSlow(cInFix, fInFix) ;
    printf("%d*%f=S=%f", cIn, fIn, fix2float(prod)) ;
    printf(" fixed rep:%04x\r\n", prod) ;
    
    
    //============================================
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = multfix(fInFix, cInFix) ;
    TCCR1B = 0;
    printf("multfix cycles=%d\n\r",TCNT1)  ;  
    
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = multfixSlow(fInFix, cInFix) ; 
    TCCR1B = 0;
    printf("multfixSlow cycles=%d\n\r",TCNT1)  ;  
    
    TCCR1B = 1 ;
    TCNT1 = 0;
    fIn = fIn * fIn ;
    TCCR1B = 0;
    printf("multfloat cycles=%d\n\r",TCNT1)  ;
    
    
  end
end