// 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 #  

//lsqrt is in math.h
#define sqrtfixSlow(a)   (lsqrt(((long)(a))<<8))   //square root  
 
//==Fast fixed multiply================================= 
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 sqrtfix(int aa)
begin
    // do not change the order of these variables
    // the compiler puts them in
    // r16:17, r18, r19, r20:21
    int a;
    char nextbit, ahigh;
    int root ; 
    a = aa; 
    #asm("mov r19, r17")     //set: ahigh = a>>8
    //
    // range sort to get integer part and to
    // check for weird bits near the top of the range
    if (ahigh >= 0x40)     //bigger than 64?
    begin
        if (a > 0x7e8f)    //>=126.562 = 11.25^2
        begin
            root = 0x0b40;  // 11
            nextbit = 0x10 ;
        end 
        else if (ahigh >= 0x79)    //>=121
        begin
            root = 0x0b00;  // 11
            nextbit = 0x40 ;
        end
        else if (ahigh >= 0x64)    //>=100
        begin
            root = 0x0a00;  // 10
            nextbit = 0x80 ;
        end  
        else if (ahigh >= 0x51)    //>=81
        begin
            root = 0x0900;  // 9
            nextbit = 0x80 ;
        end 
        else   //64
        begin
            root = 0x0800;      //8
            nextbit = 0x80 ; 
        end 
    end
    else if  (ahigh >= 0x10)  //16    //smaller than 64 and bigger then 16
    begin 
        if (ahigh >= 0x31)  //49
        begin
            root = 0x0700;      //7
            nextbit = 0x80 ; 
        end
        else if (ahigh >= 0x24)  //36
        begin
            root = 0x0600;      //6
            nextbit = 0x80 ; 
        end
        else if (ahigh >= 0x19)  //25
        begin
            root = 0x0500;      //5
            nextbit = 0x80 ; 
        end   
        else //16
        begin
            root = 0x0400;      //4
            nextbit = 0x80 ; 
        end
    end  
    else       //smaller than 16
    begin 
         if (ahigh >= 0x09)  //9
        begin
            root = 0x0300;      //3
            nextbit = 0x80 ; 
        end 
        else if (ahigh >= 0x04)  //4
        begin
            root = 0x0200;      //2
            nextbit = 0x80 ; 
        end
        else if (ahigh >= 0x01)  //1
        begin
            root = 0x0100;      //1
            nextbit = 0x80 ; 
        end  
        else     //less than one
        begin
            root = 0;
            nextbit = 0x80 ;  
        end
    end
    // now get the low order bits  
    // while (nextbit)
    // begin
        #asm
        beginloop: 
        ;tst r18   ;dont need-- lsr at bottom of loop sets current SREG
        breq endit ; jumps out of loop if nextbit is zero
    	;r21:r20 contains root because of compiler trick 
    	; root = nextbit + root; 
    	add r20, r18
    	;adc r21, r19    ; dont need: never any carry
        ;
        ; p =  multfix(root,root);  
        mov r22, r20            ;load root into other mult input
        mov r23, r21
        ;
    	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
    	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  
    	; p is in r30:31
    	;
    	; if (p >= a) root = root - nextbit ;
    	cp  r30, r16            ; compare root^2 with a
    	cpc r31, r17
    	brlt nosub
    	sub r20, r18            ; if r^2 >= a form r=r-nextbit
    	;sbc r21, r19           ; dont need--high byte always zero
    	nosub:
    	; nextbit = nextbit>>1 ;
    	;lsr r19                 ;shift nextbit right
    	;ror r18                  ;simplify--high byte always zero
    	lsr r18
    	rjmp beginloop
    	endit:
    	#endasm
   // end
    return root ;  
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 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 sqrtfix
    root = sqrtfix(fInFix);
    printf("sqrt(%f)=%f", fIn,fix2float(root)) ;
    printf(" fixed rep:%04x\r\n",root); 
    
    //============================================
    //TCCR1B = 1 ;
    //TCNT1 = 0;
    //prod = multfix(fInFix, cInFix) ;
    //TCCR1B = 0;
    //printf("multfix cycles=%d\n\r",TCNT1)  ;  
    
    //============================================
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = sqrtfix(fInFix) ;
    TCCR1B = 0;
    printf("sqrtfix cycles=%d\n\r",TCNT1)  ;
    
    TCCR1B = 1 ;
    TCNT1 = 0;
    fIn = sqrt(fIn) ;
    TCCR1B = 0;
    printf("sqrtfloat cycles=%d\n\r",TCNT1)  ;  
    
    //============================================
     
   
  end
end