(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))