(defpackage #:3dr.octcone3
  (:use #:cl #:alexandria #:fw.lu))

(in-package #:3dr.octcone3)

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

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

(defclass bintree ()
  ((left :initarg :left :accessor left :initform nil)
   (right :initarg :right :accessor right :initform nil)
   (duplicates :initarg :duplicates :accessor duplicates :initform (make-adj-arr))
   (center :initarg :center :accessor center)))

(defclass quadtree ()
  ((nw :initarg :nw :accessor nw :initform nil)
   (ne :initarg :ne :accessor ne :initform nil)
   (se :initarg :se :accessor se :initform nil)
   (sw :initarg :sw :accessor sw :initform nil)
   (duplicates :initarg :duplicates :accessor duplicates :initform (make-adj-arr))
   (center :initarg :center :accessor center)))

(defgeneric insert-in-tree (bintree child))
(defmethod insert-in-tree ((bintree bintree) (child real))
  (with-slots (left right duplicates center) bintree
    (if (= child center)
      (vector-push-extend center duplicates)
      (if (< child center)
        (etypecase left
          (bintree (insert-in-tree left child))
          (real (setf left (make-instance 'bintree :center left))
                (insert-in-tree left child))
          (null (setf left child)))

        (etypecase right
          (bintree (insert-in-tree right child))
          (real (setf right (make-instance 'bintree :center right))
                (insert-in-tree right child))
          (null (setf right child)))))))

(defmethod insert-in-tree ((quadtree quadtree) (child vector))
  (labels ((vector-value (child)
             (etypecase child
               (quadtree (center child))
               (vector child)))
           (dist-to-child (child)
             (c-distance (center quadtree) (vector-value child))) 
           (replace-bucket (name)
             (etypecase (slot-value quadtree name)
               (quadtree (insert-in-tree (slot-value quadtree name) child))
               (vector (let ((old-bucket (slot-value quadtree name)))
                         (if (<= (dist-to-child child) (dist-to-child old-bucket))
                           (let ((new-bucket (make-instance 'quadtree :center child)))
                             (setf (slot-value quadtree name) new-bucket)
                             (with-accessors ((old-bucket-x point-x) (old-bucket-y point-y)) (vector-value old-bucket) 
                               (with-accessors ((new-bucket-x point-x) (new-bucket-y point-y)) (vector-value new-bucket)
                                 (if (< old-bucket-x new-bucket-x)
                                   (if (>= old-bucket-y new-bucket-y)
                                     (setf (nw new-bucket) old-bucket)
                                     (setf (sw new-bucket) old-bucket)) 
                                   (if (>= old-bucket-y new-bucket-y)
                                     (setf (ne new-bucket) old-bucket)
                                     (setf (se new-bucket) old-bucket))))))
                           (setf (slot-value quadtree name)
                                 (make-instance 'quadtree :center old-bucket))))
                       (insert-in-tree (slot-value quadtree name) child))
               (null (setf (slot-value quadtree name) child)))))

    (with-slots (nw ne se sw duplicates center) quadtree
      (if (equalp child center)
        (vector-push-extend center duplicates)
        (with-accessors ((center-x point-x) (center-y point-y)) center
          (with-accessors ((child-x point-x) (child-y point-y)) child
            (if (< child-x center-x)
              (if (>= child-y center-y)
                (replace-bucket 'nw)
                (replace-bucket 'sw))
              (if (>= child-y center-y)
                (replace-bucket 'ne)
                (replace-bucket 'se)))))))))

(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 one-layer-of-bintree (bintree)
  (flet ((bintree-as-point (b)
           (etypecase b
             (null nil)
             (real b)
             (bintree (center b)))))
    (with-slots (left right) bintree
      (list (mapcar #'bintree-as-point (list bintree left right))))))

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

(defun bintree-to-list (bintree)
  (typecase bintree
    (vector (list (list bintree nil nil)))
    (bintree (with-slots (left right) bintree
               (append (one-layer-of-bintree bintree)
                       (when left (bintree-to-list left))
                       (when right (bintree-to-list right)))))))

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


(defun bintreelist-to-graph (bintreelist)
  (loop for list in bintreelist
        append (loop with head = (car list)
                     for tail in (cdr list)
                     when tail collect (list head tail))))

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

(defun draw-line (buffer p1 p2)
  (with-accessors ((x1 point-x) (y1 point-y)) p1
    (with-accessors ((x2 point-x) (y2 point-y)) p2
      (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
                  (incf (aref buffer x y) 1)
                  (incf (aref buffer y x) 1))
            (setf error (- error delta-y))
            (when (< error 0)
              (incf y y-step)
              (incf error delta-x))))
        buffer))))

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


(defun main ()
  (let ((res (make-instance 'quadtree :center #(50 50))))
    (loop repeat 400
          do (insert-in-tree res (vector (random 100) (random 100)))
          finally (return-from main res))))