;;;; cl-scopes/csys/space
;;;; definitions for (spatial) registering and retrieving objects by location

(defpackage :scopes/csys/space
  (:use :common-lisp)
  (:local-nicknames (:alx :alexandria)
                    (:util :scopes/util))
  (:export #:create #:put #:del #:fetch #:query 
           #:neighbors #:free-loc-at #:distance-sq
           #:loc-iterator))

(in-package :scopes/csys/space)

(defun create ()
  (make-hash-table :test #'equalp))

(defun put (spc loc obj)
  (let ((current (gethash loc spc))) 
    (if current
      (error "location already occupied! space: ~s, loc: ~s, current: ~s, new: ~s" 
             spc loc current obj)
      (setf (gethash loc spc) obj))))

(defun del (spc loc)
  (remhash loc spc))

(defun fetch (spc loc)
  (gethash loc spc))

(defun query (spc pattern)
  nil)

(defun neighbors (spc loc &key (dist 2))
  (let* ((to (floor (sqrt dist)))
         (from (- to))
         nbrs)
    (util:with-iterator (loc-iterator from to)
      (lambda (rloc)
        (let* ((cloc (loc-add rloc loc))
               (cell (fetch spc cloc)))
          (when cell (push cell nbrs)))))
    nbrs))

(defun free-loc-at (spc loc &key (dist 2)))

(defun distance-sq (spc loc1 loc2))

(defun loc-iterator (from to &key (dim 2))
  (labels (
    (inc (cur idx)
      (when (< idx dim)
        (let ((new (1+ (aref cur idx))))
          (setf (aref cur idx) new)
          (when (> new to)
            (setf (aref cur idx) from)
            (setf cur (inc cur (1+ idx))))
          cur))))
    (let ((cur (make-array dim :initial-element from)))
      (lambda ()
        (when cur
          (prog1 (make-array dim :initial-contents cur)
            (setf cur (inc cur 0))))))))

(defun loc-add (l1 l2)
  (let* ((dim (car (array-dimensions l1)))
         (loc (make-array dim :initial-contents l1)))
    (loop for i to (1- dim) do (setf (aref loc i) (+ (aref l1 i) (aref l2 i))))
    loc))