/* Fixed point math API.
   Rick Smereka, Copyright (C) 2002-2004.

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, get a copy via the Internet at
   http://gnu.org/copyleft/gpl.html or write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston,
   MA 02111-1307 USA

   You can contact the author via email at rsmereka@future-lab.com

   This module performs basic math operations (add, subtract,
   multiply and divide) on floating point numbers using fixed
   point math. Floating point numbers are input in string form
   and stored as long integers. The position of the deciaml
   point, the number as a string and the current value as a
   long are stored in a structure called 'fxp.'

   Original Linux version Apr/2002, Rick Smereka

   Ported to 32-bit Windows. May/2002, Rick Smereka

   Ported to DOS and QNX 4.x. Jul/2002, Rick Smereka

   Added function 'fxp_remove_decimal'. Aug/2002, Rick Smereka

   Ported to Debian Linux. Nov/2002, Rick Smereka

   Corrected bug in 'fxp_math' where the result of a math operation
   could be expressed in the wrong scale if the answer was zero.
   Feb/2004, Rick Smereka

   Changed all logging calls from 'log_file_date' to 'logman'.
   Mar/2004, Rick Smereka */

#include "stdhead.h"

/* private functions */

static struct fxp *fxp_math(struct fxp *, struct fxp *, int);
static int fxp_comp(struct fxp *, struct fxp *, int);
static struct fxp *fxp_new(void);

struct fxp *fxp_iconv(char *cval)
{
   /* Perform an input conversion on a floating point number.
      This function will dynamically create an 'fxp' structure.
      It is the responsibility of the caller to deallocate the
      structure after use. */

   struct fxp *result;
   char mname[] = "fxp_iconv", ch;
   int len, i, found_point, out_pos;

   logman("%s:enter", mname);

   if (cval == (char *)NULL || !strlen(cval))
      {
      logman("%s:null or empty[cval]", mname);
      return(FXP_OT_NULL);
      }

   len = strlen(cval);

   if ((result = fxp_new()) == FXP_OT_NULL)
      {
      logman("%s:alloc fail[result]", mname);
      return(FXP_OT_NULL);
      }

   if ((result->charval = (char *)malloc(len + 1)) == (char *)NULL)
      {
      logman("%s:alloc fail[result->charval]", mname);
      fxp_free(result);
      return(FXP_OT_NULL);
      }

   result->charval[0] = EOS;
   found_point = FALSE;
   out_pos = 0;

   for(i = 0; i < len; i++)
      {
      ch = cval[i];
      
      switch(ch)
         {
         case '.':
            /* if we have already found the decimal, error */

            if (found_point)
               {
               logman("%s:more than one decimal point found", mname);
               fxp_free(result);
               return(FXP_OT_NULL);
               }

            found_point = TRUE;
            break;
         
         case '-':
            /* minus sign must be in zeroth position */

            if (i != 0)
               {
               logman("%s:found hyphen not at beginning", mname);
               fxp_free(result);
               return(FXP_OT_NULL);
               }

            result->charval[0] = '-';
            result->charval[1] = EOS;
            out_pos++;
            break;

         default:
            /* must be a digit */

            if (ch < '0' || ch > '9')
               {
               logman("%s:char[%d] is not a number", mname, i + 1);
               fxp_free(result);
               return(FXP_OT_NULL);
               }

            result->charval[out_pos++] = ch;
            result->charval[out_pos] = EOS;

            /* if we have already found the decimal point,
               increase scale */

            if (found_point)
               result->scale++;
         };
      }

   /* set long value */

   result->val = atol(result->charval);
   logman("%s:normal exit", mname);
   return(result);
}

