;; 10/27/08  hacking r3tree.lisp to make faster.  Like r4tree, but using sign of 1st element, if neg, is short

;; works.  see r4tree.lisp for comments
;; try hacking more, declaring a few fixnums, perhaps overoptimistically
;; also, we have a program to pick out the smallest index, useful if
;; we want to program a DIVISION!!

(eval-when (:compile-toplevel :load-toplevel :execute)
  (declaim (optimize (speed 3)(safety 0)))
  (defconstant logsize 3))		; best for debugging, not bad for running some tests.

(defun init-rtree(key val) ;; always make full array; truncate it initially
  (let ((ans (init-rtree-mt)))
    (setf (aref ans 0) -1 (aref ans 1) val (aref ans 2) key)  ;;[-1, val, key,  0,...]
    ;; note negative. flag that it is a 2-fer node. store key in 3rd spot, not negative.
    ans))

(defun init-rtree-mt()			; empty rtree initialization
   (make-array #.(expt 2 logsize) :initial-element 0))
       
(defmacro leafp(x) `(numberp ,x))
(defmacro nodep(x) `(arrayp ,x))
(defmacro shortkey(h)`(eq ,h -1))

(defparameter *debug* nil)

(defun insert-rtree(key val tree &aux node)
  (declare(type (simple-array t (*)) tree) (fixnum key))
  (cond ((shortkey(aref tree 0))		; it is a single-entry node
	 (cond ((=  key (aref tree 2))
		(setf (aref tree 1) val)) ;; case where key matches exactly. replace value
	       (t
		(let ((oldkey (aref tree 2))
		      (oldval (aref tree 1)))
		  (setf (aref tree 0) 0) ;clear out the old
		  (setf (aref tree 1) 0)
		  (setf (aref tree 2) 0)
		  (if *debug*   (format t "~% oldkey= ~s key=~s tree=~s" oldkey key tree))
		  (setf tree(insert-rtree oldkey oldval tree)) ; re-insert the old key ;ok
		  (if *debug*  (format t "~%  tree=~s" tree))
		  (insert-rtree key val tree)))))
	;; the node (i.e. tree) is array >2 in length
	;; the key is small enough to fit in this array
	((< key #.(expt 2 logsize))
	 ;; we have found the level, or level-1 for the key.
	 (cond ((nodep (setf node (aref tree key))) ; not a leaf
		;; if a node here, must descend one level further in tree; key now is 0
		(insert-rtree 0 val node)); and update that node's location 0.
	       (t 
		(if *debug* (format t "~%case 1 key=~s" key))
		(setf (aref tree key) val))));; put it here.
	;;The key has too many bits to insert at this level. 
	;;Compute the subkey and separate the rest of the key by masking and shifting.
	(t (let ((ind(logand key #.(1- (expt 2 logsize))));rightmost logsize bits
		 (newkey (ash key #.(- logsize)))
		 )			; reduced key
	     (declare (fixnum ind newkey))
	     (if *debug* (format t "~%case 2, newkey=~s ind=~s tree=~s" newkey ind tree))
	     (setf node (aref tree ind))
	     (cond((nodep node) (setf (aref tree ind) (insert-rtree newkey val node)))
		  (t ;; a single item, must be moved
		   (let ()
		     (setf (aref tree ind) (init-rtree newkey  val))
		    ;;  (insert-rtree thiskey thisval node)
		      (unless (= node 0)(insert-rtree ind node tree)) ;; re-insert 
		     ))))))

  tree)

(defvar *mt* nil) ;empty tree
(defun t2()(setf *mt* (init-rtree-mt))(ir 1000 1000)(ir 2000 2000) (ir 1002 10002) (ordertree-nz *mt*))
(defun ir(a b)(insert-rtree a b *mt*))

(defun query-rtree(key tree) ;; returns 0 if no entry in the tree for that key
  (declare (fixnum key))
  (cond 
   ((leafp tree) 0)
   ((shortkey(aref tree 0))
    (if (= (aref tree 2) key)(aref tree 1) nil))

   ;; case 1: the key should be in this node.
   ((< key #.(expt 2 logsize)) 
    (if (arrayp (aref tree key));; if the node is an array
	(query-rtree 0 (aref tree key));; look in the zero location
      (aref tree key)));; the node is NOT an array: this is the value.
   (t (let* ((ind(logand key #.(1- (expt 2 logsize))));;otherwise compute the offset
	     (h (aref tree ind)))	; and the subtree to use for further search
	(if h (query-rtree (ash key #.(- logsize)) h) nil)))))

(defun map-rtree (fn mt);; order appears jumbled, actually reversed-radix order
  ;; apply fn, a function of 2 arguments, to key and val, for each entry in tree.
  ;; order is "radix reversed"
   (labels 
       ((tt1 (path displace mt);; path = path to the key's location
	  (declare (fixnum path displace))
	 (cond ((leafp mt) (unless (= 0 mt)(funcall fn path mt)));; function applied to key  and val.
	       ((shortkey(aref mt 0))
		(funcall fn (+ (ash (the fixnum (aref mt 2)) displace) path) (aref mt 1)))
	       (t;; must be an array
		(do ((i 0 (1+ i)))
		    ((= i #.(expt 2 logsize)))
		  (declare (fixnum i))
		  (tt1 (+ (the fixnum (ash i displace)) path) (+ displace logsize)(aref mt i)))))))
    (tt1 0 0 mt)))

#| example
(setf mt (init-rtree-mt))
(dotimes (i (expt 8 3))(insert-rtree i i mt))
mt  ;;; a fully populated radix-rtree with value n at location n, 0<= n < 8^3
(ordertree-nz mt)
|#

(defun ordertree-nz(mt)
  (declare (type (simple-array t (#.(expt 2 logsize))) tree))
  (let ((ans (make-array 10 :adjustable t :fill-pointer 0)))
    (declare  (type (array t (*)) ans))
   (map-rtree #'(lambda(key val)
		(vector-push-extend (cons key val) ans))
	    mt)
   (sort ans #'(lambda(a b)(declare (fixnum a b)) (< a b)) ;faster because of declarations?
	 ;;#'< 
	  :key #'car)))

(defun mul-rtree4 (r s)			;multiply two polynomials, in arrays
  (declare (optimize (speed 3)(safety 0)))
  (let* ((er (car r)) (es (car s))
	 (cr (cdr r)) (cs (cdr s))
	 (cri 0) (eri 0)
	 (ler (length er))
	 (les (length es))
	 (ans (init-rtree-mt)) )
    (declare (type (simple-array integer (*)) cr cs er es)
	     (fixnum ler les))		;maybe some others are fixnums too..
    ;; do the NXM multiplies into the answer tree
    (dotimes (i ler (A2PA (ordertree-nz ans)))
      (declare (fixnum i))
      (setf cri (aref cr i) eri(aref er i))
      (dotimes (j les)
	(declare (fixnum j))
	(update-rtree (+ eri(aref es j)) (* cri(aref cs j)) ans)))))


(defun mul-rtree4-nosort(r s)			;multiply two polynomials, in arrays
  (declare (optimize (speed 3)(safety 0)))
  (let* ((er (car r)) (es (car s))
	 (cr (cdr r)) (cs (cdr s))
	 (cri 0) (eri 0)
	 (ler (length er))
	 (les (length es))
	 (ans (init-rtree-mt)) )
    (declare (type (simple-array integer (*)) cr cs er es)
	     (fixnum ler les))		;maybe some others are fixnums too..
    ;; do the NXM multiplies into the answer tree
    (dotimes (i ler ans ) ;;; (A2PA (ordertree-nz ans))) ;;dont sort
      (declare (fixnum i))
      (setf cri (aref cr i) eri(aref er i))
      (dotimes (j les)
	(declare (fixnum j))
	(update-rtree (+ eri(aref es j)) (* cri(aref cs j)) ans)))))



(defun A2PA(A) ;; array of (exp . coef) to 2 arrays.
  (let* ((LA (length A))
	 (V nil)
	 (anse (make-array LA))
	 (ansc (make-array LA)))
    (dotimes (i LA (cons anse ansc))
      (setf V (aref A i))
      (setf (aref anse i) (car V))
      (setf (aref ansc i) (cdr V)))))


(defun update-rtree(key val tree &aux node) 
  ;; like insert, except val is added in to previous value, instead of replacing.
  (declare (type (simple-array t (*)) tree) (fixnum key))
  (cond ((shortkey (aref tree 0))
	 (cond ((= key (aref tree 2))
		(incf (aref tree 1) val));; case where key matches exactly. replace value
	       (t
		(let ((oldkey (aref tree 2))
		      (oldval (aref tree 1)))
		  (setf (aref tree 0) 0)
		  (setf (aref tree 1) 0)
		  (setf (aref tree 2) 0)
		  (if *debug*   (format t "~% oldkey= ~s key=~s tree=~s" oldkey key tree))
		  (setf tree(insert-rtree oldkey oldval tree)); re-insert the old key ;ok
		  (if *debug*  (format t "~%  tree=~s" tree))
		  (update-rtree key val tree)
					;	   (format t "~%  tree=~s" tree)
		  ))))
	;; the node (i.e. tree) is a long node AND
	;; the key is small enough to fit in this array
	((< key #.(expt 2 logsize))
	 ;; we have found the level, or level-1 for the key.
	 (cond ((nodep (setf node (aref tree key))); not a leaf
		;; if a node here, must descend one level further in tree; key now is 0
		(update-rtree 0 val node)); and update that node's location 0.
	       (t 
		(if *debug* (format t "~%case 1 key=~s" key))
		(incf (aref tree key) val))));; put it here.
	;;The key has too many bits to insert at this level. 
	;;Compute the subkey and separate the rest of the key by masking and shifting.
	(t (let ((ind(logand key #.(1- (expt 2 logsize))));rightmost logsize bits
		 (newkey (ash key #.(- logsize)))
		 )			; reduced key
	     	     (if *debug* (format t "~%case 2, newkey=~s ind=~s tree=~s" newkey ind tree))
	     (setf node (aref tree ind))
	     (cond((nodep node) (setf (aref tree ind) (update-rtree newkey val node)))
		  (t;; a single item, must be moved
		   (setf (aref tree ind) (init-rtree newkey  val))
		   (if *debug* (format t "~%case2b node =~s tree=~s" node tree))
		   
		   (unless (= node 0)(insert-rtree ind node tree));; re-insert 
		   )))))  tree)


;; example loop iteration..
;; (loop for i across z when (> i 0) do (print i))
;; (loop for i across z maximizing i)
	  

;; this picks the item with the smallest index and returns a pair:
;; (index . val)
;; useful for division!


(defun map-rtreexx (fn mt);; order appears jumbled, actually reversed-radix order
  ;; apply fn, a function of 2 arguments, to key and val, for each entry in tree.
  ;; order is "radix reversed"
   (labels 
       ((tt1 (path displace mt);; path = path to the key's location
	  (declare (fixnum path displace) (special *sm*))
	  
	  (cond ((leafp mt) (unless (= 0 mt)(funcall fn path mt)
				    ));; function applied to key  and val.
	       ((shortkey(aref mt 0))
		(funcall fn (+ (ash (the fixnum (aref mt 2)) displace) path) (aref mt 1)))
	       (t;; must be an array
		(do* ((i 0 (1+ i))
		     (path2 path (+ (ash i displace) path)))
		    ((= i #.(expt 2 logsize)))
		  (declare (fixnum i))
		  ;; we cut this off if
		  ;; (aref mt i) is a leaf and index is too big
		  (if (and (leafp (aref mt i))
			       (> path2 *sm*)) (return) ;from do*
		  (tt1  path2 (+ displace logsize)(aref mt i))))))))
    (tt1 0 0 mt)))


(defun smallest(mt)
  (let ((*sm* most-positive-fixnum)
	(val 0))
    (declare (special *sm*))
    (map-rtreexx #'(lambda(k v)
		    ;		   (format t "~%testing k=~s"k)
		     (if (< k *sm*)
			 (setf *sm* k val v)))
		 mt)
   (cons *sm* val)))