# Fixed point math API.
# Rick Smereka, Copyright (C) 2005.

# 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 integers. The position of the deciaml
# point and number as a string are stored in a list.

# Initial tcl version. This version uses a list in place of the
# 'fxp' structure used in the 'C' source. I refer to this list
# as the 'fxp list'. Mar/2005, Rick Smereka

# element position in list
const FXP_VAL 0
const FXP_SCALE 1

# math operations
const FXP_OP_ADD 1
const FXP_OP_SUB 2
const FXP_OP_MUL 3
const FXP_OP_DIV 4
const FXP_OP_EQ 5
const FXP_OP_LT 6
const FXP_OP_GT 7
const FXP_OP_LE 8
const FXP_OP_GE 9

# misc defines
const FXP_DEFAULT -1

proc fxp_iconv {cval} {
   # Perform an input conversion on a floating point number.
   # Number is stored in a list which is returned upon success,
   # an empty string returned otherwise.
   set mname "fxp_iconv"
   set result ""
   
   logman "$mname:enter"
   set len [string length $cval]

   if {!$len} {
      logman "$mname:null or empty[cval]"
      return $result
   }

   set found_point 0
   set scale 0

   for {set i 0} {$i < $len} {incr i} {
      set ch [string index $cval $i]
      
      switch -glob -- $ch {
         . {
            # if we have already found the decimal, error

            if {$found_point} {
               logman "$mname:exit:more than one decimal point found"
               return $result
            }

            set found_point 1
         }         
         
         - {
            # minus sign must be in zeroth position

            if {$i != 0} {
               logman "$mname:exit:found hyphen not at beginning"
               return $result
            }

            set result "-"
         }

         [0-9] {
            append result $ch

            # if we have already found the decimal point,
            # increase scale
            if {$found_point} {
               incr scale
            }
         }

         default {
               set j [expr $i + 1]
               logman "$mname:exit:char\[$j\] is not a number"
               return ""
         }
      }
   }

   lappend result $scale
   logman "$mname:normal exit"
   return $result
}

proc fxp_oconv {fxp_list tscale outstr} {
   # Perform an output conversion on a fixed point number.
   # Upon success, 'outstr' will be loaded with the result
   # to the precision specified by 'tscale'. Returns 1 upon
   # success, 0 otherwise.
   global FXP_DEFAULT FXP_VAL FXP_SCALE
   upvar $outstr lk
   set lk ""
   set mname "fxp_oconv"

   logman "$mname:enter"
   set result $fxp_list

   if {![fxp_get_list $result l_num l_scale]} {
      logman "$mname:exit:bad rc(0) from fxp_get_list(result)"
      return 0
   }

   if {$tscale < $FXP_DEFAULT} {
      logman "$mname:exit(0):out of range(tscale)"
      return 0
   }

   # round number if there is sufficient scale
   if {$tscale != $FXP_DEFAULT && $l_scale > $tscale} {
      if {![fxp_scale result [expr $tscale + 1]]} {
         logman "$mname:exit:bad rc from fxp_scale"
         return 0
      }

      set l_num [lindex $result $FXP_VAL]
      set i [string index $l_num end]

      if {$i >= 5} {
         # if the number is positive add 10
         if {$l_num > 0} {
            set l_num [expr $l_num + 10]
         } else {
            # number is negative, subtract 10
            set l_num [expr $l_num - 10]
         }

         set result [lreplace $result $FXP_VAL $FXP_VAL $l_num]
      }
   }

   if {![fxp_scale result $tscale]} {
      logman "$mname:exit(0):bad rc(0) from fxp_scale"
      return 0
   }

   # if scale is zero (integer), just copy into output string
   if {$tscale == 0} {
      set lk [lindex $result $FXP_VAL]
      logman "$mname:normal exit"
      return 1
   }

   # if scale is default, take it from source list
   if {$tscale == $FXP_DEFAULT} {
      set tmp [lindex $fxp_list $FXP_SCALE]
   } else {
      set tmp $tscale
   }

   # calculate where the decimal point should be
   set l_num [lindex $result $FXP_VAL]
   set len [string length $l_num]
   set dpos [expr $len - $tmp]

   if {$dpos < 0} {
      logman "$mname:exit(0):out of range(tscale)"
      return 0
   }

   # copy inserting decimal point
   set i 0

   for {set j 0} {$i < $len} {incr j} {
      if {$j == $dpos} {
         append lk "."
      } else {
         append lk [string index $l_num $i]
         incr i
      }
   }

   logman "$mname:normal exit(1)"
   return 1
}