int fxp_oconv(struct fxp *tfxp, int tscale, char *outstr)
{
   /* Perform an output conversion on a fixed point number.
      Upon success, the string 'outstr' will be loaded with
      the result. An attempt is made to round the result
      to the precision specified by 'tscale'. Function
      returns 'TRUE' upon success, 'FALSE' otherwise. */

   struct fxp *result;
   char mname[] = "fxp_oconv", rstr[128];
   int len, dpos, i, j, tmp;

   logman("%s:enter", mname);

   if (tfxp == FXP_OT_NULL)
      {
      logman("%s:exit:null[tfxp]", mname);
      return(FALSE);
      }

   if (outstr == (char *)NULL)
      {
      logman("%s:exit:null[outstr]", mname);
      return(FALSE);
      }
      
   outstr[0] = EOS;

   if (tscale < FXP_DEFAULT)
      {
      logman("%s:exit[FALSE]:out of range[tscale]", mname);
      return(FALSE);
      }

   if ((result = fxp_dup(tfxp)) == FXP_OT_NULL)
      {
      logman("%s:exit:bad rc from fxp_dup", mname);
      return(FALSE);
      }

   /* round number if there is sufficient scale */

   if (tscale != FXP_DEFAULT && result->scale > tscale)
      {
      if (!fxp_scale(result, tscale + 1))
         {
         logman("%s:exit:bad rc from fxp_scale", mname);
         fxp_free(result);
         return(FALSE);
         }

      len = strlen(result->charval);
      i = result->charval[len - 1] - 48;

      if (i >= 5)
         {
         /* if the number is positive, add 10 */

         if (result->val > 0L)
            result->val += 10L;
         else
            /* number is negative, subtract 10 */

            result->val -= 10L;

         free(result->charval);
         sprintf(rstr, "%ld", result->val);

         if ((result->charval = initstring(rstr)) == (char *)NULL)
            {
            logman("%s:exit:bad rc from initstring[result->charval]",
                          mname);
            free(result);
            return(FALSE);
            }
         }
      }

   if (!fxp_scale(result, tscale))
      {
      logman("%s:exit:bad rc from fxp_scale", mname);
      fxp_free(result);
      return(FALSE);
      }

   /* if scale is zero (integer), just copy into output string */

   if (tscale == 0)
      {
      strcpy(outstr, result->charval);
      fxp_free(result);
      logman("%s:normal exit[TRUE]", mname);
      return(TRUE);
      }
   
   /* if scale is default, take it from the source struct */

   if (tscale == FXP_DEFAULT)
      tmp = tfxp->scale;
   else
      tmp = tscale;

   /* calculate where the deciaml point should be */

   len = strlen(result->charval);
   dpos = len - tmp;

   if (dpos < 0)
      {
      fxp_free(result);
      logman("%s:exit[FALSE]:out of range[tscale]", mname);
      return(FALSE);
      }

   /* copy inserting decimal point */

   for(i = 0, j = 0; i < len; j++)
      {
      if (j == dpos)
         outstr[j] = '.';
      else
         outstr[j] = result->charval[i++];

      outstr[j + 1] = EOS;
      }

   fxp_free(result);
   logman("%s:normal exit[TRUE]", mname);
   return(TRUE);
}

struct fxp *fxp_add(struct fxp *ad1, struct fxp *ad2)
{
   /* Add two fixed point numbers. A new 'fxp' structure will
      be dynamically allocated for the result. This must be
      de-allocated by the caller. Function returns an 'fxp'
      structure upon success, a null pointer otherwise. */

   struct fxp *result;
   char mname[] = "fxp_add";

   logman("%s:enter", mname);
   result = fxp_math(ad1, ad2, FXP_OP_ADD);
   logman("%s:normal exit", mname);
   return(result);
}

struct fxp *fxp_sub(struct fxp *ad1, struct fxp *ad2)
{
   /* Subtract two fixed point numbers. A new 'fxp' structure will
      be dynamically allocated for the result. This must be
      de-allocated by the caller. Function returns an 'fxp'
      structure upon success, a null pointer otherwise.
      Note that 'ad2' is subtracted from 'ad1'. */

   struct fxp *result;
   char mname[] = "fxp_sub";

   logman("%s:enter", mname);
   result = fxp_math(ad1, ad2, FXP_OP_SUB);
   logman("%s:normal exit", mname);
   return(result);
}

struct fxp *fxp_mul(struct fxp *ad1, struct fxp *ad2)
{
   /* Multiply two fixed point numbers. A new 'fxp' structure will
      be dynamically allocated for the result. This must be
      de-allocated by the caller. Function returns an 'fxp'
      structure upon success, a null pointer otherwise.
      Note that 'ad1' is multiplied by 'ad2'. */

