#include <stdio.h>
#include "fixed_point.h"

// compute 2^x, for any integer x
static double 
power2(int x) {
   unsigned char ispos = x >= 0;

   int absvalx = ispos ? x : -x;
   double value;

   if (absvalx < 32)
   {
      value = 1 << absvalx;
   }
   else {
      double value31 = 1 << 31;
      value = 1.0;
      
      do {
	 value *= value31;
	 absvalx -= 31;
      }
      while (absvalx > 31);
      
      while (absvalx > 0) {
	 value *= 2.0;
	 absvalx--;
      }
   }
   return ispos ? value : 1.0/value;
}

// Convert a double to a fixed point word.  If the
// double is out of range of the fixed-point representation,
// return the closest value.
int 
d2w(double x, unsigned char bitsize, int exponent, unsigned char sign)
{
   // The min of a signed number has a 1 in the msb and
   // zeros everywhere else.
   int wMin = sign ? 1 << (bitsize-1) : 0;
   // For signed or unsigned, the max is the bitwise negation of the
   // min (within bitsize)
   int wMax = ~wMin;

   double dMin = w2d(wMin, bitsize, exponent, sign);

   double dMax = w2d(wMax, bitsize, exponent, sign);
   double value;

   // If the floating point value exceeds the range of this
   // fixed-point representation, then just return the mix or max.
   if (x >= dMax) {
      return wMax;
   }
   else if (x <= dMin) {
      return wMin;
   }

   // You are about to round to an integer, so add 0.5
   value = x * power2(-exponent) + 0.5;

   return sign ? (int) value : (unsigned int) value;
}

// Convert a fixed-point word to a double.  
double 
w2d(int x, unsigned char bitsize, int exponent, unsigned char sign)
{
   double value;
   int y;

   // remove all bits above bitsize
   y = x << (32-bitsize);
   if (sign) {
      y = y >> (32-bitsize);
      value = ((double)y)*power2(exponent);
   }
   else {
      y = ((unsigned int) y) >> (32-bitsize);
      value = ((double)((unsigned int) y)) * power2(exponent);
   }

   return value;
}

