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