   struct fxp *result;
   char mname[] = "fxp_mul";

   logman("%s:enter", mname);
   result = fxp_math(ad1, ad2, FXP_OP_MUL);
   logman("%s:normal exit", mname);
   return(result);
}

struct fxp *fxp_div(struct fxp *ad1, struct fxp *ad2)
{
   /* Divide two fixed point numbers. A new 'fxp' structure will
      be dynamically allocated for the result. This must be
      de-allocated by the caller. Function returns an 'fxp'
      structure upon success, a null pointer otherwise.
      Note that 'ad1' is divided by 'ad2'. */

   struct fxp *result;
   char mname[] = "fxp_div";

   logman("%s:enter", mname);
   result = fxp_math(ad1, ad2, FXP_OP_DIV);
   logman("%s:normal exit", mname);
   return(result);
}

int fxp_eq(struct fxp *fxp1, struct fxp *fxp2)
{
   /* Perform an equality test on two fixed point numbers.
      Function returns 'TRUE' if the two are equal,
      'FALSE' otherwise. */

   char mname[] = "fxp_eq";
   int ret;

   logman("%s:enter", mname);
   ret = fxp_comp(fxp1, fxp2, FXP_OP_EQ);
   logman("%s:normal exit[%d]", mname, ret);
   return(ret);
}

int fxp_lt(struct fxp *fxp1, struct fxp *fxp2)
{
   /* Determine whether one fixed point number is less than the other.
      Function returns 'TRUE' if 'fxp1' is less than 'fxp2',
      'FALSE' otherwise. */

   char mname[] = "fxp_lt";
   int ret;

   logman("%s:enter", mname);
   ret = fxp_comp(fxp1, fxp2, FXP_OP_LT);
   logman("%s:normal exit[%d]", mname, ret);
   return(ret);
}

int fxp_gt(struct fxp *fxp1, struct fxp *fxp2)
{
   /* Determine whether one fixed point number is greater than the other.
      Function returns 'TRUE' if 'fxp1' is greater than 'fxp2',
      'FALSE' otherwise. */

   char mname[] = "fxp_gt";
   int ret;

   logman("%s:enter", mname);
   ret = fxp_comp(fxp1, fxp2, FXP_OP_GT);
   logman("%s:normal exit[%d]", mname, ret);
   return(ret);
}

int fxp_le(struct fxp *fxp1, struct fxp *fxp2)
{
   /* Determine whether one fixed point number is less than
      or equal to the other.
      Function returns 'TRUE' if 'fxp1' is less than or equal
      to 'fxp2', 'FALSE' otherwise. */

   char mname[] = "fxp_le";
   int ret;

   logman("%s:enter", mname);
   ret = fxp_comp(fxp1, fxp2, FXP_OP_LE);
   logman("%s:normal exit[%d]", mname, ret);
   return(ret);
}

int fxp_ge(struct fxp *fxp1, struct fxp *fxp2)
{
   /* Determine whether one fixed point number is greater than
      or equal to the other.
      Function returns 'TRUE' if 'fxp1' is greater than or equal
      to 'fxp2', 'FALSE' otherwise. */

   char mname[] = "fxp_ge";
   int ret;

   logman("%s:enter", mname);
   ret = fxp_comp(fxp1, fxp2, FXP_OP_GE);
   logman("%s:normal exit[%d]", mname, ret);
   return(ret);
}

static struct fxp *fxp_math(struct fxp *ad1, struct fxp *ad2, int opcode)
{
   /* Perform math on two fixed point numbers and return the result.
      A new 'fxp' structure will be dynamically allocated
      for the result. This must be de-allocated by the
      caller.  Function returns an 'fxp' structure upon
      success, a null pointer otherwise. */

   struct fxp *nad1, *nad2, *result;
   char mname[] = "fxp_math", cval[128];
   int nscale, cscale, readjust;

   logman("%s:enter", mname);

   if (ad1 == FXP_OT_NULL)
      {
      logman("%s:exit:null[ad1]", mname);
      return(FXP_OT_NULL);
      }

   if (ad2 == FXP_OT_NULL)
      {
      logman("%s:exit:null[ad2]", mname);
      return(FXP_OT_NULL);
      }