proc fxp_add {ad1 ad2} {
   # Add two fixed point numbers and return the result. The result
   # will be returned in a new 'fxp' list. Returns the new 'fxp'
   # list upon success, an empty string otherwise.
   # Note that 'ad2' is subtracted from 'ad1'.
   global FXP_OP_ADD
   set mname "fxp_add"

   logman "$mname:enter"
   set result [fxp_math $ad1 $ad2 $FXP_OP_ADD]
   logman "$mname:normal exit"
   return $result
}

proc fxp_sub {ad1 ad2} {
   # Subtract two fixed point numbers and return the result. The result
   # will be returned in a new 'fxp' list. Returns the new 'fxp'
   # list upon success, an empty string otherwise.
   global FXP_OP_SUB
   set mname "fxp_sub"

   logman "$mname:enter"
   set result [fxp_math $ad1 $ad2 $FXP_OP_SUB]
   logman "$mname:normal exit"
   return $result
}

proc fxp_mul {ad1 ad2} {
   # Multiply two fixed point numbers and return the result. The result
   # will be returned in a new 'fxp' list. Returns the new 'fxp'
   # list upon success, an empty string otherwise.
   global FXP_OP_MUL
   set mname "fxp_mul"

   logman "$mname:enter"
   set result [fxp_math $ad1 $ad2 $FXP_OP_MUL]
   logman "$mname:normal exit"
   return $result
}

proc fxp_div {ad1 ad2} {
   # Divide two fixed point numbers and return the result. The result
   # will be returned in a new 'fxp' list. Returns the new 'fxp'
   # list upon success, an empty string otherwise.
   # Note that 'ad1' is divided by 'ad2'.
   global FXP_OP_DIV
   set mname "fxp_div"

   logman "$mname:enter"
   set result [fxp_math $ad1 $ad2 $FXP_OP_DIV]
   logman "$mname:normal exit"
   return $result
}

proc fxp_eq {fxp1 fxp2} {
   # Perform an equality test on two fixed point numbers.
   # Returns 1 if the two are equal, 0 otherwise.
   global FXP_OP_EQ
   set mname "fxp_eq"

   logman "$mname:enter"
   set ret [fxp_comp $fxp1 $fxp2 $FXP_OP_EQ]
   logman "$mname:normal exit($ret)"
   return $ret
}

proc fxp_lt {fxp1 fxp2} {
   # Determine whether one fixed point number is less than the other.
   # Returns 1 if 'fxp1' is less than 'fxp2', 0 otherwise.
   global FXP_OP_LT
   set mname "fxp_lt"

   logman "$mname:enter"
   set ret [fxp_comp $fxp1 $fxp2 $FXP_OP_LT]
   logman "$mname:normal exit($ret)"
   return $ret
}

proc fxp_gt {fxp1 fxp2} {
   # Determine whether one fixed point number is greater than the other.
   # Returns 1 if 'fxp1' is greather than 'fxp2', 0 otherwise.
   global FXP_OP_GT
   set mname "fxp_gt"

   logman "$mname:enter"
   set ret [fxp_comp $fxp1 $fxp2 $FXP_OP_GT]
   logman "$mname:normal exit($ret)"
   return $ret
}

proc fxp_le {fxp1 fxp2} {
   # Determine whether one fixed point number is less than 
   # or equal to the other.
   # Returns 1 if 'fxp1' is less than or equal to 'fxp2',
   # 0 otherwise.
   global FXP_OP_LE
   set mname "fxp_le"

   logman "$mname:enter"
   set ret [fxp_comp $fxp1 $fxp2 $FXP_OP_LE]
   logman "$mname:normal exit($ret)"
   return $ret
}

proc fxp_ge {fxp1 fxp2} {
   # Determine whether one fixed point number is greater than
   # or equal to the other.
   # Returns 1 if 'fxp1' is greather than or equal to 'fxp2',
   # 0 otherwise.
   global FXP_OP_GE
   set mname "fxp_gt"

   logman "$mname:enter"
   set ret [fxp_comp $fxp1 $fxp2 $FXP_OP_GE]
   logman "$mname:normal exit($ret)"
   return $ret
}

