(in-package #:3dr.octcone)
 
 (defun make-adj-arr (&optional (size 25))
   (make-array size :adjustable t :fill-pointer 0))
 
 (defstruct (bounds (:type vector))
   tl tr bl br)
 
 (defclass octcone ()
   ((nw-bucket :initarg :nw-bucket :accessor nw-bucket :initform (make-adj-arr))
    (ne-bucket :initarg :ne-bucket :accessor ne-bucket :initform (make-adj-arr))
    (sw-bucket :initarg :sw-bucket :accessor sw-bucket :initform (make-adj-arr))
    (se-bucket :initarg :se-bucket :accessor se-bucket :initform (make-adj-arr))
    (overflow-bucket :initarg :overflow-bucket :accessor overflow-bucket :initform (make-adj-arr))
    (center :initarg :center :accessor center)))
 
 (defun bucket-point (point octcone)
   (with-slots (nw-bucket ne-bucket sw-bucket se-bucket center) octcone

     (with-accessors ((center-y 3dr::point-y) (center-z 3dr::point-z)) center

       (with-accessors ((point-y 3dr::point-y) (point-z 3dr::point-z)) point

         (vector-push-extend
           point
           (if (< point-y center-y)
             (if (< point-z center-z)
               sw-bucket
               nw-bucket)
             (if (< point-z center-z)
               se-bucket
               ne-bucket)))))))

 (defun normalize-bucket (octcone bucket)
   (let ((the-bucket (slot-value octcone bucket)))
     (setf (slot-value octcone bucket)
           (sort the-bucket #'< :key (lambda (point) (3dr::c-distance point #(0 0 0)))))))
 
 (defun normalize-buckets (octcone)
   (loop for bucket in '(nw-bucket ne-bucket se-bucket sw-bucket)
         collect (normalize-bucket octcone bucket)))
 
 (defun half-power-two (low high)

   (* (signum low)
      (expt 2
            (floor (log (abs (+ low
                                (floor (- high low)
                                       2)))
                        2)))))

 
 (defun subdivide-bucket (octcone bucket)

   (declare (optimize (debug 3)))

   (let ((the-bucket (slot-value octcone bucket)))

     (check-type the-bucket vector)
     (if (< 1 (length the-bucket))

       (let* ((max-y (reduce #'max the-bucket :key #'3dr::point-y))
              (min-y (reduce #'min the-bucket :key #'3dr::point-y))

              (max-z (reduce #'max the-bucket :key #'3dr::point-z))
              (min-z (reduce #'min the-bucket :key #'3dr::point-z))

              (new-octc (make-instance 'octcone
                                       :center (vector 0
                                                       (half-power-two min-y max-y)
                                                       (half-power-two min-z max-z)))))

         (values (prog1 new-octc

                   (when (or (= min-y max-y) (= min-z max-z))
                     (setf (slot-value octcone bucket)
                           new-octc)
                     (map nil
                          (lambda (point)
                            (bucket-point point new-octc))
                          the-bucket)))

                 (vector min-y max-y)

                 (vector min-z max-z)))
       nil)))
 
 (defmethod bucket-length (bucket)
   (declare (optimize (debug 3)))
   (typecase bucket
     (vector (length bucket))
     (octcone
       (with-slots (nw-bucket ne-bucket sw-bucket se-bucket overflow-bucket) bucket
         (+ (bucket-length nw-bucket)
            (bucket-length ne-bucket)
            (bucket-length se-bucket)
            (bucket-length sw-bucket)
            (bucket-length overflow-bucket))))))
 
 (defmethod print-object ((object octcone) s)
   (print-unreadable-object (object s :type t :identity t)
     (with-slots (nw-bucket ne-bucket sw-bucket se-bucket overflow-bucket center) object
       (format s "center: (~{~a~^,~}) sizes: nw ~d, ne ~d, se ~d, sw ~d, overflow ~d"
               (coerce center 'list)
               (bucket-length nw-bucket)
               (bucket-length ne-bucket)
               (bucket-length se-bucket)
               (bucket-length sw-bucket)
               (bucket-length overflow-bucket)))))

 
 (defun subdivide-buckets (octcone)

   (let ((result (loop for bucket in '(nw-bucket ne-bucket se-bucket sw-bucket)
                       collect (subdivide-bucket octcone bucket))))))

 
 (defun show-bucket-sizes (octcone)
   (loop for bucket in '(nw-bucket ne-bucket se-bucket sw-bucket)
         collect (length (slot-value octcone bucket))))
 
 (defvar *octc*)
 (defvar *points*)
 
 (defun main ()
   (setf *octc* (make-instance 'octcone :center (vector 0 0 0)))
 
   (flet ((100- (x) (- 100 x)))
     (setf *points*
           (loop repeat 1000
                 collect (vector (random 200) (100- (random 200)) (100- (random 200))))))
 
   (mapcar (lambda (x)
             (bucket-point x *octc*))
           *points*))

 (defun make-graph-conns (conns)
   (format nil "~:{~4t~(\"~a\" -> \"~a\"~%~)~}" conns))
 
 (defun make-graph (conns)
   (format nil "digraph {~%~a~%}~%" (make-graph-conns conns)))
 
 (defun bucket-to-pair (bucket parent)
   (declare (optimize (debug 3)))
   (flet ((bucket-to-string (center bucket title)
            (format nil "\"(~a: <~{~d~^, ~}> ~d)\""
                    (sxhash bucket)
                    (coerce center 'list)
                    (bucket-length bucket))))
     (with-slots (center) bucket
       (with-slots ((parent-center center)) parent
         (list (bucket-to-string parent-center parent)
               (bucket-to-string center bucket))))))
 
 (defun octcone-to-graph-pairs (octcone)
   (declare (optimize (debug 3)))
   (with-slots (nw-bucket ne-bucket se-bucket sw-bucket center) octcone
     (list
       (typecase nw-bucket
         (octcone (bucket-to-pair nw-bucket octcone)))
       (typecase ne-bucket
         (octcone (bucket-to-pair ne-bucket octcone)))
       (typecase se-bucket
         (octcone (bucket-to-pair se-bucket octcone)))
       (typecase sw-bucket
         (octcone (bucket-to-pair sw-bucket octcone))))))
 
 (defun bucket-to-graph-list (bucket)
   (declare (optimize (debug 3)))
   (typecase bucket
     (octcone
       (with-slots (nw-bucket ne-bucket se-bucket sw-bucket center) bucket
         (append (octcone-to-graph-pairs bucket)
                 (bucket-to-graph-list nw-bucket)
                 (bucket-to-graph-list ne-bucket)
                 (bucket-to-graph-list sw-bucket)
                 (bucket-to-graph-list se-bucket))))))