   /* calculate the common scale to both numbers which is the
      largest of the two */

   cscale = (ad1->scale > ad2->scale) ? ad1->scale : ad2->scale;
   nscale = cscale;

   /* duplicate two input numbers changing their scale to
      the largest common scale */

   if ((nad1 = fxp_dup(ad1)) == FXP_OT_NULL)
      {
      logman("%s:exit:alloc fail[nad1]", mname);
      return(FXP_OT_NULL);
      }

   /* if dividing, increase scale of dividend */

   if (opcode == FXP_OP_DIV)
      nscale = ad1->scale + ad2->scale + 1;

   if (!fxp_scale(nad1, nscale))
      {
      fxp_free(nad1);
      logman("%s:exit:bad rc from fxp_scale[nad1]", mname);
      return(FXP_OT_NULL);
      }

   if ((nad2 = fxp_dup(ad2)) == FXP_OT_NULL)
      {
      fxp_free(nad1);
      logman("%s:exit:alloc fail[nad2]", mname);
      return(FXP_OT_NULL);
      }

   if (!fxp_scale(nad2, cscale))
      {
      fxp_free(nad1);
      fxp_free(nad2);
      logman("%s:exit:bad rc from fxp_scale[nad2]", mname);
      return(FXP_OT_NULL);
      }

   logman("%s:ad1 contents", mname);
   fxp_debug(ad1);
   logman("%s:nad1 contents", mname);
   fxp_debug(nad1);

   logman("%s:ad2 contents", mname);
   fxp_debug(ad2);
   logman("%s:nad2 contents", mname);
   fxp_debug(nad2);

   /* create the result structure */

   if ((result = fxp_new()) == FXP_OT_NULL)
      {
      fxp_free(nad1);
      fxp_free(nad2);
      logman("%s:exit:bad rc from fxp_new[result]", mname);
      return(FXP_OT_NULL);
      }

   /* load the result scale */

   result->scale = nad1->scale;
   readjust = FALSE;

   /* if second operand is zero, do not calc and the result will
      be the contents of the first operand */

   if (nad2->val == 0L)
      {
      logman("%s:second operand is zero,no calc", mname);
      result->val = nad1->val;

      // if first operand is zero, set scale of result to zero

      if (nad1->val == 0L)
         {
         readjust = TRUE;
         result->scale = 0;
         }
      }
   else
      {
      /* calculate result value */

      switch(opcode)
         {
         case FXP_OP_ADD:
            result->val = nad1->val + nad2->val;
            break;

         case FXP_OP_SUB:
            result->val = nad1->val - nad2->val;
            break;

         case FXP_OP_MUL:
            result->val = nad1->val * nad2->val;
            result->scale += nad2->scale;
            break;

         case FXP_OP_DIV:
            result->val = nad1->val / nad2->val;

            /* result scale is the smallest of the two plus one */

            result->scale = (ad1->scale < ad2->scale) ? ad1->scale + 1 :
                             ad2->scale + 1;
            break;

         default:
            fxp_free(nad1);
            fxp_free(nad2);
            fxp_free(result);
            logman("%s:exit:unknown math operation", mname);
            return(FXP_OT_NULL);
         };
      }

   sprintf(cval, "%ld", result->val);
   logman("%s:cval=%s", mname, cval);

   if ((result->charval = (char *)malloc(strlen(cval) + 1)) == (char *)NULL)
      {
      fxp_free(nad1);
      fxp_free(nad2);
      fxp_free(result);
      logman("%s:exit:alloc fail[result->charval]", mname);
      return(FXP_OT_NULL);
      }
  
   strcpy(result->charval, cval);

   // readjust scale if necessary

   if (readjust)
      if (!fxp_scale(result, nad1->scale))
         {
         fxp_free(nad1);
         fxp_free(nad2);
         fxp_free(result);
         logman("%s:exit[NULL]:readjust scale error", mname);
         return(FXP_OT_NULL);
         }

   fxp_free(nad1);
   fxp_free(nad2);
   logman("%s:result contents", mname);
   fxp_debug(result);
   logman("%s:normal exit", mname);
   return(result);
}

