(defconstant MinChk 32)

(defmacro leftchild (node)
   `(caddr ,node))

(defun bldMemMgr (N)
   (let ((root nil) 
         (hmap (make-hash-table)) 
         (totMem minChk))

   (labels (
       (valAddr (A)
           (cond
               ((not (integerp A)) (format t "Invalid address ~A, not an integer~%" A))
               ((< A 1) (format t "Invalid address ~A, must be in 1..~A~%" A totMem))
               ((> A totMem) (format t "Invalid address ~A, must be in 1..~A~%" A totMem))
               (t t)))
       (valSize (S)
           (cond
               ((not (integerp S)) (format t "Invalid size ~A, not an integer~%" S))
               ((< S 1) (format t "Invalid size ~A, must be in 1..~A~%" S totMem))
               ((> S totMem) (format t "Invalid size ~A, must be in 1..~A~%" S totMem))
               (t t)))
       (valBool (B)
           (if (or (equalp t B) (equalp nil B)) t
               (format t "Error: ~A is not a t/nil value~%" B)))
       (adjusted (N)
          (cond
              ((not (integerp N)) minChk)
              ((< N minChk) minChk)
              ((= 0 (mod N minChk)) N)
              (t (* minChk (+ 1 (floor (/ N minChk)))))))
       (taken (addr)
           (let ((result (gethash addr hmap)))
                (if (and (listp result) (> (length result) 2) (leftchild result)) t nil)))
       (mkChunk (addr size taken)
           (if (and (valAddr addr) (valSize size) (valBool taken))
               (list addr size taken)
               (format t "Error: invalid chunk arguments (~A ~A ~A)~%" addr size taken)))
       (valChunk (ch)
           (and (listp ch)
                (= 3 (length ch))
                (valAddr (car ch))
                (valSize (cadr ch))
                (valBool (caddr ch))))
       (chPrint (ch)
          (if (valChunk ch)
              (format t "Addr ~A: ~A bytes (~A)~%"
                 (car ch) (cadr ch) (if (caddr ch) "taken" "free"))
              (format t "Invalid chunk: ~A~%" ch)))
       (tr_leaf (addr size)
           (if (and (valAddr addr) (valSize size)) (list addr size nil nil)
               (format t "Error: attempt to make leaf with invalid addr/size ~A/~A~%" addr size)))
       (tr_insert (addr size subtree)
          (cond
              ((null subtree) (setf root (tr_leaf addr size)))
              ((and (< size (cadr subtree)) (null (caddr subtree)))
                    (setf (caddr subtree) (tr_leaf addr size)))
              ((< size (cadr subtree)) (tr_insert addr size (caddr subtree)))
              ((null (cadddr subtree)) (setf (cadddr subtree) (tr_leaf addr size)))
              (t (tr_insert addr size (cadddr subtree)))))
       (tr_remove (addr size subtree)
          (let ((parent (tr_findparent addr size subtree))
                 (target (nth-value 1 (tr_findparent addr size subtree))))
             (cond
                 ((null target) nil) 
                 ((and (null (caddr target)) (null (cadddr target)) (null parent))
                     (setf root nil))
                 ((and (null (caddr target)) (null (cadddr target)) (equalp target (caddr parent)))
                    (setf (caddr parent) nil)) 
                 ((and (null (caddr target)) (null (cadddr target)))
                    (setf (cadddr parent) nil)) 
                 ((and (null (caddr target)) (null parent))
                     (setf root (cadddr target))) 
                 ((and (null (cadddr target)) (null parent))
                     (setf root (caddr target))) 
                 ((and (null (caddr target)) (equalp target (caddr parent)))
                    (setf (caddr parent) (cadddr target))) 
                 ((null (caddr target))
                    (setf (cadddr parent) (cadddr target))) 
                 ((and (null (cadddr target)) (equalp target (caddr parent)))
                    (setf (caddr parent) (caddr target))) 
                 ((null (cadddr target))
                    (setf (cadddr parent) (caddr target))) 
                 (t 
                    (let* ((smallest (tr_smallest (cadddr target)))
                           (smAddr (if (not (null smallest)) (car smallest) nil))
                           (smSize (if (not (null smallest)) (cadr smallest) nil)))
                        (setf (car target) smAddr)
                        (setf (cadr target) smSize)
                        (if (equal smallest (cadddr target)) (setf (cadddr target) nil)
                            (tr_remove smAddr smSize (cadddr target))))))))
       (tr_smallest (subtree)
           (cond
               ((null subtree) nil) 
               ((null (caddr subtree)) subtree) 
               (t (tr_smallest (caddr subtree))))) 
          (cond
              ((null subtree) (values nil nil)) 
              ((and (equalp addr (car subtree)) (equalp size (cadr subtree))) (values nil subtree)) 
              ((and (< size (cadr subtree)) (null (caddr subtree))) (values nil nil)) 
              ((and (< size (cadr subtree))
                    (equalp addr (car (caddr subtree)))
                    (equalp size (cadr (caddr subtree))))
                    (values subtree (caddr subtree))) 
              ((< size (cadr subtree)) (tr_findparent addr size (caddr subtree))) 
              ((null (cadddr subtree)) (values nil nil)) 
              ((and (equalp addr (car (cadddr subtree))) (equalp size (cadr (cadddr subtree))))
                    (values subtree (cadddr subtree))) 
              (t (tr_findparent addr size (cadddr subtree))))) 
       (tr_findfit (size subtree)
          (cond
              ((null subtree) nil) 
              ((> size (cadr subtree)) (tr_findfit size (cadddr subtree))) 
              ((null (caddr subtree)) subtree) 
              ((> size (cadr (caddr subtree))) subtree) 
              (t (tr_findfit size (caddr subtree))))) 
       (tr_print (subtree)
          (unless (null subtree)
             (tr_print (caddr subtree))
             (format t "~A free bytes @addr ~A~%" (cadr subtree) (car subtree))
             (tr_print (cadddr subtree))))

       (h_insert (addr size taken)
          (if (valChunk (list addr size taken))
              (setf (gethash addr hmap) (list addr size taken))
              (format t "Insert failed due to invalid chunk args: ~A,~A,~A~%" size addr taken)))
       (h_update (addr size taken) (h_insert addr size taken))
       (h_lookup (addr) (gethash addr hmap))
       (h_remove (addr) (remhash addr hmap))
       (h_print ()
          (let ((chunks (loop for addr being the hash-keys of hmap collect
                              (gethash addr hmap))))
             (dolist (c (sort chunks (lambda (e1 e2) (< (car e1) (car e2)))))
                (format t "Addr: ~A, bytes ~A, taken ~A~%" (car c) (cadr c) (caddr c)))))

       (prt (which)
          (when which
             (format t "Current memory map:~%")
             (h_print))
          (unless which
             (format t "Current free memory sections:~%")
             (tr_print root)))
       (req (size)
          (let ((found -1)(extraChunk nil)(allocsize (adjusted size)))
             (when (valAddr size)
                (setf found (tr_findfit allocsize root))
                (when (not (null found))
                   (tr_remove (car found) (cadr found) root)
                   (when (> (cadr found) allocsize)
                       (setf extraChunk (list (+ (car found) allocsize) (- (cadr found) allocsize) nil)) 
                       (setf found (list (car found) allocsize t)) 
                       (tr_insert (car extraChunk) (cadr extraChunk) root)
                       (setf (gethash (car extraChunk) hmap) extraChunk))
                   (setf (caddr found) t)
                   (setf (gethash (car found) hmap) found)))
                (if (integerp found) found
                    (if (not (null found)) (car found) -1))))
       (free (addr)
           (let ((nextChunk nil)
                 (addrChunk nil))
           (cond
               ((not (valAddr addr)) (format t "Error: attempt to free invalid addr ~A~%" addr))
               ((not (taken addr)) (format t "Error: attempt to free unallocated addr ~A~%" addr))
               (t (block ReleaseTakenBlock
                  (setf addrChunk (h_lookup addr))
                  (setf nextChunk (h_lookup (+ addr (cadr addrChunk))))
                  (when (or (null nextChunk) (caddr nextChunk))
                      (tr_insert addr (cadr addrChunk) root)
                      (setf (gethash addr hmap) (list addr (cadr addrChunk) nil)))
                  (unless (or (null nextChunk) (caddr nextChunk))
                      (tr_remove (car nextChunk) (cadr nextChunk) root)
                      (remhash (car nextChunk) hmap)
                      (setf (gethash addr hmap) (list addr (+ (cadr addrChunk) (cadr nextChunk)) nil))
                      (tr_insert addr (+ (cadr addrChunk) (cadr nextChunk)) root)))))))

       (help ()
          (format t "Available commands are:~%")
          (format t "   prt t: print all memory chunks (sorted by address)~%")
          (format t "   prt nil: print free memory chunks (sorted by size)~%")
          (format t "   req k: request a k-byte chunk of memory~%")
          (format t "   free a: release the chunk of memory at address a~%")
          (format t "Debugging commands:~%")
          (format t "   hinsert addr size taken: insert entry into hash table~%")
          (format t "   hremove addr: remove from hash table~%")
          (format t "   tfit size: find smallest fitting node in tree~%")
          (format t "   tinsert addr size: insert node into tree~%")
          (format t "   tremove addr size: remove specified node from tree~%")
          (format t "   tparent addr size: find parent of specified node in tree~%")
          (format t "   debug: print raw tree structure~%")
          (format t "Working with pool of ~A bytes of memory~%" totMem))
        )

    (when (or (not (integerp N)) (< N 1))
       (format t "Error: attempt to initialize with invalid size ~A~%" N)
       (format t "   attempting with ~A instead~%" (adjusted N)))
    (setf totMem (adjusted N))
    (tr_insert 1 totMem root)
    (h_insert 1 totMem nil)
    (format t "memory pool initialized: ~A bytes available~%" totMem)

    (lambda (cmd &optional (arg nil) (debug1 nil) (debug2 nil))
      (cond
          ((equalp cmd 'prt) (prt arg)) 
          ((equalp cmd 'req) (req arg)) 
          ((equalp cmd 'free) (free arg)) 
          ((equalp cmd 'hremove) (h_remove arg)) 
          ((equalp cmd 'tfit) (tr_findfit arg root)) 
          ((equalp cmd 'tinsert) (tr_insert arg debug1 root)) 
          ((equalp cmd 'tremove) (tr_remove arg debug1 root)) 
          ((equalp cmd 'tparent) (tr_findparent arg debug1 root)) 
          ((equalp cmd 'hinsert) (h_insert arg debug1 debug2)) 
          ((equalp cmd 'debug) (format t "Raw tree structure:~%   ~A~%" root))
          (t (help)))))))