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

#define int2fix(a)   (((int)(a))<<8)            //Convert char to fix  
#define fix2int(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 multfix(a,b) ((int)((((long)(a))*((long)(b)))>>8)) //multiply two fixed #
#define divfix(a,b)  ((int)((((long)(a))<<8)/((long)(b)))) //divide two fixed #

//lsqrt is in math.h
#define sqrtfix(a)   (lsqrt(((long)(a))<<8))   //square root  

//for CORDIC sine/cos only
flash int Angles[]={
	float2fix(45.0),	float2fix(26.565),	float2fix(14.0362),
	float2fix(7.12502),	float2fix(3.57633),	float2fix(1.78991),
	float2fix(0.895174),float2fix(0.447614),float2fix(0.223811),
	float2fix(0.111906),float2fix(0.055953),float2fix(0.027977) };    


signed char cIn;
float fIn; 
char fInString[16];

signed int cInFix, fInFix, prod, ratio, root, s, c;

//===CORDIC================================================
//From: http://my.execpc.com/~geezer/embed/cordic.htm --- geezer@execpc.com
int sinefix(int TargetAngle)
begin
    int X, Y, CurrAngle;
	unsigned Step;
	
	Y=0;
	CurrAngle=0;
	X = float2fix(0.6072529350);
	
	for(Step=0; Step < 12; Step++)
	{	int NewX;

		if(TargetAngle > CurrAngle)
		{	NewX=X - (Y >> Step);
			Y=(X >> Step) + Y;
			X=NewX;
			CurrAngle += Angles[Step]; }
		else
		{	NewX=X + (Y >> Step);
			Y=-(X >> Step) + Y;
			X=NewX;
			CurrAngle -= Angles[Step]; }} 
		return Y;
end

//From: http://my.execpc.com/~geezer/embed/cordic.htm --- geezer@execpc.com
int cosfix(int TargetAngle)
begin
    int X, Y, CurrAngle;
	unsigned Step;
	
	Y=0;
	CurrAngle=0;
	X = float2fix(0.6072529350);
	
	for(Step=0; Step < 12; Step++)
	{	int NewX;

		if(TargetAngle > CurrAngle)
		{	NewX=X - (Y >> Step);
			Y=(X >> Step) + Y;
			X=NewX;
			CurrAngle += Angles[Step]; }
		else
		{	NewX=X + (Y >> Step);
			Y=-(X >> Step) + Y;
			X=NewX;
			CurrAngle -= Angles[Step]; }} 
		return X;
end

//==========================================================
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("enter a signed char:") ;
    scanf("%d",&cIn);printf("%d",cIn);
    cInFix =  int2fix(cIn);
    printf(" fixed rep:%04x\r\n\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\n",fInFix); 
    
    //test multiply and fix2float and fix2int
    prod = multfix(fInFix, cInFix) ;
    printf("char*float=%f\r\n", fix2float(prod)) ;
    printf("(char)char*float=%d\r\n\n", fix2int(prod)) ;
    
    //test divide and fix2float
    prod = divfix(cInFix, fInFix) ;
    printf("char/float=%f\r\n\n", fix2float(prod)) ;
    
    //test sqrtfix
    root = sqrtfix(fInFix);
    printf("sqrt(float)=%f\r\n\n", fix2float(root)) ;
    
    s = sinefix(fInFix);
    printf("sin(float)=%f\r\n\n", fix2float(s)) ;
    
    c = cosfix(fInFix);
    printf("cos(float)=%f\r\n\n", fix2float(c)) ;
    
  end
end