static int fxp_comp(struct fxp *ad1, struct fxp *ad2, int opcode)
{
   /* Perform a comparison of two fixed point numbers and return the result.
      Function returns 'TRUE' if the comparison was positive, 'FALSE'
      otherwise. */

   struct fxp *nad1, *nad2;
   char mname[] = "fxp_comp";
   int nscale, ret;

   logman("%s:enter", mname);

   if (ad1 == FXP_OT_NULL)
      {
      logman("%s:exit:null[ad1]", mname);
      return(FALSE);
      }

   if (ad2 == FXP_OT_NULL)
      {
      logman("%s:exit:null[ad2]", mname);
      return(FALSE);
      }

   nscale = (ad1->scale > ad2->scale) ? ad1->scale : ad2->scale;

   /* duplicate two input numbers changing their scale to
      the largest common scale */

   if ((nad1 = fxp_dup(ad1)) == FXP_OT_NULL)
      {
      logman("%s:exit:alloc fail[nad1]", mname);
      return(FALSE);
      }

   if (!fxp_scale(nad1, nscale))
      {
      fxp_free(nad1);
      logman("%s:exit:bad rc from fxp_scale[nad1]", mname);
      return(FALSE);
      }

   if ((nad2 = fxp_dup(ad2)) == FXP_OT_NULL)
      {
      fxp_free(nad1);
      logman("%s:exit:alloc fail[nad2]", mname);
      return(FALSE);
      }

   if (!fxp_scale(nad2, nscale))
      {
      fxp_free(nad1);
      fxp_free(nad2);
      logman("%s:exit:bad rc from fxp_scale[nad2]", mname);
      return(FALSE);
      }

   logman("%s:ad1 contents", mname);
   fxp_debug(ad1);
   logman("%s:nad1 contents", mname);
   fxp_debug(nad1);

   logman("%s:ad2 contents", mname);
   fxp_debug(ad2);
   logman("%s:nad2 contents", mname);
   fxp_debug(nad2);

   switch(opcode)
      {
      case FXP_OP_EQ:
         ret = (nad1->val == nad2->val) ? TRUE : FALSE;
         break;

      case FXP_OP_LT:
         ret = (nad1->val < nad2->val) ? TRUE : FALSE;
         break;

      case FXP_OP_GT:
         ret = (nad1->val > nad2->val) ? TRUE : FALSE;
         break;

      case FXP_OP_LE:
         ret = (nad1->val <= nad2->val) ? TRUE : FALSE;
         break;

      case FXP_OP_GE:
         ret = (nad1->val >= nad2->val) ? TRUE : FALSE;
         break;

      default:
         logman("%s:exit:unknown comparison op[%d]", mname, opcode);
         fxp_free(nad1);
         fxp_free(nad2);
         return(FALSE);
      }

   logman("%s:normal exit[%d]", mname, ret);
   fxp_free(nad1);
   fxp_free(nad2);
   return(ret);
}

int fxp_scale(struct fxp *tfxp, int tscale) 
{
   /* Change the scale of a fixed point number. Digits
      will be added or taken away from the right side
      of the number. Function returns 'TRUE' upon success,
      'FALSE' otherwise. */

   char mname[] = "fxp_scale", *new_charval;
   int len, tmp, i, j;

   logman("%s:enter", mname);
   
   if (tfxp == FXP_OT_NULL)
      {
      logman("%s:exit:null struct", mname);
      return(FALSE);
      }

   if (tfxp->charval == (char *)NULL || !strlen(tfxp->charval))
      {
      logman("%s:exit:null or empty[tfxp->charval]", mname);
      return(FALSE);
      }

   if (tscale == FXP_DEFAULT || tscale == tfxp->scale)   
      {
      logman("%s:exit:default scale requested", mname);
      return(TRUE);
      }

   len = strlen(tfxp->charval);

   /* scaling to less than current */

   if (tscale < tfxp->scale)
      {
      /* if specified scale is zero, make integer */

      if (tscale == 0)
         tmp = tfxp->scale;
      else
         tmp = tfxp->scale - tscale;

      /* if number is only a decimal and we are making an int,
         set to zero */

      if (tmp == len)
         {
         tfxp->charval[0] = '0';
         tfxp->charval[1] = EOS;
         }
      else
         {
         /* go right to left in string, wiping */

         for(i = len - 1, j = 0; j < tmp; i--, j++)
            tfxp->charval[i] = EOS;
         }

      tfxp->val = atol(tfxp->charval);
      tfxp->scale = tscale;
      logman("%s:normal exit[TRUE]", mname);
      return(TRUE);
      }

   /* scaling to more than current */

   tmp = tscale - tfxp->scale;

   if ((new_charval = (char *)malloc(len + tmp + 1)) == (char *)NULL)
      {
      logman("%s:exit:alloc fail[new_charval]", mname);
      return(FALSE);
      }

   strcpy(new_charval, tfxp->charval);

   for(i = len, j = 0; j < tmp; i++, j++)
      {
      new_charval[i] = '0';
      new_charval[i+ 1] = EOS;
      }

   free(tfxp->charval);

   if ((tfxp->charval = initstring(new_charval)) == (char *)NULL)
      {
      tfxp->charval = (char *)NULL;
      free(new_charval);
      logman("%s:exit:alloc fail[tfxp->charval]", mname);
      return(FALSE);
      }

   free(new_charval);
   tfxp->val = atol(tfxp->charval);
   tfxp->scale = tscale;
   logman("%s:normal exit[TRUE]", mname);
   return(TRUE);
}

