(defpackage quadtree
   (:use :cl)
   (:export make-rect quick-point new-quadtree contains-point intersects-boundary
            apply-to-children query-range insert))
 
 (in-package :quadtree)
 
 (defstruct point x y)
 (defstruct size  w h)
 (defstruct bounds center size)
 (defun make-rect (x y w h)
   (make-bounds
     :center (make-point :x x :y y)
     :size   (make-size :w w :h h)))
 
 (defclass quadtree ()
 
   ((bounds :initarg :bounds
            :initform (error "must provide boundaries"))
    (point-count :initform 1)
    (points :initform (make-array 5 :fill-pointer 0 :adjustable t))
    (nw :initform nil :type quadtree)
    (ne :initform nil :type quadtree)
    (sw :initform nil :type quadtree)
    (se :initform nil :type quadtree)))
 
 (defun twiddle (lis prefix &rest end)
   (cond ((null lis) `(progn ,@end))
         (t (let ((a (car lis))
                  (b (cdr lis)))
              `(,prefix ,(car a) ,(cadr a) ,(apply #'twiddle b prefix end))))))
 
 (defmacro mwith-slots (arg-forms &body rst)
   (apply #'twiddle arg-forms 'with-slots rst))
 
 (defmacro with-foursides (names center size &body body)
   "Binds the coordinates of the foor sides of a region: the names are bound to w,e,s,n in that order"
   `(mwith-slots (((x y) ,center)
                  ((w h) ,size))
      (let ((,(first names) (- x w))
            (,(second names) (+ x w))
            (,(third names) (- y h))
            (,(fourth names) (+ y h)))
        ,@body)))
 
 (defmacro with-bounds-foursides (names bounds &body bod)
   `(with-slots ((a center) (b size)) ,bounds
      (with-foursides ,names a b ,@bod)))
 
 (defun contains-point (bounds point)
   (with-slots ((x2 x) (y2 y)) point
     (with-bounds-foursides (w_x e_x s_y n_y) bounds
       (and (and (<= w_x x2) (> e_x x2))
            (and (<= s_y y2) (> n_y y2))))))
 
 (defun between (x a b)
   "Return T if x is between a and b (order insensitive)"
   (if (> a b) (rotatef a b)) (and (< a x) (< x b)))
 
 (defun quick-point (x y) (make-point :x x :y y))
 
 (defun intersects~boundary (bounds1 bounds2)
   (flet
     ((inner (bounds1 bounds2)
        (with-bounds-foursides (w_x1 e_x1 s_y1 n_y1) bounds1
          (with-bounds-foursides (w_x2 e_x2 s_y2 n_y2) bounds2
            (or (contains-point bounds1 (quick-point w_x2 n_y2))
                (contains-point bounds1 (quick-point e_x2 n_y2))
                (contains-point bounds1 (quick-point w_x2 s_y2))
                (contains-point bounds1 (quick-point e_x2 s_y2)))
            ))))
     (or (inner bounds1 bounds2) (inner bounds2 bounds1))))
 
 (defmacro combine (names func first_forms second_forms &body body)
   (flet ((name_combs (n1s n2s) (loop for n1 in n1s append (loop for n2 in n2s collect (list n1 n2)))))
     `(let
        ,(loop for n in names and cs in (name_combs first_forms second_forms) collect (list n (cons func cs)))
        (progn ,@body))))
 
 (defun subdivide (quadtree)
   (flet ((divquad (bounds)
            (mwith-slots (((center size) bounds)
                          ((x y) center)
                          ((w h) size))
              (let* ((half_w (/ w 2))
                     (w_x (- x half_w))
                     (e_x (+ x half_w))
                     (half_h (/ h 2))
                     (n_y (- y half_h))
                     (s_y (+ y half_h))
                     (newsize (make-size :w half_w :h half_h)))
                (combine (p1 p2 p3 p4) quick-point (n_y s_y) (w_x e_x)
                  (vector (make-bounds :center p1 :size newsize)
                        (make-bounds :center p2 :size newsize)
                        (make-bounds :center p3 :size newsize)
                        (make-bounds :center p4 :size newsize)))))))
     (let ((bounds (divquad (slot-value quadtree 'bounds))))
       (with-slots (nw sw ne se) quadtree
         (if (null nw) (setf nw (make-instance 'quadtree :bounds (elt bounds 0))))
         (if (null ne) (setf ne (make-instance 'quadtree :bounds (elt bounds 1)))) 
         (if (null sw) (setf sw (make-instance 'quadtree :bounds (elt bounds 2)))) 
         (if (null se) (setf se (make-instance 'quadtree :bounds (elt bounds 3))))))))
 
 (defmacro apply-to-children (quadtree func &rest args)
   (let*
     ((nw (gensym))
      (ne (gensym))
      (sw (gensym))
      (se (gensym))
      (qt (gensym))
      )
     `(let ((,qt quadtree))
        (with-slots (,nw ,ne ,sw ,se) ,qt
          ,@(loop for x in `(,nw ,ne ,sw ,se) collect
                  `(,func ,x ,@args))))))
 
 (defun insert (quadtree point)
   (with-slots (bounds points point-count nw ne sw se) quadtree
     (if (contains-point bounds point)
       (if (< (length points) point-count)
         (progn
           (vector-push-extend point points)
           t)
         (progn
           (subdivide quadtree)
           (or (apply-to-children quadtree insert point))))
       nil))) 
 
 (defun query-range (quadtree bounds)
   (let ((pir (make-array 5 :fill-pointer 0 :adjustable t)))
     (with-slots (qbounds qpoints nw) quadtree
       (if (intersects-boundary qbounds bounds)
         (progn
           (loop for p across qpoints
                 if (contains-point bounds p)
                 do (vector-push-extend p pir))
           (if (not (null nw))
             (setf pir
                   (concatenate 'vector
                                (apply-to-children quadtree query-range bounds)))))))
     pir))