;;;;	-------------------------------
;;;;	Copyright (c) 2000-2003 Corman Technologies
;;;;	All rights reserved.
;;;;	-------------------------------
;;;;
;;;;	File:		math2.lisp
;;;;	Contents:	Corman Lisp math functions.
;;;;	History:	4/1/98  RGC  Created.
;;;;				4/25/01 RGC  Added LOGBITP, LOGCOUNT, LDB, (SETF LDB), EXPT, LDB-TEST
;;;;				8/15/01 RGC  Fixed MOD function.
;;;;				5/29/02 RGC	 Added Mayer Goldberg's complex trig function implementation.
;;;;							 Reimplemented trig functions to incorporate these.
;;;;                12/19/02 RGC Incorporated JP Massar's fix to %chars-to-float.
;;;;                11/03/03 RGC Fixed a problem with %chars-to-float implementation.
;;;;

(in-package "C-TYPES")

;;(defctype double :double-float)
;;(defctype float  :single-float)

(in-package "COMMON-LISP")
 
;;;
;;;	Common Lisp LOG function
;;;
;(defconstant e (exp 1))

(ct:defun-dll c-log ((d :double-float))
   :return-type :double-float
   :library-name "msvcrt.dll"
   :entry-name "log"
   :linkage-type :c)

(ct:defun-dll c-sin ((d :double-float))
   :return-type :double-float
   :library-name "msvcrt.dll"
   :entry-name "sin"
   :linkage-type :c)

(ct:defun-dll c-cos ((d :double-float))
   :return-type :double-float
   :library-name "msvcrt.dll"
   :entry-name "cos"
   :linkage-type :c)

(ct:defun-dll c-atan2 ((d1 :double-float)(d2 :double-float))
   :return-type :double-float
   :library-name "msvcrt.dll"
   :entry-name "atan2"
   :linkage-type :c)

(ct:defun-dll c-atan ((d1 :double-float))
   :return-type :double-float
   :library-name "msvcrt.dll"
   :entry-name "atan"
   :linkage-type :c)

(ct:defun-dll c-pow ((x :double-float)(y :double-float))
   :return-type :double-float
   :library-name "msvcrt.dll"
   :entry-name "pow"
   :linkage-type :c)

(ct:defun-dll c-exp ((x :double-float))
   :return-type :double-float
   :library-name "msvcrt.dll"
   :entry-name "exp"
   :linkage-type :c)

(ct:defun-dll c-atof ((d (:char *)))
   :return-type :double-float
   :library-name "msvcrt.dll"
   :entry-name "atof"
   :linkage-type :c)

;;;
;;;	Common Lisp LOG function.
;;;
(defun log (number &optional base)
	(if base 
		(if (zerop base)
			0
			(/ (log number) (log base)))
		(if (or (complexp number)(minusp number))
			(complex (c-log (float (abs number) 0d0)) (phase number))
			(c-log (float number 0d0)))))