proc fxp_math {ad1 ad2 opcode} {
   # Perform math on two fixed point numbers and return the result.
   # The result will be returned in a new 'fxp' list. Returns the
   # new 'fxp' list upon success, an empty string otherwise.
   global FXP_OP_ADD FXP_OP_SUB FXP_OP_MUL FXP_OP_DIV
   set mname "fxp_math"

   logman "$mname:enter"

   if {![fxp_get_list $ad1 ad1_num ad1_scale]} {
      logman "$mname:exit:bad rc(0) from fxp_get_list(ad1)"
      return ""
   }

   if {![fxp_get_list $ad2 ad2_num ad2_scale]} {
      logman "$mname:exit:bad rc(0) from fxp_get_list(ad2)"
      return ""
   }

   # calculate the common scale to both numbers which is the
   # largest of the two
   if {$ad1_scale > $ad2_scale} {
      set cscale $ad1_scale
   } else {
      set cscale $ad2_scale
   }

   set nscale $cscale

   # duplicate the two input numbers changing their scale
   # to the largest common scale
   set nad1 $ad1

   # if dividing, increase scale of dividend
   if {$opcode == $FXP_OP_DIV} {
      set nscale [expr $ad1_scale + $ad2_scale + 1]
   }

   if {![fxp_scale nad1 $nscale]} {
      logman "$mname:exit:bad rc from fxp_scale(nad1)"
      return ""
   }

   set nad2 $ad2

   if {![fxp_scale nad2 $cscale]} {
      logman "$mname:exit:bad rc from fxp_scale(nad2)"
      return ""
   }

   logman "$mname:ad1 contents"
   fxp_debug $ad1
   logman "$mname:nad1 contents"
   fxp_debug $nad1
   logman "$mname:ad2 contents"
   fxp_debug $ad2
   logman "$mname:nad2 contents"
   fxp_debug $nad2
   
   if {![fxp_get_list $nad1 nad1_num nad1_scale]} {
      logman "$mname:exit:bad rc(0) from fxp_get_list(nad1)"
      return ""
   }

   if {![fxp_get_list $nad2 nad2_num nad2_scale]} {
      logman "$mname:exit:bad rc(0) from fxp_get_list(nad2)"
      return ""
   }

   # 'result_num' and 'result_scale' will hold the result
   set result_scale $nad1_scale
   set readjust 0

   # if the second operand is zero, do not calc and the result
   # will be the contents of the first operand
   if {$nad2_num == 0} {
      logman "$mname:second operand is zero,no calc"
      set result_num $nad1_num

      # if the first operand is zero, set scale of result to zero
      if {$nad1_num == 0} {
         set readjust 1
         set result_scale 0
      }
   } else {
      # calculate result value
      # I have to use the single line version of the 'switch'
      # statement cause this #^&@ language does not do var subs
      # with the multi-line version
      switch $opcode \
         $FXP_OP_ADD {set result_num [expr $nad1_num + $nad2_num]} \
         $FXP_OP_SUB {set result_num [expr $nad1_num - $nad2_num]} \
         $FXP_OP_MUL {set result_num [expr $nad1_num * $nad2_num]; \
                      set result_scale [expr $result_scale + $nad2_scale]} \
         $FXP_OP_DIV {set result_num [expr $nad1_num / $nad2_num]; \
                      if {$ad1_scale < $ad2_scale} \
                          {set result_scale [expr $ad1_scale + 1]}; \
                      if {$ad1_scale >= $ad2_scale} \
                          {set result_scale [expr $ad2_scale + 1]}} \
         default {logman "$mname:exit:unknown math operation"; \
            return ""}
   }

   logman "$mname:after op:val=$result_num,scale=$result_scale"
   set result [list $result_num $result_scale]

   # readjust scale if necessary
   if {$readjust} {
      if {![fxp_scale result $nad1_scale]} {
         logman "$mname:exit:readjust scale error"
         return ""
      }
   }

   logman "$mname:result contents"
   fxp_debug $result
   return $result
}

