;; syntax test

(load "~/dev/lisp/lists.lisp")
(load "~/dev/lisp/macroplay.lisp")

;; heap?

(defun make-heap (&optional (max-size 64))
  (list 0 (make-array max-size :initial-element nil)))

(defun heap-get-pos (h pos) (aref (cadr h) pos))
(defun heap-set-pos (h pos el) (setf (aref (cadr h) pos) el))

(defun swap-heap-elems (heap ix1 ix2)
  (let ((tmp (heap-get-pos heap ix1)))
    (heap-set-pos heap ix1 (heap-get-pos heap ix2))
    (heap-set-pos heap ix2 tmp)))
(defun left-child (ix) (+ 1 (* 2 ix)))
(defun right-child (ix) (+ 2 (* 2 ix)))
(defun parent (ix) (floor (/ ix 2)))

(defun bubble-heap-up (heap pos)
  "Take the element at pos and move elements as long as the invariant is not fulfilled."
  (loop while (and (> pos 0) (< (heap-get-pos heap (parent pos)) (heap-get-pos heap pos)))
     do
       (swap-heap-elems heap (parent pos) pos)
       (setf pos (parent pos))))

(defun bubble-heap-fill (heap)
  "After popping the top element, move elements until the invariant is fulfilled"
  (let ((pos 0))
	(loop while (heap-get-pos heap pos) do
	     (let ((left-elem (heap-get-pos heap (left-child pos)))
		   (right-elem (heap-get-pos heap (right-child pos))))
	       (cond ((and (eql nil left-elem) (not (eql nil right-elem)))
		      (progn
			(swap-heap-elems heap (right-child pos) pos)
			(setf pos (right-child pos))))
		     ((and (eql nil right-elem) (not (eql nil left-elem)))
		      (progn
			(swap-heap-elems heap (left-child pos) pos)
			(setf pos (left-child pos))))
		     ((and (eql nil left-elem) (eql nil right-elem)) (return))
		     ('t
		      (if (> left-elem right-elem)
			  (progn
			    (swap-heap-elems heap (left-child pos) pos)
			    (setf pos (left-child pos)))
			  (progn
			    (swap-heap-elems heap (right-child pos) pos)
			    (setf pos (right-child pos))))))))
	(heap-set-pos heap pos nil)))

(defvar *debug-heap-level* 0)
(defun debug-heap-at-level (heap target-level pos)
  (defun or_ (a b) (or a b))
  (if (/= target-level *debug-heap-level*)
      (let ((*debug-heap-level* (1+ *debug-heap-level*)))
	(or_ (debug-heap-at-level heap target-level (left-child pos))
	     (debug-heap-at-level heap target-level (right-child pos))))
      (if (< pos (length heap))
	  (progn (format t "~a " (heap-get-pos heap pos)) 't)
	  nil)))

(defun debug-heap (heap)
  (loop
     for level from 0
     with ok = 't
     while ok
     do
       (format t "level ~a:~%" level)
       (setf ok (debug-heap-at-level heap level 0))
       (format t "~%")))

(defun put-heap (heap elem)
  (let ((elems (if (listp elem) elem (list elem)))
	(h (cadr heap)))
    (loop for e in elems do
	 (let ((newpos (car heap)))
	   (heap-set-pos heap newpos e)
	   (incf (car heap))
	   (bubble-heap-up heap newpos)))))

(defun pop-heap (heap)
  (let ((elem (heap-get-pos heap 0)))
    (if (eql 0 (car heap))
	nil
	(progn (decf (car heap))
	       (bubble-heap-fill heap)
	       elem))))

(defun len-heap (heap)
  (car heap))

(defparameter *test-suite* 'heap-self-test)
(defmacro check-true (form)
  (let ((result-name (gensym)))
    `(let ((,result-name ,form))
       (format t "~a: ~:[FAIL~;pass~] ~a ~%" *test-suite* ,result-name ',form)
       ,result-name)))

(defun heap-self-test ()
  (let ((heap (make-heap)))
    (or (and (check-true (eql nil (put-heap heap '(5 2 1 8 3 0))))
	     (check-true (eql 8 (pop-heap heap)))
	     (check-true (eql 5 (pop-heap heap)))
	     (check-true (eql 4 (len-heap heap))))
	(progn (format t "~a~%" heap) (debug-heap heap)))
    ))

(heap-self-test)