(defvar k-d-tree nil "Root of K-D tree")

(defun add1 (a)
  (+ a 1))

(defun sub1 (a)
  (- a 1))

(defun lessp (a b)
  (< a b))

(defun mod (a b)
  (% a b))

(defun k-d-key (tree)
  (nth 0 tree))

(defun k-d-item (key level dimension)
  (nth (mod level dimension) key))

(defun k-d-data (tree)
  (nth 1 tree))

(defun k-d-left (tree)
  (nth 2 tree))

(defun k-d-right (tree)
  (nth 3 tree))

(defun item-lessp (item1 item2)
  (cond
    ((eq item1 '-infinity)
      (not (eq item2 '-infinity)))
    ((eq item2 'infinity)
      (not (eq item1 'infinity)))
    ((and (numberp item1) (numberp item2))
      (lessp item1 item2))
    ((and (stringp item1) (stringp item2))
      (string-lessp item1 item2))
    ((and (atom item1) (atom item2))
      (string-lessp (prin1-to-string item1) (prin1-to-string item2)))
    (t (error "Incompatible atom types given to item-lessp."))))

(defun store (key data)
  (setq k-d-tree (k-d-store k-d-tree key data)))  

(defun dimensions-match (tree key)
  (or
    (null tree)
    (eq (length key) (length (k-d-key tree)))))

(defun k-d-store (tree key data)
  (cond
    ((not (dimensions-match tree key))
      (error "Key and K-D tree different dimensions.")
      tree)
    (t
      (k-d-store-1 tree key data 0 (length key)))))

(defun k-d-store-1 (tree key data level dimension)
  (cond
    ((null tree)
      (list
	key
	data
	nil
	nil))
    ((item-lessp
       (k-d-item key level dimension)
       (k-d-item (k-d-key tree) level dimension))
      (list
	(k-d-key tree)
	(k-d-data tree)
	(k-d-store-1 (k-d-left tree) key data (add1 level) dimension)
	(k-d-right tree)))
    (t
      (list
	(k-d-key tree)
	(k-d-data tree)
	(k-d-left tree)
	(k-d-store-1 (k-d-right tree) key data (add1 level) dimension)))))
      
(defun show (tree)
  (terpri)
  (show-1 tree "Node "))

(defun show-1 (tree prefix)
  (cond
    ((null tree) nil)
    (t
      (princ (concat prefix ": "))
      (princ (k-d-key tree))
      (princ " = ")
      (princ (k-d-data tree))
      (terpri)
      (show-1 (k-d-left tree) (concat prefix "l"))
      (show-1 (k-d-right tree) (concat prefix "r")))))

(defun key-equal (key pattern)
  (cond
    ((null key)
      t)
    ((or
       (equal (car pattern) '*)
       (equal (car key) (car pattern)))
      (key-equal (cdr key) (cdr pattern)))
    (t
      nil)))

(defun p-m-search (tree key)
  (cond
    ((not (dimensions-match tree key))
      (error "Key and K-D tree different dimensions.")
      nil)
    (t
      (p-m-search-1 tree key 0 (length key)))))

(defun p-m-search-1 (tree key level dimension)
  (cond
    ((null tree)
      nil)
    ((eq (k-d-item key level dimension) '*)
      (append
        (p-m-search-1 (k-d-left tree) key (add1 level) dimension)
	(cond
	  ((key-equal (k-d-key tree) key)
	    (list (list (k-d-key tree) (k-d-data tree))))
	  (t
	    nil))
	(p-m-search-1 (k-d-right tree) key (add1 level) dimension)))
    (t
      (cond
	((item-lessp
	   (k-d-item key level dimension)
	   (k-d-item (k-d-key tree) level dimension))
	  (p-m-search-1 (k-d-left tree) key (add1 level) dimension))
	(t
	  (append
	    (cond
	      ((key-equal (k-d-key tree) key)
		(list (list (k-d-key tree) (k-d-data tree))))
	      (t
		nil))
	    (p-m-search-1 (k-d-right tree) key (add1 level) dimension)))))))

(defun k-d-region-search (tree min max)
  (cond
    ((not (and
	    (dimensions-match tree min)
	    (dimensions-match tree max)))
      (error "Key and K-D tree different dimensions.")
      nil)
    (t
      (catch 'k-d-region-search
	(let ((dimension (length min)))
	  (k-d-region-search-1 tree 0
	    (make-list dimension '-infinity)
	    (make-list dimension 'infinity)))))))

(defun split-region (key limit)
  (split-region-1 key limit (mod level dimension)))

(defun split-region-1 (key point item)
  (cond
    ((zerop item)
      (cons
	(car key)
	(cdr point)))
    (t
      (cons
	(car point)
	(split-region-1 (cdr key) (cdr point) (sub1 item))))))

(defun part-of-region (region-min region-max min max)
  (cond
    ((null region-min)
      t)
    (t 
      (and
	(item-lessp (car min) (car region-max))
	(item-lessp (car region-min) (car max))
	(part-of-region
	  (cdr region-min) (cdr region-max) (cdr min) (cdr max))))))

(defun in-region (key min max)
  (part-of-region key key min max))

(defun all-of-region (region-min region-max min max)
  (cond
    ((null region-min)
      t)
    (t
      (and
	(not (item-lessp (car region-min) (car min)))
	(not (item-lessp (car max) (car region-max)))
	(all-of-region
	  (cdr region-min) (cdr region-max) (cdr min) (cdr max))))))

(defun k-d-region-search-1 (tree level box-min box-max)
  (cond
    ((null tree)
      nil)
    (t
      (let*
	(
	  (left-max (split-region (k-d-key tree) box-max))
	  (right-min (split-region (k-d-key tree) box-min))
	  (matches
	    (append
	      (cond
		((part-of-region box-min left-max min max)
		  (k-d-region-search-1
		    (k-d-left tree) (add1 level) box-min left-max))
		(t
		  nil))
	      (cond
		((in-region (k-d-key tree) min max)
		  (list (list (k-d-key tree) (k-d-data tree))))
		(t
		  nil))
	      (cond
		((part-of-region right-min box-max min max)
		  (k-d-region-search-1
		    (k-d-right tree) (add1 level) right-min box-max))
		(t
		  nil)))))
	(cond
;	  ((all-of-region box-min box-max min max)
;	    (throw 'k-d-region-search matches))
	  (t
	    matches))))))

(load "kd-data")

(defun make-tree ()
  (setq k-d-tree nil)
  (mapcar '(lambda (x) (store (car x) (car (cdr x)))) k-d-data)
  t)