;;;; PMSF-Lib --- PMSF Common Lisp Utility Library
;;;; This is copyrighted software.  See documentation for terms.
;;;; 
;;;; float-utilities.lisp --- Floating Point Utilities
;;;; 
;;;; $Id$

(cl:in-package #:pmsf-lib)

;;;; %File Description:
;;;; 
;;;; This file contains utilities for handling floating point
;;;; numbers and their processing.
;;;; 

;;;
;;; Prevent compilation errors in this file due to FPU issues
;;;

#+SBCL
(eval-when (:compile-toplevel :load-toplevel :execute)
  (sb-int:set-floating-point-modes :traps nil :rounding-mode :nearest
				   :fast-mode nil
				   #+X86 :precision #+X86 :53-bit))

(defconstant single-float-positive-infinity
  #+sbcl
  sb-ext:single-float-positive-infinity
  #-sbcl
  (+ most-positive-single-float most-positive-single-float)
  "Single Float Positivie Infinity")

(defconstant single-float-negative-infinity
  #+sbcl
  sb-ext:single-float-negative-infinity
  #-sbcl
  (- (+ most-positive-single-float most-positive-single-float))
  "Single Float Negative Infinity")

(defconstant single-float-nan
  ;; Ensure positive NaN
  (float-sign 1.0f0 
	      (/ single-float-negative-infinity single-float-negative-infinity))
  "Single Float NaN")

(defconstant double-float-positive-infinity
  #+sbcl
  sb-ext:double-float-positive-infinity
  #-sbcl
  (+ most-positive-double-float most-positive-double-float)
  "Double Float Positive Infinity")

(defconstant double-float-negative-infinity
  #+sbcl
  sb-ext:double-float-negative-infinity
  #-sbcl
  (- (+ most-positive-double-float most-positive-double-float))
  "Double Float Negative Infinity")

(defconstant double-float-nan
  ;; Ensure positive NaN
  (float-sign 1.0d0
	      (/ double-float-negative-infinity double-float-negative-infinity))
  "Double Float NaN")

(defun float-infinity-p (float)
  (and 
   (floatp float)
   #+sbcl
   (sb-ext:float-infinity-p float)
   #-sbcl
   (etypecase float
     (double-float (or (= float double-float-positive-infinity)
		       (= float double-float-negative-infinity)))
     (single-float (or (= float single-float-positive-infinity)
		       (= float single-float-negative-infinity))))))

(defun float-nan-p (float)
  (and 
   (floatp float)
   #+sbcl
   (sb-ext:float-nan-p float)
   #+lispworks
   (system::nan-p float)
   #-(or sbcl lispworks)
   (not (= float float))))

(defun float-denormalized-p (float)
  (and 
   (floatp float)
   #+sbcl
   (sb-ext:float-denormalized-p float)
   #-sbcl
   (and (not (float-nan-p float))
	(not (float-infinity-p float))
	(not (zerop float))
	(< (float-precision float) (float-digits float)))))

;;;
;;; Floating-Point Compare
;;;

(defun bit-decode-float (a)
  "Decode the floating point number A into three numbers MANTISSA, EXPONENT
and SIGN, so that A = SIGN * MANTISSA * 2^EXPONENT, and the returned MANTISSA
is an integer that is bit-identical to the mantissa field of the floating
point number in IEEE 754 with the hidden MSB included."
  (multiple-value-bind (mantissa exp sign) (decode-float a)
    (values (truncate (* mantissa (expt 2 (float-digits a))))
	    (- exp (float-digits a))
	    sign)))

(defun ieee-decode-float (a)
  "Decode the floating point number A into three numbers MANTISSA, EXPONENT
and SIGN, so that A = SIGN * MANTISSA * 2^EXPONENT, and the returned MANTISSA
is a floating point number with 1 <= MANTISSA < 2, thus matching the IEEE 754
definition of the mantissa."
  (multiple-value-bind (mantissa exp sign) (decode-float a)
    (values (* mantissa 2)
	    (1- exp)
	    sign)))

(defun float-equal (a b &key epsilon-bits)
  "Compare to floating point numbers A and B on equality, treating two NaNs
as equal as well.  If EPSILON-BITS is supplied it is the number of least
significant bits that the two mantissas are allowed to differ to still be
treated as equal for the purposes of the comparison."
  (unless (or (null epsilon-bits)
	      (<= 0 epsilon-bits (float-digits a)))
    (error "Illegal value ~S for epsilon-bits in call to float-equal."
	   epsilon-bits))
  (or
   ;; NaNs
   (and (float-nan-p a) (float-nan-p b))
   ;; Identical
   (eql a b)
   ;; Identical modulo epsilon bits differences
   (when epsilon-bits
     (unless (or (float-infinity-p a) (float-infinity-p b))
       (multiple-value-bind (mant-a exp-a sign-a) (bit-decode-float a)
	 (multiple-value-bind (mant-b exp-b sign-b) (bit-decode-float b)
	   (and (= exp-a exp-b)
		(= sign-a sign-b)
		(< (abs (- mant-a mant-b)) (expt 2 epsilon-bits)))))))))

;;;
;;; Floating-Point I/O
;;;

(defun read-double-float-from-string (string)
  (cond
    ((string-equal string "NaN")
     double-float-nan)
    ((or (string-equal string "+Inf")
	 (string-equal string "Inf"))
     double-float-positive-infinity)
    ((string-equal string "-Inf")
     double-float-negative-infinity)
    ((cl-ppcre:scan "^[-+]?([0-9]+)?[.]?(?(1)[0-9]*|[0-9]+)([eE][-+]?[0-9]+)?$"
		    string)
     (let ((*read-default-float-format* 'double-float)
	   (*read-eval* nil))
       #+lispworks
       (hcl:parse-float string)
       #-lispworks
       (coerce
        (read-from-string string)
	'double-float)))
    (t
     (error "Value ~S is not a valid floating point number." string))))

(defun read-single-float-from-string (string)
  (cond
    ((string-equal string "NaN")
     single-float-nan)
    ((or (string-equal string "+Inf")
	 (string-equal string "Inf"))
     single-float-positive-infinity)
    ((string-equal string "-Inf")
     single-float-negative-infinity)
    ((cl-ppcre:scan "^[-+]?([0-9]+)?[.]?(?(1)[0-9]*|[0-9]+)([eE][-+]?[0-9]+)?$"
		    string)
     (let ((*read-default-float-format* 'single-float)
	   (*read-eval* nil))
       #+lispworks
       (hcl:parse-float string)
       #-lispworks
       (coerce
	(read-from-string string)
	'single-float)))
    (t
     (error "Value ~S is not a valid floating point number." string))))

(defun write-float (x stream)
  (cond
    ((float-infinity-p x)
     (if (< x (float 0 x))
	 (write-string "-INF" stream)
	 (write-string "INF" stream)))
    ((float-nan-p x)
     (write-string "NaN" stream))
    (t
     (with-standard-io-syntax
       (let ((*read-default-float-format* (if (typep x 'double-float)
					      'double-float
					      'single-float)))
	 (write x :stream stream))))))

(defun float-integer-value (a)
  "Decode the floating point number A so as to produce the corresponding IEEE-754
bit pattern as an integer value."
  (let ((mantissa-bits (1- (float-digits a)))
        (exponent-bits (etypecase a (double-float 11) (single-float 8))))
    (multiple-value-bind (mantissa exponent sign)
        (cond
         ;; NaNs
         ((float-nan-p a)
          (values (1- (expt 2 mantissa-bits))
                  (1- (expt 2 exponent-bits))
                  0))
         ;; Infinities
         ((float-infinity-p a)
          (values 0
                  (1- (expt 2 exponent-bits))
                  (if (minusp (float-sign a)) 1 0)))
         ;; Zero is special as well
         ((zerop a)
          (values 0
                  0
                  (if (minusp (float-sign a)) 1 0)))
         ;; Normals and Denormals
         (t
          (multiple-value-bind (mantissa exp sign) (ieee-decode-float a)
            (if (not (plusp (+ exp (1- (expt 2 (1- exponent-bits))))))
                ;; Denormals
                (values (ldb (byte mantissa-bits 0) 
                             (truncate 
			      (* mantissa
				 (expt 2 
				       (+ mantissa-bits (1- exp)
					  (1- (expt 2 (1- exponent-bits))))))))
                        0
                        (if (minusp sign) 1 0))
                ;; Normals
                (values (ldb (byte mantissa-bits 0) 
                             (truncate (* mantissa (expt 2 mantissa-bits))))
                        (+ exp (1- (expt 2 (1- exponent-bits))))
                        (if (minusp sign) 1 0))))))
      ;; Construct result
      (dpb sign (byte 1 (+ mantissa-bits exponent-bits))
           (dpb exponent (byte exponent-bits mantissa-bits)
                mantissa)))))

(defun write-hex (x width stream &optional hide-prefix)
  (format stream "~:[0x~;~]~v,'0X"
          hide-prefix
          (ceiling width 4)
          (if (floatp x)
              (float-integer-value x)
              (ldb (byte width 0) x))))

(defun pprint-float (stream object &optional colon-p at-sign-p width)
  (if (and at-sign-p width)
      (write-hex object width stream colon-p)
      (write-float object stream)))

(defun integer-float-value (a type)
  "Encode an integer A that is constructed according to the IEEE-754 format
description into a floating point number of the given common lisp type."
  (let ((mantissa-bits (ecase type (double-float 52) (single-float 23)))
        (exponent-bits (ecase type (double-float 11) (single-float 8))))
    (multiple-value-bind (mantissa exponent sign)
        (values (ldb (byte mantissa-bits 0) a)
                (ldb (byte exponent-bits mantissa-bits) a)
                (ldb (byte 1 (+ exponent-bits mantissa-bits)) a))
      (cond
       ;; NaNs
       ((and (= exponent (1- (expt 2 exponent-bits))) (/= mantissa 0))
        (ecase type
          (double-float double-float-nan)
          (single-float single-float-nan)))
       ;; Infinities
       ((= exponent (1- (expt 2 exponent-bits)))
        (if (= sign 1)
            (ecase type
              (double-float double-float-negative-infinity)
              (single-float single-float-negative-infinity))
            (ecase type
              (double-float double-float-positive-infinity)
              (single-float single-float-positive-infinity))))
       ;; Zero is special as well
       ((and (zerop exponent) (zerop mantissa))
        (if (= sign 1)
            (ecase type
              (double-float -0.0d0)
              (single-float -0.0f0))
            (ecase type
              (double-float 0.0d0)
              (single-float 0.0f0))))
       ;; Denormals
       ((zerop exponent)
        (float-sign
         (coerce (if (= sign 1) -1 1) type)
         (scale-float (coerce mantissa type)
                      (- (+ (1- (expt 2 (1- exponent-bits))) (1- mantissa-bits))))))
       ;; Normals
       (t
        (float-sign
         (coerce (if (= sign 1) -1 1) type)
         (scale-float (coerce (dpb 1 (byte 1 mantissa-bits) mantissa) type)
                      (- exponent (1- (expt 2 (1- exponent-bits))) mantissa-bits))))))))

(defun signed-integer-value (integer width)
  (if (= 1 (ldb (byte 1 (1- width)) integer))
      (- integer (expt 2 width))
      integer))