// Test fixed-point multiply     
             
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>
//#include <stdlib.h>

//set up the debugging utility ASSERT
#define __ASSERT_USE_STDERR
#include <assert.h>
#include "uart.h"

// UART file descriptor
// putchar and getchar are in uart.c
FILE uart_str = FDEV_SETUP_STREAM(uart_putchar, uart_getchar, _FDEV_SETUP_RW);
       

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


//=== Macros for comparison to asm/c versions ===============
// multiply two fixed 8:8 numbers 
#define multfixSlow(a,b) ((int)((((long)(a))*((long)(b)))>>8)) 
// divide two fixed 8:8 numbers 
#define divfixSlow(a,b)  ((int)((((long)(a))<<8)/((long)(b))))

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

//========================================================
int sqrtfix(int aa)
begin
   
    int a;
    char nextbit, ahigh;
    int root, p ; 
    a = aa; 
    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
		root = nextbit + root;
		p =  multfix(root,root);  
        if (p >= a) root = root - nextbit ;
    	nextbit = nextbit>>1 ;
    end
    return root ;  
end
  

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

volatile int fix1, fix2, prod ; 

int main(void)
begin  
  //init the UART -- uart_init() is in uart.c
  uart_init();
  stdout = stdin = stderr = &uart_str;
  fprintf(stdout,"Starting...\n\r");
  
  while(1)
  begin
  
    // fixed input
    fprintf(stdout,"\r\n enter a fix:") ;
    fscanf(stdin,"%04x", &fix1); 
    
    // fixed input
    fprintf(stdout," enter a fix:") ;
    fscanf(stdin,"%04x", &fix2); 
    
    // test multiply 
    prod = multfix(fix1, fix2) ;
    fprintf(stdout," product:%04x\r\n", prod) ;

	// test multiply 
    prod = multfixSlow(fix1, fix2) ;
    fprintf(stdout," product:%04x\r\n", prod) ;

	// test divide 
    prod = divfix(fix1, fix2) ;
    fprintf(stdout," ratio:%04x\r\n", prod) ;

	// test divide 
    prod = divfixSlow(fix1, fix2) ;
    fprintf(stdout," ratio:%04x\r\n", prod) ;

	// test sqrt 
    prod = sqrtfix(fix1) ;
    fprintf(stdout," sqrt:%04x\r\n", prod) ;
         
         
    //============================================
	// time the multiplies
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = multfix(fix1, fix2) ;
    TCCR1B = 0;
    fprintf(stdout,"multfix cycles=%d\n\r",TCNT1)  ;  
    
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = multfixSlow(fix1, fix2) ; 
    TCCR1B = 0;
    fprintf(stdout,"multfixSlow cycles=%d\n\r",TCNT1)  ; 
	
	// time the divides
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = divfix(fix1, fix2) ;
    TCCR1B = 0;
    fprintf(stdout,"divfix cycles=%d\n\r",TCNT1)  ;  
    
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = divfixSlow(fix1, fix2) ; 
    TCCR1B = 0;
    fprintf(stdout,"divfixSlow cycles=%d\n\r",TCNT1) ;

	// time the sqrt
    TCCR1B = 1 ;
    TCNT1 = 0;
    prod = sqrtfix(fix1) ;
    TCCR1B = 0;
    fprintf(stdout,"sqrtfix cycles=%d\n\r",TCNT1)  ; 
	   
  end
end