;; Copyright (C) 1994 M. Hagiya, W. Schelter, T. Yuasa

;; This file is part of GNU Common Lisp, herein referred to as GCL
;;
;; GCL is free software; you can redistribute it and/or modify it under
;;  the terms of the GNU LIBRARY GENERAL PUBLIC LICENSE as published by
;; the Free Software Foundation; either version 2, or (at your option)
;; any later version.
;; 
;; GCL 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 Library General Public 
;; License for more details.
;; 
;; You should have received a copy of the GNU Library General Public License 
;; along with GCL; see the file COPYING.  If not, write to the Free Software
;; Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.


;;;;    numlib.lsp
;;;;
;;;;                           number routines


(in-package :si)


(proclaim '(optimize (safety 2) (space 3)))


(defconstant imag-one #C(0.0d0 1.0d0))


(defun isqrt (i)
       (unless (and (integerp i) (>= i 0))
               (error "~S is not a non-negative integer." i))
       (if (zerop i)
           0
           (let ((n (integer-length i)))
                (do ((x (ash 1 (ceiling n 2)))
                     (y))
                    (nil)
                    (setq y (floor i x))
                    (when (<= x y)
                          (return x))
                    (setq x (floor (+ x y) 2))))))


(defun abs (z)
  (cond ((complexp z)
	 ;; Compute (sqrt (+ (* x x) (* y y))) carefully to prevent
	 ;; overflow!
	 (let* ((x (abs (realpart z)))
		(y (abs (imagpart z))))
	   (if (< x y)
	       (rotatef x y))
	   (if (zerop x)
	       x
	       (let ((r (/  y x)))
		 (* x (sqrt (+ 1 (* r r))))))))
	(t				; Should this be (realp z) instead of t?
	 (if (minusp z)
	     (- z)
	     z))))


(defun phase (x)
       (atan (imagpart x) (realpart x)))

(defun signum (x) (if (zerop x) x (/ x (abs x))))

(defun cis (x) (exp (* imag-one x)))

(defun real-asinh (x)
  (declare (real x))
  (float (log (+ x (sqrt (+ 1.0 (* x x))))) (float x)))

(defun asin (z)
  (declare (optimize (safety 1)))
  (check-type z number)
  (if (unless (complexp z) (<= -1 z 1))
      (atan z (sqrt (- 1 (* z z))))
    (let* ((a (sqrt (- 1 z)))
	   (b (sqrt (+ 1 z))))
      (complex (atan (realpart z) (realpart (* a b)))
	       (real-asinh (imagpart (* (conjugate a) b)))))))

(defun acos (z)
  (declare (optimize (safety 1)))
  (check-type z number)
  (if (unless (complexp z) (<= -1 z 1))
      (* 2 (atan (- 1 z) (sqrt (- 1 (* z z)))))
    (let* ((a (sqrt (- 1 z)))
	   (b (sqrt (+ 1 z))))
      (complex (* 2 (atan (realpart a) (realpart b)))
	       (real-asinh (imagpart (* (conjugate b) a)))))))

(defun asinh (x)
  (declare (optimize (safety 1)))
  (check-type x number)
  (if (realp x)
      (real-asinh x)
    (let* ((r (asin (complex (- (imagpart x)) (realpart x)))))
      (complex (imagpart r) (- (realpart r))))))

(defun acosh (z)
  (declare (optimize (safety 1)))
  (check-type z number)
  (if (unless (complexp z) (>= z 1))
      (real-asinh (sqrt (- (* z z) 1)))
    (let* ((a (sqrt (- z 1)))
	   (b (sqrt (+ z 1))))
      (complex (real-asinh (realpart (* (conjugate a) b))) (* 2 (atan (imagpart a) (realpart b)))))))

(defun atanh (x)
  (declare (optimize (safety 1)))
  (check-type x number)
  (if (unless (complexp x) (< -1 x 1))
      (/ (log (/ (+ 1 x) (- 1 x))) 2)
    (/ (- (log (+ 1 x)) (log (- 1 x))) 2)))


(defun sinh (z)
  (cond ((complexp z)
	 ;; For complex Z, compute the real and imaginary parts
	 ;; separately to get better precision.
	 (let ((x (realpart z))
	       (y (imagpart z)))
	   (complex (* (sinh x) (cos y))
		    (* (cosh x) (sin y)))))
	(t				; Should this be (realp z) instead of t?
	 (let ((limit #.(expt (* double-float-epsilon 45/2) 1/5)))
	   (if (< (- limit) z limit)
	       ;; For this region, write use the fact that sinh z =
	       ;; z*exp(z)*[(1 - exp(-2z))/(2z)].  Then use the first
	       ;; 4 terms in the Taylor series expansion of
	       ;; (1-exp(-2z))/2/z.  series expansion of (1 -
	       ;; exp(2*x)).  This is needed because there is severe
	       ;; roundoff error calculating (1 - exp(-2z)) for z near
	       ;; 0.
	       (* z (exp z)
		  (- 1 (* z
			  (- 1 (* z
				  (- 2/3 (* z
					    (- 1/3 (* 2/15 z)))))))))
	       (let ((e (exp z)))
		 (* 1/2 (- e (/ e)))))))))

;(defun sinh (x) (/ (- (exp x) (exp (- x))) 2.0d0))

(defun cosh (z)
  (cond ((complexp z)
	 ;; For complex Z, compute the real and imaginary parts
	 ;; separately to get better precision.
	 (let ((x (realpart z))
	       (y (imagpart z)))
	   (complex (* (cosh x) (cos y))
		    (* (sinh x) (sin y)))))
	(t				; Should this be (realp z) instead of t?
	 ;; For real Z, there's no chance of round-off error, so
	 ;; direct evaluation is ok.
	 (let ((e (exp z)))
	   (* 1/2 (+ e (/ e)))))))
;(defun cosh (x) (/ (+ (exp x) (exp (- x))) 2.0d0))
(defun tanh (x) (/ (sinh x) (cosh x)))


(defun rational (x)
  (etypecase x
    (float	  
      (multiple-value-bind (i e s) (integer-decode-float x)
			   (if (>= s 0)
			       (* i (expt (float-radix x) e))
			     (- (* i (expt (float-radix x) e))))))
    (rational x)))


(setf (symbol-function 'rationalize) (symbol-function 'rational))

;; although the following is correct code in that it approximates the
;; x to within eps, it does not preserve (eql (float (rationalize x) x) x)
;; since the test for eql is more strict than the float-epsilon

;;; Rationalize originally by Skef Wholey.
;;; Obtained from Daniel L. Weinreb.
;(defun rationalize (x)
;  (typecase x
;    (rational x)
;    (short-float (rationalize-float x short-float-epsilon 1.0s0))
;    (long-float (rationalize-float x long-float-epsilon 1.0d0))
;    (otherwise (error "~S is neither rational nor float." x))))
;
;(defun rationalize-float (x eps one)
;  (cond ((minusp x) (- (rationalize (- x))))
;        ((zerop x) 0)
;        (t (let ((y ())
;                 (a ()))
;             (do ((xx x (setq y (/ one
;                                   (- xx (float a x)))))
;                  (num (setq a (truncate x))
;                       (+ (* (setq a (truncate y)) num) onum))
;                  (den 1 (+ (* a den) oden))
;                  (onum 1 num)
;                  (oden 0 den))
;                 ((and (not (zerop den))
;                       (not (> (abs (/ (- x (/ (float num x)
;                                               (float den x)))
;                                       x))
;                               eps)))
;                  (/ num den)))))))


(defun ffloor (x &optional (y 1.0s0))
       (multiple-value-bind (i r) (floor (float x) (float y))
        (values (float i r) r)))

(defun fceiling (x &optional (y 1.0s0))
       (multiple-value-bind (i r) (ceiling (float x) (float y))
        (values (float i r) r)))

(defun ftruncate (x &optional (y 1.0s0))
       (multiple-value-bind (i r) (truncate (float x) (float y))
        (values (float i r) r)))

(defun fround (x &optional (y 1.0s0))
       (multiple-value-bind (i r) (round (float x) (float y))
        (values (float i r) r)))


(defun lognand (x y) (boole boole-nand x y))
(defun lognor (x y) (boole boole-nor x y))
(defun logandc1 (x y) (boole boole-andc1 x y))
(defun logandc2 (x y) (boole boole-andc2 x y))
(defun logorc1 (x y) (boole boole-orc1 x y))
(defun logorc2 (x y) (boole boole-orc2 x y))

(defun lognot (x) (logxor -1 x))
(defun logtest (x y) (not (zerop (logand x y))))


(defun byte (size position)
  (cons size position))

(defun byte-size (bytespec)
  (car bytespec))

(defun byte-position (bytespec)
  (cdr bytespec))

;; (defun ldb (bytespec integer)
;;   (logandc2 (ash integer (- (byte-position bytespec)))
;;             (- (ash 1 (byte-size bytespec)))))

;; (defun ldb-test (bytespec integer)
;;   (not (zerop (ldb bytespec integer))))

;; (defun mask-field (bytespec integer)
;;   (ash (ldb bytespec integer) (byte-position bytespec)))

;; (defun dpb (newbyte bytespec integer)
;;   (logxor integer
;;           (mask-field bytespec integer)
;;           (ash (logandc2 newbyte
;;                          (- (ash 1 (byte-size bytespec))))
;;                (byte-position bytespec))))

;; (defun deposit-field (newbyte bytespec integer)
;;   (dpb (ash newbyte (- (byte-position bytespec))) bytespec integer))


(defun ldb (bytespec integer)
  (logand (ash integer (- (byte-position bytespec)))
	  (1- (ash 1 (byte-size bytespec)))))
(defun ldb-test (bytespec integer)
  (not (zerop (ldb bytespec integer))))
(defun dpb (newbyte bytespec integer &aux (z (1- (ash 1 (byte-size bytespec)))))
  (logior (logandc2 integer (ash z (byte-position bytespec)))
	  (ash (logand newbyte z) (byte-position bytespec))))
(defun deposit-field (newbyte bytespec integer &aux (z (ash (1- (ash 1 (byte-size bytespec))) (byte-position bytespec))))
  (logior (logandc2 integer z) (logand newbyte z)))
