// 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 int2fix4_12(a)   (((int)(a))<<12)            //Convert char to fix  
#define fix2intSlow4_12(a)  ((signed char)((a)>>12))    //Convert fix to char
#define float2fix4_12(a) ((int)((a)*4096.0))         //Convert float to fix
#define fix2float4_12(a) ((float)(a)/4096.0)         //Convert fix to float

#define multfixSlow4_12(a,b) ((int)((((long)(a))*((long)(b)))>>12)) //multiply two fixed # 


//==Fast fixed multiply=================================

int mult4_12(int a,int b)
begin
    #asm
 ;******************************************************************************
;*
;* FUNCTION
;*	muls16x16_32
;* DECRIPTION
;*	Signed multiply of two 16bits numbers with 32bits result.
;* USAGE
;*	r19:r18:r17:r16 = r23:r22 * r21:r20
;*	r31:r30:r27:rxx = r23:r22 * r21:r20
;* STATISTICS
;*	Cycles :	19 + ret
;*	Words :		15 + ret
;*	Register usage: r0 to r2 and r16 to r23 (11 registers)
;* NOTE
;*	The routine is non-destructive to the operands.
;*
;******************************************************************************
    push r20
    push r21
    
    LDD  R22,Y+2  ;load a
	LDD  R23,Y+3 
	
	LD   R20,Y    ;load b
	LDD  R21,Y+1
	
	clr	r26
	muls	r23, r21		; (signed)ah * (signed)bh
	movw	r31:r30, r1:r0
	mul	    r22, r20		; al * bl
	mov 	r27, r1
	mulsu	r23, r20		; (signed)ah * bl
	sbc	r31, r26
	add	r27, r0
	adc	r30, r1
	adc	r31, r26
	mulsu	r21, r22		; (signed)bh * al
	sbc	r31, r26
	add	r27, r0
	adc	r30, r1
	adc	r31, r26
	
	lsl r27                 ;shift 4 bits left 
	rol r30                 ;instead of 12 bits right
	rol r31  
	lsl r27
	rol r30
	rol r31
	lsl r27
	rol r30
	rol r31
	lsl r27
	rol r30
	rol r31
	
    pop r21
    pop r20

    #endasm
end
 

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

float fin1, fin2 ;       //an input float
char fInString[16];

signed int fin2Fix, fin1Fix, prod ;   
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
  
    //input first number
    printf("enter a float:") ;
    scanf("%s",fInString); fin1 = atof(fInString);
    printf("%f",fin1);
    fin1Fix = float2fix4_12(fin1);
    printf(" fixed rep:%04x\r\n",fin1Fix); 
    
    //test float2fix
    printf("enter a float:") ;
    scanf("%s",fInString); fin2 = atof(fInString);
    printf("%f",fin2);
    fin2Fix = float2fix4_12(fin2);
    printf(" fixed rep:%04x\r\n",fin2Fix); 
    
    //test multiply and fix2float 
    prod = mult4_12(fin1Fix, fin2Fix) ;
    printf("%f*%f=%f", fin1, fin2, fix2float4_12(prod)) ;
    printf(" fixed rep:%04x\r\n", prod) ;
    
    //============================================
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = mult4_12(fin1Fix, fin2Fix) ;
    TCCR1B = 0;
    printf("multfixSlow4_12 cycles=%d\n\r",TCNT1)  ;  
    
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = multfixSlow4_12(fin1Fix, fin2Fix) ; 
    TCCR1B = 0;
    printf("multfixSlow4_12 cycles=%d\n\r",TCNT1)  ;  
    
    TCCR1B = 1 ;
    TCNT1 = 0;
    fin1 = fin1 * fin2 ;
    TCCR1B = 0;
    printf("multfloat cycles=%d\n\r",TCNT1)  ;
    
    printf("\n\r");
  end
end