;;;
;;;	Common Lisp ATAN function.
;;; arctan x = -i log  ((1+ix) sqrt(1/(1+x^2)) )
;;; 
(defun atan (x &optional y)
	(if y
		(c-atan2 (float x 0d0)(float y 0d0))
		(if (complexp x)
			(let* ((i #C(0 1))
				   (ix (* i x)))
				(* (- i)
					(log (* (+ 1 ix) (sqrt (/ 1 (+ 1 (* x x))))))))
			(c-atan (float x 0d0)))))

;;;;
;;;; Common Lisp LOGBITP function
;;;;
(defun logbitp (index integer)
	(unless (integerp integer)
		(error 'type-error :datum integer :expected-type 'integer))
	(unless (and (fixnump index) (>= index 0))
		(error 'type-error :datum index :expected-type '(integer 0 *)))
	(> (logand integer (expt 2 index)) 0))

;;;;
;;;; Common Lisp LOGCOUNT function
;;;;
(defun logcount (integer)
	(unless (integerp integer)
		(error 'type-error :datum integer :expected-type 'integer))
	;; if negative, use two's complement to flip
	(do ((x (if (< integer 0) (- (+ integer 1)) integer)(ash x -1))
		 (count 0 (+ count (logand x 1))))
		((= x 0) count)))

;;;;
;;;; Common Lisp LDB function
;;;;
(defun ldb (bytespec integer)
	(let ((mask (- (ash 1 (byte-size bytespec)) 1)))
		(logand (ash integer (- (byte-position bytespec))) mask)))

;;;;
;;;; Common Lisp (SETF LDB) function
;;;;
(defmacro |(SETF LDB)| (new-byte bytespec place)
	(if (and (consp place) (some #'consp place))
		(let ((retval (%once-only-forms place))
			  (sym (gensym)))
			`(let ,(car retval)
				(let ((,sym ,new-byte)) 
					(setf ,(cdr retval) (dpb ,sym ,bytespec ,(cdr retval)))
					,sym)))
		(let ((sym (gensym)))
			`(let ((,sym ,new-byte))
				(setf ,place (dpb ,sym ,bytespec ,place))
				,sym))))

(register-setf-function 'ldb '|(SETF LDB)|)


;;;;
;;;; Common Lisp LDB-TEST function
;;;;
(defun ldb-test (bytespec integer)
	(/= (ldb bytespec integer) 0))

(defun complex-expt (base power)
	(let* ((rbase (float (realpart base) 0d0))
		   (ibase (float (imagpart base) 0d0))
		   (rpower (float (realpart power) 0d0))
		   (ipower (float (imagpart power) 0d0))
		   abs)
		(setf abs
			(cond ((= rbase 0) (- (abs ibase)))
				  ((= ibase 0) (abs rbase))
				  ((> rbase ibase)
				   (* rbase (sqrt (+ 1 (c-pow (/ ibase rbase) 2d0)))))
				  (t 
				   (* ibase (sqrt (+ 1 (c-pow (/ rbase ibase) 2d0)))))))
		(let* ((rlog (c-log abs))
			   (ilog (c-atan2 ibase rbase))
			   (rphase (c-exp (- (* rlog rpower) (* ilog ipower))))
			   (iphase (+ (* rlog ipower) (* ilog rpower))))
			(complex (* rphase (cos iphase)) 
				     (* rphase (sin iphase))))))

;;;;
;;;; Common Lisp EXPT function
;;;;
(defun expt (base power)
	(unless (numberp base)
		(error 'type-error :datum base :expected-type 'number))
	(unless (numberp power)
		(error 'type-error :datum power :expected-type 'number))
	(if (and (integerp power)
			(or (rationalp base)
				(and (complexp base) 
					(rationalp (realpart base))
					(rationalp (imagpart base)))))
		;; calculate exact result
		(if (< power 0)
			(do ((result 1)
				 (p (- power)))
				((= p 0) (/ 1 result))
				(if (oddp p)
					(setf result (* result base)))
				(setf p (ash p -1))
				(setf base (* base base)))
			(do ((result 1))
				((= power 0) result)
				(if (oddp power)
					(setf result (* result base)))
				(setf power (ash power -1))
				(setf base (* base base))))
		
		;; approximate result
		(if (or (complexp base) 
				(complexp power)
				(and (< base 0) (not (integerp power))))
			(complex-expt base power)
			(c-pow (float base 0d0) (float power 0d0)))))

;;;
;;;	Both arguments should be fixnums.
;;;
(x86:defasm pos-fixnum-logbitp (index n)
	{
		push	ebp
		mov		ebp, esp
		mov		ecx, [ebp + (+ ARGS_OFFSET 4)]	;; ecx = index
		cmp		ecx, 232						;; 29, tagged
		jg		short :false
		mov		edx, [ebp + ARGS_OFFSET]		;; edx = num
		mov		eax, 8
		shr		ecx, 3
		shl		eax, cl							;; eax has mask
		and		eax, edx
		jne		short :true
	:false
		mov		eax, [esi]
		jmp		short :t1
	:true
		mov		eax, [esi + t-offset]
	:t1
		mov		ecx, 1
		pop		ebp
		ret
	})

;;;
;;;	First argument should be fixnum, the second a bignum.
;;;
(x86:defasm pos-bignum-logbitp (index n)
	{
		push	ebp
		mov		ebp, esp
		mov		edx, [ebp + (+ ARGS_OFFSET 4)]	;; edx = index
		mov		ecx, edx
		shr		edx, 8							;; edx = word offset (untagged)
		shl		edx, 4							;; edx = word offset (tagged) * 2
		mov		eax, [ebp + ARGS_OFFSET]		;; eax = num
   		mov		eax, [eax + (uvector-offset cl::bignum-num-cells-offset)]
		cmp		edx, eax
		jge		short :false 
		shr		edx, 2							;; edx = word offset (untagged) * 4
		and		ecx, #xff
		shr		ecx, 3							;; ecx = index (untagged)
		mov		ebx, 1
		mov		eax, [ebp + ARGS_OFFSET]		;; eax = num
		begin-atomic
		mov		eax, [eax + edx + (uvector-offset cl::bignum-first-cell-offset)]
		shl		ebx, cl							;; ebx = bit mask
		and		eax, ebx		
		jne		short :true
	:false
		mov		eax, [esi]
		jmp		short :t1
	:true
		mov		eax, [esi + t-offset]
	:t1
		end-atomic
		mov		ecx, 1
		pop		ebp
		ret
    
	})

(defun logbitp (index integer)
	(unless (integerp integer)
		(error 'type-error :datum integer :expected-type 'integer))
	(unless (and (fixnump index) (>= index 0))
		(error 'type-error :datum index :expected-type '(integer 0 *)))
	(if (fixnump integer)
		(if (< integer 0)
			(not (pos-fixnum-logbitp index (+ 1 (lognot integer))))	;; use twos complement
			(pos-fixnum-logbitp index integer))
		(if (< integer 0)
			(not (pos-bignum-logbitp index (+ 1 (lognot integer))))	;; use twos complement
			(pos-bignum-logbitp index integer))))

;;;
;;; Fixnum MOD
;;;
(x86:defasm mod-fixnums (fixnum divisor)
	{
		push	ebp
		mov		ebp, esp
		mov		eax, [ebp + (+ ARGS_OFFSET 4)]	;; eax = fixnum
		xor		edx, edx
		begin-atomic
		div		[ebp + ARGS_OFFSET]
		mov		eax, edx
		end-atomic
		mov		ecx, 1
		pop		ebp
		ret
	})

;;;
;;;	Common Lisp MOD function.
;;;
(defun mod (number divisor)
	(if (and (integerp number) (integerp divisor))
		(if (or (bignump number)(bignump divisor))
			(if (>= number 0)
				(if (>= divisor 0)
					(mod-bignums number divisor)
					(- (mod-bignums number (- divisor)) (- divisor)))
				(if (>= divisor 0)
					(let ((result  (mod-bignums (- number) divisor)))      
						(if (eq result 0) 0 (- divisor result)))
					(- (mod-bignums (- number) (- divisor)))))
			(if (>= number 0)
				(if (>= divisor 0)
					(mod-fixnums number divisor)
					(- (mod-fixnums number (- divisor)) (- divisor)))
				(if (>= divisor 0)
					(let ((result  (mod-fixnums (- number) divisor)))      
						(if (eq result 0) 0 (- divisor result)))
					(- (mod-fixnums (- number) (- divisor))))))
		(multiple-value-bind (value remainder)
			(floor number divisor)
			(declare (ignore value))
			remainder)))

;; this overrides the kernel routine
(defun %chars-to-float (chars)
	(let ((default-format *read-default-float-format*)
		  (c chars)
		  (digits 0)
		  (precision 'single-float)
		  (exp-digits 0)
		  (decimal 0)
          (exponent-index nil)
          (index 0))
		(unless (member default-format '(short-float double-float single-float))
			(setf default-format 'single-float))
		(unless (listp chars) (error "Expected a list of characters"))
				
		;; skip +/- if present
		(when (or (char= (car c) #\+) (char= (car c) #\-))
			(setf c (cdr c))
            (incf index))
		
		;; check mantissa
		(do ((char (car c)(car c)))
			((null c))
			(if (digit-char-p char)
				(incf digits)
				(if (char= char #\.)
					(if (> (incf decimal) 1)
						(return-from %chars-to-float nil))	;; more than one decimal point!			
					(return)))
			(setf c (cdr c))
            (incf index))
		
		(if (= digits 0)
			(return-from %chars-to-float nil))
		
		;; get exponent
		(if c
			(let ((char (char-upcase (car c))))
				(if (member char '(#\E #\F #\L #\S #\D))
					(progn
						(setf precision
							(cond
								((char= char #\E) default-format)
								((char= char #\D) 'double-float)
								((char= char #\S) 'short-float)
								((char= char #\F) 'single-float)
								((char= char #\L) 'double-float)))
		                (when (null (cdr c)) (return-from %chars-to-float nil))
   						(setf exponent-index index)
						(setf c (cdr c))
                        (incf index)
						
						;; allow +/- on exponent
						(when (or (char= (car c) #\+) (char= (car c) #\-))
							(setf c (cdr c))
                            (incf index))
					
						;; check exponent digits
						(do ((char (car c)(car c)))
							((or (null c)(not (digit-char-p char))))
							(if (digit-char-p char)
								(incf exp-digits))
							(setf c (cdr c))
                            (incf index))
						(if (= exp-digits 0)
							(return-from %chars-to-float nil))))))
		
		(unless (null c)
			(return-from %chars-to-float nil))	;; extra chars at end
		
        (let ((str (concatenate 'string chars)))
            (if exponent-index
                (setf (elt str exponent-index) #\E))
    		(let ((d (c-atof (ct:lisp-string-to-c-string str))))
    			(case precision
    				(double-float d)
    				(single-float (float d 0f0))
    				(short-float (float d 0s0)))))))

;;; complex-trig.lisp
;;; Programmer: Mayer Goldberg, 2002

(defun complex-sin (x)
  (let* ((eix (exp (* #c(0.0 1.0) x)))
	 (e-ix (/ 1.0 eix)))
    (/ (- eix e-ix) (* #c(0.0 1.0) 2.0))))

(defun complex-cos (x)
  (let* ((eix (exp (* #c(0.0 1.0) x)))
	 (e-ix (/ 1.0 eix)))
    (/ (+ eix e-ix) 2.0)))

(defun complex-tan (x)
  (let* ((eix (exp (* #c(0.0 1.0) x)))
	 (e-ix (/ 1.0 eix)))
    (/ (- eix e-ix) (+ eix e-ix) #c(0.0 1.0))))

#| not used
(defun complex-asin (x)
  (- (* #c(0.0 1.0)
	(log (+ (* #c(0.0 1.0) x)
		(sqrt (- 1.0 (* x x))))))))

(defun complex-acos (x) (- (/ pi 2.0) (complex-asin x)))

(defun complex-atan (y)
	(/ (- (log (+ 1.0 (* #c(0 1) y)))
	      (log (- 1.0 (* #c(0 1) y))))
	   (* 2 #c(0 1))))
|#

;;;
;;; Common Lisp SIN function
;;;
(defun sin (x)
	(if (complexp x)
		(complex-sin x)
		(c-sin (float x 0d0))))

;;;
;;; Common Lisp COS function
;;;
(defun cos (x)
	(if (complexp x)
		(complex-cos x)
		(c-cos (float x 0d0))))

;;;
;;;	Common Lisp TAN function.
;;;
(defun tan (x)
	(if (complexp x)
		(complex-tan x)
		(/ (sin x) (cos x))))