proc fxp_comp {ad1 ad2 opcode} {
   # Perform a comparison on two fixed point numbers and return the result.
   # Returns 1 if the comparison was positive, 0 otherwise.
   global FXP_OP_EQ FXP_OP_LT FXP_OP_GT FXP_OP_LE FXP_OP_GE
   set mname "fxp_comp"

   logman "$mname:enter"

   if {![fxp_get_list $ad1 ad1_num ad1_scale]} {
      logman "$mname:exit:bad rc(0) from fxp_get_list(ad1)"
      return 0
   }

   if {![fxp_get_list $ad2 ad2_num ad2_scale]} {
      logman "$mname:exit:bad rc(0) from fxp_get_list(ad2)"
      return 0
   }

   # calculate the common scale to both numbers which is the
   # largest of the two
   if {$ad1_scale > $ad2_scale} {
      set nscale $ad1_scale
   } else {
      set nscale $ad2_scale
   }

   # duplicate the two input numbers changing their scale
   # to the largest common scale
   set nad1 $ad1

   if {![fxp_scale nad1 $nscale]} {
      logman "$mname:exit:bad rc from fxp_scale(nad1)"
      return 0
   }

   set nad2 $ad2

   if {![fxp_scale nad2 $nscale]} {
      logman "$mname:exit:bad rc from fxp_scale(nad2)"
      return 0
   }

   logman "$mname:ad1 contents"
   fxp_debug $ad1
   logman "$mname:nad1 contents"
   fxp_debug $nad1
   logman "$mname:ad2 contents"
   fxp_debug $ad2
   logman "$mname:nad2 contents"
   fxp_debug $nad2
   set ret 0
   
   if {![fxp_get_list $nad1 nad1_num nad1_scale]} {
      logman "$mname:exit:bad rc(0) from fxp_get_list(nad1)"
      return 0
   }

   if {![fxp_get_list $nad2 nad2_num nad2_scale]} {
      logman "$mname:exit:bad rc(0) from fxp_get_list(nad2)"
      return 0
   }

   # compare
   switch $opcode \
      $FXP_OP_EQ {if {$nad1_num == $nad2_num} {set ret 1}} \
      $FXP_OP_LT {if {$nad1_num < $nad2_num} {set ret 1}} \
      $FXP_OP_GT {if {$nad1_num > $nad2_num} {set ret 1}} \
      $FXP_OP_LE {if {$nad1_num <= $nad2_num} {set ret 1}} \
      $FXP_OP_GE {if {$nad1_num >= $nad2_num} {set ret 1}} \
      default {logman "$mname:exit:unknown comparison"; \
            return $ret}

   logman "$mname:normal exit($ret)"
   return $ret
}

proc fxp_scale {fxp_list tscale} {
   # Change the scale of a fixed point number. Digits
   # will be added or taken away from the right side
   # of the number. Returns 1 upon success, 0
   # otherwise.
   global FXP_DEFAULT
   upvar $fxp_list lk
   set mname "fxp_scale"
   
   logman "$mname:enter"

   if {![fxp_get_list $lk l_num l_scale]} {
      logman "$mname:exit(0):bad rc(0) from fxp_get_list(fxp_list->lk)"
      return 0
   }
        
   if {$tscale == $FXP_DEFAULT || $tscale == $l_scale} {
      logman "$mname:exit(1):default scale requested"
      return 1
   }

   set len [string length $l_num]

   # scaling to less than current
   if {$tscale < $l_scale} {
      # if specified scale is zero, make integer
      if {$tscale == 0} {
         set tmp $l_scale
      } else {
         set tmp [expr $l_scale - $tscale]
      }

      # if number is only a decimal and we are making an int,
      # set to zero
      if {$tmp == $len} {
         set l_num 0
      } else {
         # grab required left side of string
         set l_num [string range $l_num 0 end-$tmp]
      }

      set l_scale $tscale
      set lk [list $l_num $l_scale]
      logman "$mname:normal exit(1):scale less than current"
      return 1
   }

   # scaling to more than current
   set tmp [expr $tscale - $l_scale + [string length $l_num]]
   set l_num [rpad $l_num 0 $tmp]
   set l_scale $tscale
   set lk [list $l_num $l_scale]
   logman "$mname:normal exit(1):scale greater than current"
   return 1
}

proc fxp_get_list {fxp_list out_fxp out_scale} {
   # Extract the number and the scale from an fxp list and
   # place into 'out_fxp' and 'out_scale'. Returns 1 upon
   # success, 0 otherwise.
   global FXP_VAL FXP_SCALE
   upvar $out_fxp lk
   upvar $out_scale lk2
   set mname "fxp_get_list"
   set lk ""
   set lk2 ""

   if {[llength $fxp_list] < 2} {
      return 0
   }

   set fxp_num [lindex $fxp_list $FXP_VAL]

   if {![num $fxp_num]} {
      return 0
   }

   set scale [lindex $fxp_list $FXP_SCALE]

   if {![num $scale]} {
      return 0
   }

   set lk $fxp_num
   set lk2 $scale
   return 1
}

proc fxp_debug {f1} {
   # Output contents of an 'fxp' list to the log.
   set mname "fxp_debug"

   if {![fxp_get_list $f1 f1_num f1_scale]} {
      logman "$mname:exit:bad rc(0) from fxp_get_list"
      return
   }

   logman "$mname:val=$f1_num,scale=$f1_scale"
}

proc fxp_remove_decimal {cfin} {
   # Copy a number removing the decimal point. Returns the input string
   # (minus the decimal point) upon success. Returns an empty string
   # upon failure.
   set len [string length $cfin]
   set cfout ""

   for {set i 0} {$i < $len} {incr i} {
      set ch [string index $cfin $i]

      if {$ch != "."} {
         append cfout $ch
      }
   }

   return $cfout
}