void fxp_free(struct fxp *tfxp)
{
   /* De-allocate an 'fxp' structure. */

   char mname[] = "fxp_free";

   logman("%s:enter", mname);

   if (tfxp == FXP_OT_NULL)
      {
      logman("%s:exit:null structure", mname);
      return;
      }

   if (tfxp->charval != (char *)NULL)
      free(tfxp->charval);

   free(tfxp);
   logman("%s:normal exit", mname);
}

struct fxp *fxp_dup(struct fxp *tfxp)
{
   /* Duplicate a fixed point structure. The new
      dynamically allocated structure pointer is
      returned upon success, a null pointer
      otherwise. */

   struct fxp *result;
   char mname[] = "fxp_dup";

   logman("%s:enter", mname);
   
   if ((result = fxp_new()) == FXP_OT_NULL)
      {
      logman("%s:exit:bad rc from fxp_new", mname);
      return(FXP_OT_NULL);
      }

   if ((result->charval = initstring(tfxp->charval)) == (char *)NULL)
      {
      logman("%s:exit:alloc fail[result->charval]", mname);
      fxp_free(result);
      return(FXP_OT_NULL);
      }

   result->val = tfxp->val;
   result->scale = tfxp->scale;
   logman("%s:normal exit", mname);
   return(result);
}
   
static struct fxp *fxp_new(void)
{
   /* Create a new 'fxp' structure. Structure will be
      initialized. Structure is dynamically allocated.
      Function returns the new structure upon success,
      a 'NULL' pointer otherwise. */
     
   struct fxp *result;
   char mname[] = "fxp_new";
 
   logman("%s:enter", mname);

   if ((result = (struct fxp *)malloc(sizeof(struct fxp))) ==
       FXP_OT_NULL)
      {
      logman("%s:alloc fail[result]", mname);
      return(FXP_OT_NULL);
      }

   result->charval = (char *)NULL;
   result->val = 0L;
   result->scale = 0;
   logman("%s:normal exit", mname);
   return(result);
}

void fxp_debug(struct fxp *f1)
{
   /* Output contents of an 'fxp' structure to the log. */

   if (f1 == FXP_OT_NULL)
      return;

   logman("fxp_debug:charval=%s,val=%ld,scale=%d", f1->charval,
                 f1->val, f1->scale);
}

void fxp_remove_decimal(char *cfin, char *cfout)
{
   /* Copy a string removing the decimal point. Function returns
      the input string (minus the decimal point) in 'cfout'
      upon success. */

   char ch;
   int len, i, j;

   if (cfin == (char *)NULL || !strlen(cfin))
      return;

   if (cfout == (char *)NULL)
      return;

   cfout[0] = EOS;
   len = strlen(cfin);

   for(i = 0, j = 0; i < len; i++)
      {
      ch = cfin[i];

      if (ch != '.')
         {
         cfout[j++] = ch;
         cfout[j] = EOS;
         }
      }
}
