(in-package #:3dr.octcone2)

(defun make-adj-arr (&optional (size 25))
  (make-array size :adjustable t :fill-pointer 0))

(defclass quadtree ()
  ((nw-bucket :initarg :nw-bucket :accessor nw-bucket :initform nil)
   (ne-bucket :initarg :ne-bucket :accessor ne-bucket :initform nil)
   (sw-bucket :initarg :sw-bucket :accessor sw-bucket :initform nil)
   (se-bucket :initarg :se-bucket :accessor se-bucket :initform nil)

   (nw-bucket-bg :initarg :nw-bucket-bg :accessor nw-bucket-bg :initform nil)
   (ne-bucket-bg :initarg :ne-bucket-bg :accessor ne-bucket-bg :initform nil)
   (sw-bucket-bg :initarg :sw-bucket-bg :accessor sw-bucket-bg :initform nil)
   (se-bucket-bg :initarg :se-bucket-bg :accessor se-bucket-bg :initform nil)

   (duplicates :initarg :duplicates :accessor duplicates :initform (make-adj-arr))
   (center :initarg :center :accessor center)))

(defgeneric insert-in-quadtree (quadtree point))

(defstruct (point (:type vector)) x y z)

(defun maybe-replace-bucket (quadtree bucket point)
  (declare (optimize (debug 3)))
  (let ((bucket-val (slot-value quadtree bucket)))
    (if (not bucket-val)
      (setf (slot-value quadtree bucket) point)
      (etypecase bucket-val
        (vector (setf (slot-value quadtree bucket)
                      (insert-in-quadtree (make-instance 'quadtree
                                                         :center bucket-val)
                                          point)))
        (quadtree (if (< (c-distance (center quadtree) point)
                         (c-distance (center quadtree) (center bucket-val)))
                    (let ((new-quadtree (make-instance 'quadtree :center point)))
                      (setf (slot-value quadtree bucket) new-quadtree)
                      (insert-in-quadtree new-quadtree bucket-val))
                    (insert-in-quadtree bucket-val point)))))))

(defun c-distance (p1 p2)
  (declare (optimize (speed 3) (debug 0))
           (inline c-distance))
  (sqrt (loop for c1 across p1
              for c2 across p2
              sum (expt (- c2 c1) 2))))

(defmethod insert-in-quadtree ((quadtree quadtree) (child quadtree))
  (with-slots ((parent-center center) duplicates) quadtree
    (with-slots ((child-center center)) child
      (with-accessors ((point-x point-x) (point-y point-y) (point-z point-z)) child-center
        (with-accessors ((center-x point-x) (center-y point-y) (center-z point-z)) parent-center
          (if (equalp parent-center child-center)
            (vector-push-extend child-center duplicates)
            (maybe-replace-bucket quadtree
                                  (if (< point-x center-x)
                                    (if (< point-y center-y)
                                      (if (< point-z center-z) 'sw-bucket-bg 'sw-bucket)
                                      (if (< point-z center-z) 'nw-bucket-bg 'nw-bucket))
                                    (if (< point-y center-y)
                                      (if (< point-z center-z) 'se-bucket-bg 'se-bucket)
                                      (if (< point-z center-z) 'ne-bucket-bg 'ne-bucket)))
                                  child)))))))

(defmethod insert-in-quadtree ((quadtree quadtree) (point vector))
  (declare (optimize (debug 3)))
  (prog1 quadtree
    (with-slots (duplicates center) quadtree
      (with-accessors ((point-x point-x) (point-y point-y) (point-z point-z)) point
        (with-accessors ((center-x point-x) (center-y point-y) (center-z point-z)) center
          (if (equalp point center)
            (vector-push-extend point duplicates)
            (maybe-replace-bucket quadtree
                                  (if (< point-x center-x)
                                    (if (< point-y center-y)
                                      (if (< point-z center-z) 'sw-bucket-bg 'sw-bucket)
                                      (if (< point-z center-z) 'nw-bucket-bg 'nw-bucket))
                                    (if (< point-y center-y)
                                      (if (< point-z center-z) 'se-bucket-bg 'se-bucket)
                                      (if (< point-z center-z) 'ne-bucket-bg 'ne-bucket)))
                                  point)))))))

(defmethod print-object ((object quadtree) s)
  (declare (optimize (debug 3)))
  (print-unreadable-object (object s :type t :identity t)
    (with-slots (center nw-bucket nw-bucket-bg ne-bucket ne-bucket-bg
                        se-bucket se-bucket-bg sw-bucket sw-bucket-bg
                        duplicates) object
      (format s "~s x~d: ~@{~a~^ ~}"
              center
              (length duplicates)
              (not (null nw-bucket))
              (not (null nw-bucket-bg))
              (not (null ne-bucket))
              (not (null ne-bucket-bg))
              (not (null sw-bucket))
              (not (null sw-bucket-bg))
              (not (null se-bucket))
              (not (null se-bucket-bg))))))

(defun make-graph-conns (conns)
  (format nil "~:{~4t~(\"~a\" -> \"~a\"~%~)~}" conns))

(defun make-graph (conns)
  (format nil "digraph {splines=ortho;~%node [shape=box];~%~a~%}~%" (make-graph-conns conns)))

(defun quadtree-to-graph (quadtree &optional parent)
  (declare (optimize (debug 3)))
  (flet ((graph-bucket (bucket)
           (typecase quadtree
             (quadtree (quadtree-to-graph (slot-value quadtree bucket)
                                          quadtree)))))
    (when quadtree
      (with-slots (center) quadtree
        (with-slots ((parent-center center)) parent
          (append (when parent
                    (list (list parent-center
                                (typecase quadtree
                                  (quadtree center)
                                  (vector quadtree)))))
                  (graph-bucket 'nw-bucket-bg)
                  (graph-bucket 'ne-bucket-bg)
                  (graph-bucket 'sw-bucket-bg)
                  (graph-bucket 'se-bucket-bg)
                  (graph-bucket 'nw-bucket)
                  (graph-bucket 'ne-bucket)
                  (graph-bucket 'sw-bucket)
                  (graph-bucket 'se-bucket)))))))

(defun draw-line (buffer p1 p2)
  (with-accessors ((x1 point-x) (y1 point-y)) p1
    (let ((x1 (+ 64 x1))
          (y1 (+ 64 y1)))
      (with-accessors ((x2 point-x) (y2 point-y)) p2
        (let ((x2 (+ 64 x2))
              (y2 (+ 64 y2)))
          (let* ((dist-x (abs (- x1 x2)))
                 (dist-y (abs (- y1 y2)))
                 (steep (> dist-y dist-x)))
            (when steep
              (psetf x1 y1 y1 x1
                     x2 y2 y2 x2))
            (when (> x1 x2)
              (psetf x1 x2 x2 x1
                     y1 y2 y2 y1))
            (let* ((delta-x (- x2 x1))
                   (delta-y (abs (- y1 y2)))
                   (error (floor delta-x 2))
                   (y-step (if (< y1 y2) 1 -1))
                   (y y1))
              (loop
                :for x :upfrom x1 :to x2
                :do (if steep
                      (setf (aref buffer x y) 1)
                      (setf (aref buffer y x) 1))
                (setf error (- error delta-y))
                (when (< error 0)
                  (incf y y-step)
                  (incf error delta-x))))
            buffer))))))

(defun one-layer-of-octree (quadtree)
  (flet ((bucket-as-point (b)
           (etypecase b
             (null nil)
             (vector b)
             (quadtree (center b)))))
    (with-slots (nw-bucket nw-bucket-bg ne-bucket ne-bucket-bg se-bucket se-bucket-bg sw-bucket sw-bucket-bg) quadtree
      (list
        (mapcar #'bucket-as-point (list quadtree
                                        nw-bucket nw-bucket-bg
                                        ne-bucket ne-bucket-bg
                                        se-bucket se-bucket-bg
                                        sw-bucket sw-bucket-bg))))))

(defun octree-to-list (octree)
  (typecase octree
    (vector (list (list octree nil nil nil nil nil nil nil nil)))
    (quadtree (with-slots (nw-bucket nw-bucket-bg ne-bucket ne-bucket-bg se-bucket se-bucket-bg sw-bucket sw-bucket-bg) octree
                (append (one-layer-of-octree octree)
                        (when nw-bucket        (octree-to-list nw-bucket))
                        (when nw-bucket-bg  (octree-to-list nw-bucket-bg))
                        (when ne-bucket        (octree-to-list ne-bucket))
                        (when ne-bucket-bg  (octree-to-list ne-bucket-bg))
                        (when se-bucket        (octree-to-list se-bucket))
                        (when se-bucket-bg  (octree-to-list se-bucket-bg))
                        (when sw-bucket        (octree-to-list sw-bucket))
                        (when sw-bucket-bg  (octree-to-list sw-bucket-bg)))))))

(defun main ()
  (declare (optimize (debug 3)))
  (labels ((50- (x) (- x 50))
           (random-point (&optional (limit 100))
             (make-point :x (50- (random limit))
                         :y (50- (random limit))
                         :z 0 #|(50- (random limit))|#)))
    (let ((quadtree (make-instance 'quadtree :center #(0 0 0)))
          (points (loop repeat 100 collect (random-point))))
      (map nil (lambda (point) (insert-in-quadtree quadtree point))
        points)
      (values quadtree points))))