;;; -*- Mode: Lisp; Package: DESIGN; Syntax: Ansi-common-lisp -*-

;;; Need to add copy method:  Design model now holds circulation model that is copied to
;;; each territory model.  Territory model then mods (by adding nodes for territories and
;;; paths).  Redefine nodes and links in terms of new basic geometric objects.

;;; Problem with copying model:  If copy circulation model, may have nodes that correspond
;;; to edges that are not part of the territory model to which the circulation model is
;;; being copied.  Using the edge model's circulation model circumvents problem with concave
;;; territories (and current algorithm for adding links to circulation model).  Don't worry
;;; about concave territories at this point; each territory has its own circulation model as
;;; before. 

;;; The Circulation Model

;;; Circulation model is graph of openings between territories: nodes represent openings,
;;; links represent single-edge path between nodes (midpoints of openings) and are tagged
;;; with distance.  Using single-edge paths between openings assumes convex spaces.  (If
;;; concave, path edges cross opaque edges; need to use visibility graph to position edges.
;;; Later.)

;;; To construct path, insert destination point and maybe starting point, then find shortest
;;; path between them.  

;;; Next version needs to deal with obstacles in spaces and nonconvex spaces.
;;; In either of these cases, single-edge link between nodes will be replaced by
;;; visibility graph (within space).

;;; 4/25/96:  Calculate "minimal" paths to a circulation node on the node and store on node;
;;; shortest path is shortest of those.  "Minimal" means shortest of paths with a unique
;;; signature, which is order of traversed spaces. In legal signatures, each space is only
;;; traversed once (maybe by multiple links). Algorithm computes all paths.  Paths are no
;;; longer stored on circulation model.  Use PATHS property-value as holding place for 
;;; computing paths; when done, move paths to property-value whose name is the from-node.



(defclass BASIC-CIRCULATION-NODE (plist-mixin point)
    ())

(defun make-basic-circulation-node (x y)
  (make-instance 'basic-circulation-node :x x :y y))

;; to keep find methods from breaking (fix this!)
(defmethod node-edge ((node basic-circulation-node))
  nil)


(defclass APPROACH-CIRCULATION-NODE (basic-circulation-node)
    ((approach-point :initform nil :initarg :approach-point :accessor node-approach-point)))

(defmethod print-object ((node approach-circulation-node) stream)
  (with-slots (approach-point) node
      (format stream "<~a: ~a>" (type-of node) (name approach-point))))

(defmethod name ((node approach-circulation-node))
  (name (node-approach-point node)))

(defun make-circulation-node-for-approach-point (x y &optional point)
  (make-instance 'approach-circulation-node :x x :y y :approach-point point))

(defun find-circulation-node-for-approach-point (tmodel)
  ;; assume for now there is only one of these (for the front approach)
  (loop for node in (nodes (circulation-model tmodel))
	when (typep node 'approach-circulation-node)
	  return node))

(defun find-circulation-nodes-for-approach-points (tmodel)
  (loop for node in (nodes (circulation-model tmodel))
	when (typep node 'approach-circulation-node)
	  collect node))

;; to keep find methods from breaking (fix this!)
(defmethod node-territory-p ((node approach-circulation-node) region)
  region
  nil)

(defmethod territories ((node approach-circulation-node))
  nil)

;; to keep insert function from breaking
(defmethod corresponding-edge ((node approach-circulation-node))
  nil)

;; Territories are the territories in territory-model which share this node;
;; corresponding-edge in territory model corresponds to this node and represents an
;; opening; edges stored in circulation-node are to neighboring circulation-nodes.

(defclass CIRCULATION-NODE (basic-circulation-node territories-mixin)
    ((corresponding-edge :initform nil :initarg :corresponding-edge
			 :accessor corresponding-edge)))

(defmethod name ((node circulation-node))
  (with-slots (territories corresponding-edge) node
    (if corresponding-edge
	(if (design-element corresponding-edge)
	    (format nil "~a" (name (design-element corresponding-edge)))
	    (format nil "~a~@[~{~a~}~]" (name (car territories))(mapcar #'name (cdr territories))))
    (format nil "~a~@[~{ ~a~}~]" (name (car territories)) (mapcar #'name (cdr territories))))))

(defmethod print-object ((node circulation-node) stream)
  (with-slots (corresponding-edge territories) node
    (let ((object (when corresponding-edge (design-element corresponding-edge))))
      ;; object will be opening
      (if object (format stream "<~a: ~a>" (type-of node) (name object))
	  (format stream "<~a: ~a>" (type-of node) (name node))))))

(defmethod node-territory-p ((node circulation-node) region)
  (with-slots (territories) node
    (find region territories)))

(defmethod add-territory ((node circulation-node) region)
  (with-slots (territories) node
    (pushnew region territories)))

(defun make-circulation-node (edge &optional x y region)
  (let ((node (make-instance 'circulation-node :corresponding-edge edge)))
    (unless (and x y) (multiple-value-setq (x y) (midpoint-x-y edge)))
    (setf (point-x node) x)
    (setf (point-y node) y)
    (when region (add-territory node region))
    node))



;; rationalize args for find functions!

(defmethod find-circulation-node-for-element ((design-element-name symbol) tmodel)
  (when design-element-name
    (find design-element-name (nodes (circulation-model tmodel))
	  :key #'(lambda (x) (name (design-element (corresponding-edge x)))))))


(defmethod find-circulation-node-for-element ((design-element design-element) tmodel)
  (when design-element
    (find design-element (nodes (circulation-model tmodel))
	  :key #'(lambda (x) (design-element (corresponding-edge x))))))

;;; +++ fix this for use-space
#+ignore
(defmethod find-circulation-node-for-element ((design-element basic-space) tmodel)
  (find-or-make-circulation-node-for-region
    (bounded-region-for design-element tmodel) tmodel))

(defun find-circulation-node-for-point (x y tmodel)
  (when (and x y)
    (find `(,x ,y) (nodes (circulation-model tmodel)) :test #'equal
	  :key #'(lambda (n) `(,(point-x n) ,(point-y n))))))

(defun find-circulation-node-for-edge (edge tmodel
					    &optional (cmodel (circulation-model tmodel)))
  (when edge
    (find edge (nodes cmodel) :test #'eq :key #'corresponding-edge)))

(defun find-circulation-node-for-territory (name-or-region tmodel)
  (let ((region (if (typep name-or-region 'territory) name-or-region
		    (find-territory name-or-region tmodel))))
    (multiple-value-bind (x y)
	(physical-center region tmodel nil)
      (find-circulation-node-for-point x y tmodel))))

(defun find-or-make-circulation-node-for-territory (region tmodel)
  (or (find-circulation-node-for-territory region tmodel)
      (add-circulation-node-for-territory-center region tmodel)))

;; can pass in tmodel?
(defun find-or-make-circulation-node-for-edge (edge region cmodel)
  (let ((node (or (find-circulation-node-for-edge edge (territory-model cmodel) cmodel)
		  (make-circulation-node edge))))
    (add-territory node region)
    (add-node cmodel node)
    node))

(defun circulation-nodes-for-territory (region cmodel)
  (loop for node in (nodes cmodel)
	when (node-territory-p node region)
	  collect node))



;; add plist for storing traversed region(s); maybe later add slot

;; defn of edge has changed; no longer mixes in model info; may need to dispatch methods
;; on model-edge

(defclass CIRCULATION-LINK (edge)
    ((distance :initform nil :initarg :distance :accessor link-distance)))

(defmethod print-object ((link circulation-link) stream)
  (with-slots (distance) link
    (format stream "<~a: ~a to ~a d=~,2f>" (type-of link) (name (endpoint1 link))
	    (name (endpoint2 link)) distance)))

(defun make-circulation-link (node1 node2 distance)
  (let ((link (make-instance 'circulation-link :endpoint1 node1 :endpoint2 node2
			     :distance distance)))
    (add-edge node1 link)
    (add-edge node2 link)
    link))


;; drawing
(defvar *path-node-color* clim:+red+)
(defvar *path-link-color* clim:+blue+)

(clim::define-presentation-type circulation-node ())
(clim::define-presentation-type circulation-link ())

;; can use basic-point method if don't want color
(defmethod draw-self ((node basic-circulation-node) stream)
  (clim:with-output-as-presentation (stream node 'circulation-node)
    (draw-point (point-x node) (point-y node) stream *path-node-color*)))

;; can use basic-edge method if don't want color
(defmethod draw-self ((link circulation-link) stream)
  (let ((node1 (endpoint1 link))
	(node2 (endpoint2 link)))
    (clim:with-output-as-presentation (stream link 'circulation-link)
      (draw-edge (point-x node1) (point-y node1)
		    (point-x node2) (point-y node2) stream *path-link-color*))))

(defmethod draw-self- ((link circulation-link) stream)
  ;; regular method doesn't draw endpoints
  (draw-self link stream))


(defclass CIRCULATION-MODEL (basic-graph territory-model-mixin basic-object) 
    ())

;; don't automatically do this anymore; want to be able to store a circulation model on an
;; edge model
#+ignore
(defmethod initialize-instance :after ((model circulation-model) &rest plist)
  plist
  (let ((tmodel (territory-model model)))
    (when tmodel (set-circulation-model tmodel model))))

(defmethod add-node ((model circulation-model) node)
  (add-point model node))

(defmethod clear-paths ((model circulation-model))
  (dolist (node (nodes model))
    (clear-paths node)))
 
(defmethod connect-circulation-nodes ((node1 basic-circulation-node)
				      (node2 basic-circulation-node) cmodel)
  (let ((node1-edges (edges node1))
	(node2-edges (edges node2))
	link)
    (unless (intersection node1-edges node2-edges)
      (setf link (make-circulation-link node1 node2 
		(distance (point-x node1) (point-y node1) (point-x node2) (point-y node2))))
      (add-edge cmodel link)))) 

(defmethod connect-circulation-node ((edge edge) (edges list) region cmodel)
  (let ((node1 (find-or-make-circulation-node-for-edge edge region cmodel)))
    (connect-circulation-node node1 (car edges) region cmodel)
    (connect-circulation-node node1 (cdr edges) region cmodel)))

(defmethod connect-circulation-node ((node circulation-node) (edge edge) region
				     cmodel)
  (when edge 
    (connect-circulation-nodes node (find-or-make-circulation-node-for-edge
				      edge region cmodel)
			       cmodel)))

(defmethod connect-circulation-node ((node circulation-node) (edges list) region cmodel)
  (loop for edge in edges
	do (connect-circulation-node node edge region cmodel)))


;; territory model's circulation model later may be derived from design model's circulation
;; model; would start out as a copy, nodes for territory centers are added when path
;; between particular territories are requested; for now, create circulation model for each
;; territory model

(defun setup-circulation-model-internal (tmodel)
  ;; for each region in territory model, for each void edge in region's edges, connect
  ;; edge to all other void edges for that region
  ;; mod to only use void edges wide enough for person
  (loop with cmodel = (make-instance 'circulation-model :name (name tmodel)
				     :territory-model tmodel)
	for region in (territories tmodel)
	as void-edges = #+ignore (void-edges region)
	   (void-edges-for-circulation region)
	do (loop for edge in void-edges
		 do (connect-circulation-node edge (remove edge void-edges) region
					      cmodel)) 
	finally (progn (insert-entry-and-approach-points tmodel cmodel)
		       (return cmodel))))

(defmethod setup-circulation-model ((tmodel territory-model))
  ;; should nodes be added for all territories??
  (let ((cmodel (setup-circulation-model-internal tmodel)))
    (set-circulation-model tmodel cmodel)
    cmodel))

(defmethod setup-circulation-model ((emodel edge-model))
  (let ((cmodel (setup-circulation-model-internal
		  (territory-model-for-circulation-model (design-model emodel)))))
    (set-circulation-model emodel cmodel)
    cmodel))

(defmethod setup-circulation-model ((dmodel design-model))
  (setup-circulation-model (edge-model dmodel)))

#+ignore
;; territory models still have own circulation model, so don't use this method
(defmethod setup-circulation-model ((dmodel design-model))
  ;; really should use edge-model, but use disjoint-territory-model because simpler; assume
  ;; disjoint-territory-model won't be accessible to user (so nodes won't be added to
  ;; circulation model for its territories);  if disjoint-territory-model is 
  (let ((tmodel (disjoint-territory-model dmodel)))
    (when (typep tmodel 'territory-model)
      (loop with cmodel = (make-instance 'circulation-model :name (name tmodel)
					 :territory-model tmodel)
	    for region in (territories tmodel)
	    as void-edges = #+ignore (void-edges region)
	       (void-edges-for-circulation region)
	    do (loop for edge in void-edges
		     do (connect-circulation-node edge (remove edge void-edges) region
						  cmodel)) 
	    finally (progn (insert-entry-and-approach-points tmodel)
			   (return cmodel))))))
      
;; drawing methods assume paths have been computed
#|
;;; +++ mod these to use territories or use-spaces

(defmethod draw-all-paths-to-space ((model circulation-model) stream space)
  (with-slots (nodes) model
    (let* ((tmodel (territory-model model))
	   (node (find-circulation-node-for-territory (territory-for space tmodel)
		   tmodel)))
      (when node
	(dolist (path (all-paths-to-node node))		
	  (draw-self path stream))))))

(defmethod draw-all-paths-to-space ((model territory-model) stream space)
  (draw-all-paths-to-space (circulation-model model) stream space))

(defmethod draw-shortest-path-to-space ((model circulation-model) stream space &optional from)
  (with-slots (nodes) model
    (let* ((tmodel (territory-model model))
	   (node (find-circulation-node-for-territory (territory-for space tmodel)
		   tmodel))
	   path)
      (when (and node (setq path (shortest-path-to-node node model from)))
	(draw-self path stream)))))

(defmethod draw-shortest-path-to-space ((model territory-model) stream space &optional from)
  (draw-shortest-path-to-space (circulation-model model) stream space from))

|#

;;; PATH: lists of circulation-nodes and circulation-links, each list assumed to be in
;;; reverse order.  Toplevel path-finding functions are #'path-from-x-to-y and
;;; #'paths-from-x-to-y, where x and y currently can be territories, design-elements, edges, 
;;; symbols.  (See #'find-circulation-node for ok types.)  Path-finding functions work by
;;; finding circulation nodes that correspond to x and y, then finding a path or paths between
;;; the nodes.  Paths are stored on the circulation node for destination (i.e. y). 

(defclass PATH (basic-graph circulation-model-mixin plist-mixin)
   ())


(defun make-path (nodes edges &optional cmodel)
  (make-instance 'path :points nodes :edges edges :circulation-model cmodel))

(defmethod start-node ((path path))
  (car (last (nodes path))))

(defmethod end-node ((path path))
  (car (nodes path)))

(defmethod ordered-edges ((path path))
  (reverse (edges path)))

(defmethod ordered-nodes ((path path))
  (reverse (nodes path)))

;; currently not called
(defmethod fixup-path ((path path))
  (setf (edges path) (reverse (edges path)))
  path)

(defun open-edges-crossed-by-path (path)
  (loop for node in (nodes path)
	as seg = (corresponding-edge node)
	when seg collect it))

(defun ordered-open-edges-crossed-by-path (path)
  (reverse (open-edges-crossed-by-path path)))

(defun void-open-edges-crossed-by-path (path)
  (loop for seg in (reverse (open-edges-crossed-by-path path))
	    when (voidp seg) collect seg))

(defun paths-crossing-edge (edge paths)
  (loop for path in paths
	as segs = (open-edges-crossed-by-path path)
	when (member edge segs)
	  collect path))

#+ignore
(defmethod traversed-region ((link circulation-link))
  (or (get-property-value link 'region)
      (let ((region (car (intersection (territories (endpoint1 link))
				       (territories (endpoint2 link))))))
	(set-property-value link 'region region))))
  
;; Old version above collected territories from edges; problem:  edges may coincide with shared
;; boundary and endpoints aren't ordered when path is constructed (but when circulation model
;; is setup), so use nodes which are ordered when path is constructed

(defun path-territories (path)
  ;; territories traversed
  (mapappend+ #'territories (ordered-nodes path)))

(defun path-signature (path)
  (signature path))

(defmethod signature ((nodes list))
  (compress-list (ordered-territories nodes)))

(defmethod signature ((path path))
  (signature (ordered-nodes path)))

(defun ok-path-signature (path-or-nodes)
  (let ((signature (signature path-or-nodes)))
    (when (= (length (remove-duplicates signature))(length signature))
      signature)))

(defun ordered-territories (nodes)
  (let ((first-territories (territories (first nodes))))
    (if (= (length first-territories) 1)
	(order-territories (car first-territories)(cdr nodes))
	(order-territories 
	  (car (set-difference (territories (first nodes)) (territories (second nodes))))
	  nodes))))

(defun order-territories (start-region nodes)
  (loop for node in nodes
	with territories = (list start-region)
	with current-region-list =  (list start-region)
	as next-region-list = (set-difference (territories node) current-region-list)
	do (setf territories (append next-region-list territories))
	   (when next-region-list (setf current-region-list next-region-list))
	finally (return (reverse territories))))

(defmethod path-edges-from-start-to-end ((path path))
  (reverse (edges path)))

(defmethod print-object ((path path) stream)
  (let ((length (length (edges path))))
    (format stream "<PATH (~a): ~a to ~a ~a ~a d=~,2f>"
	    (small-string (name (circulation-model path)))
	    (name (start-node path)) (name (end-node path)) length
	    (if (= length 1) "edge" "edges")
	    (path-distance path))))
  
(defmethod path-distance ((path path))
  (apply '+ (mapcar #'link-distance (edges path))))

(defmethod path-distance ((path (eql 'nil)))
  nil)

(defmethod draw-self :around ((path path) stream)
  (let ((*path-link-color* clim:+red+))
    (call-next-method)))

;; methods for finding path

(defmethod init-for-path-search ((node basic-circulation-node))
  ; (set-property-value node 'path nil)
  (set-property-value node 'cached-paths nil))

(defmethod add-path ((node basic-circulation-node) path)
  (when path
    (add-property-value node 'cached-paths path))) 

(defmethod remove-path ((node basic-circulation-node) path)
  (when path
    (remove-property-value node 'cached-paths path)))

(defmethod remove-paths ((node basic-circulation-node) paths)
  (when paths
    (remove-property-values node 'cached-paths paths)))

(defmethod paths-to-node ((node basic-circulation-node) from-node)
  (get-property-value node from-node))

(defmethod cached-paths-to-node ((node basic-circulation-node))
  (get-property-value node 'cached-paths))

(defmethod all-paths-to-node ((node basic-circulation-node))
  (loop for (a b) on (plist node) by #'cddr
	when (typep (car b) 'path)
	  append b))

(defmethod clear-paths ((node basic-circulation-node))
  (set-property-value node 'paths nil))

(defun find-path-with-signature (paths sig)
  (find sig paths :key #'path-signature :test #'equal))

(defmethod copy-path ((path path))
  (make-path (nodes path) (edges path) (circulation-model path)))

(defun path-between-p (path node1 node2)
  (or (and (eq (start-node path) node1)
	   (eq (end-node path) node2))
      (and (eq (start-node path) node2)
	   (eq (end-node path) node1))))

(defun shortest-cached-path (node from-node)
  ;; works only when constructing paths
  (loop for path in (cached-paths-to-node node)
        as path-d = (path-distance path)
	with min-d 
	with min-path
	when (and (path-between-p path node from-node)
		  (or (null min-d) (< path-d min-d)))
	  do (setq min-d path-d)
	    (setq min-path path)
	finally (return min-path)))

(defun shortest-path (paths &optional (distance-fcn #'path-distance))
  (loop for path in paths
        as path-d = (funcall distance-fcn path)
	with min-d 
	with min-path
	when (or (null min-d) (< path-d min-d))
	  do (setq min-d path-d)
	    (setq min-path path)
	finally (return min-path)))

(defun shortest-paths (paths &optional (distance-fcn #'path-distance))
  (let ((min-d (funcall distance-fcn (shortest-path paths distance-fcn))))
    (loop for path in paths
	  as path-d = (funcall distance-fcn path)
	  with min-paths
	  when (= path-d min-d)
	    do (push path min-paths)
	  finally (return min-paths))))

(defun shortest-saved-path (node from-node)
  (shortest-path (paths-to-node node from-node)))

(defun shortest-path-to-node (node cmodel &optional from-node)
  (unless from-node
    (setq from-node (find-circulation-node 'front-door (territory-model cmodel))))
  (shortest-saved-path node from-node))

(defun visit-node (node link path-so-far cmodel)
  (let ((new-path nil))
    (if link
	(let* ((edges (cons link (edges path-so-far)))
	       (nodes (cons node (nodes path-so-far)))
	       (new-signature (ok-path-signature (reverse nodes)))
	       (path (find-path-with-signature (cached-paths-to-node node) new-signature)))
	  (flet ((make-new-path ()
		   (setq new-path (make-path (cons node (nodes path-so-far)) edges cmodel)))
		 (remove-paths-with-signature (sig node)
		   (loop for path in (cached-paths-to-node node)
			 when (equal (path-signature path) sig)
			   collect path into paths
			 finally (remove-paths node paths))))
	    (if path
		;; same signature: if fewer number of edges, replace; if same, save also
		;; have to remove all with that signature, because saved all with same length
		(cond ((< (1+ (number-of-edges path-so-far)) (number-of-edges path))
		       (remove-paths-with-signature new-signature node)
		       (add-path node (make-new-path)))
		      ((= (1+ (number-of-edges path-so-far)) (number-of-edges path))
		       (add-path node (make-new-path))))
		;; no existing path with signature
		(when new-signature (add-path node (make-new-path))))))
	(setq new-path path-so-far))
    (when new-path
      (loop for next-link in (edges node)
	    as other-endpoint = (other-endpoint next-link node)
	    when (and (not (eq link next-link))
		      (not (member other-endpoint (nodes path-so-far))))
	      do (visit-node other-endpoint next-link new-path cmodel))))) 


;; Main path finding function:  find all paths with unique (and valid) signatures; may have
;; more than one path per signature

#+ignore
;; don't need anymore, call #'shortest-path on result of #'find-all-paths
(defun find-shortest-path (from-node to-node cmodel)
  (when (and from-node to-node)
    (let ((path-so-far (make-path `(,from-node) nil)))
      (visit-node from-node nil path-so-far cmodel)
      (prog1 (shortest-cached-path to-node from-node)
	     (save-all-cached-paths cmodel from-node)))))

(defun find-all-paths (from-node to-node cmodel)
  (when (and from-node to-node)
    (let ((path-so-far (make-path `(,from-node) nil)))
      (visit-node from-node nil path-so-far cmodel)
      (prog1 (cached-paths-to-node to-node)
	     (save-all-cached-paths cmodel from-node)))))


(defun remove-cached-paths (to-node)
  (remove-property to-node 'cached-paths))

(defun save-all-cached-paths (cmodel from-node)
  (dolist (to-node (nodes cmodel))
    (set-property-value to-node from-node (cached-paths-to-node to-node))
    (remove-cached-paths to-node)))

;; insert centers as nodes

;; use center of space (default is center of bounding rectangle), use midpoint of design
;; element edge, use center of design element bounded region.

#+ignore
(defun find-or-add-circulation-node (region x y edge tmodel)
  ;; create new node and connect to other nodes in region
  (let ((cmodel (circulation-model tmodel)))
  (or (find-circulation-node-for-point x y tmodel)
      (find-circulation-node-for-edge edge tmodel)
      (let ((nodes (circulation-nodes-for-territory region cmodel))
	    (new-node (make-circulation-node edge x y region)))
	(add-node cmodel new-node)
	(loop for node in nodes
	      do (connect-circulation-nodes new-node node cmodel))
	new-node))))

(defun add-circulation-node (region x y edge tmodel)
  ;; create new node and connect to other nodes in region
  (let* ((cmodel (circulation-model tmodel))
	 (nodes (circulation-nodes-for-territory region cmodel))
	 (new-node (make-circulation-node edge x y region)))
    (add-node cmodel new-node)
    (loop for node in nodes
	  do (connect-circulation-nodes new-node node cmodel))
    new-node))

(defun add-circulation-node-for-territory-center (name-or-region tmodel)
  (let ((region (if (typep name-or-region 'territory) name-or-region
		    (find-territory name-or-region tmodel))))
    (multiple-value-bind (x y)
	(physical-center region tmodel nil)
      (add-circulation-node region x y nil tmodel))))


;; Add entry point and approach point

;;; +++ Is exterior region needed?

#+ignore
(defun insert-entry-and-approach-points (tmodel)
  ;; assume that entry-edge has already been added to the circulation model (because it
  ;; is a void edge)
  (let ((entry-edge (find-entry-point-edge tmodel))
	(approach-point (find-approach-point tmodel))
	(cmodel (circulation-model tmodel)))
    (when (and entry-edge approach-point)
      (let ((entry-node (find-circulation-node-for-edge entry-edge tmodel))
	    (approach-node (make-circulation-node-for-approach-point (point-x approach-point)
						       	     (point-y approach-point)
							     approach-point))
	    link)
	(add-node cmodel approach-node)
	(setq link (connect-circulation-nodes entry-node approach-node cmodel))
	#+ignore
	(set-link-region link 'exterior)))))

;; Approach points and entry segments: assumes more than one

(defmethod find-approach-points ((model design-model))
  (find-markers :approach-point (markers model)))

(defmethod find-approach-points ((model territory-model))
  (find-approach-points (design-model model)))

(defmethod find-entry-edge-for-approach-point ((model design-model) approach-point)
  (find-entry-edge-for-approach-point (edges model) approach-point))

(defmethod find-entry-edge-for-approach-point ((model territory-model) approach-point)
  (find-entry-edge-for-approach-point (design-model model) approach-point))

(defmethod find-entry-edge-for-approach-point ((segments list) approach-point)
  ;; find closest entry-point
  (let ((entry-segments (loop for segment in segments
			    when (get-property-value segment :entry-point)
			      collect segment)))
    ;; if only one, assume it matches
    (if (= (length entry-segments) 1) (car entry-segments)
	(loop for segment in entry-segments
	      as d = (abs (midpoint-point-distance segment approach-point))
	      with min-d 
	      with min-segment
	      do (if min-d
		     (when (> min-d d)
		       (setq min-d d) (setq min-segment segment))
		     (progn (setq min-d d) (setq min-segment segment)))
	      finally (return min-segment)))))

(defmethod find-private-area-entry-point-edge ((segments list))
  (find :private-area-entry-point segments
	:key #'(lambda (x) (get-property-value x :private-area-entry-point))))

(defmethod find-private-area-entry-point-edge ((model territory-model))
  (find-private-area-entry-point-edge (edges model)))


(defun insert-entry-and-approach-points (tmodel &optional (cmodel (circulation-model tmodel)))
  ;; assume that entry-edges have already been added to the circulation model (because they
  ;; are void edges)
    (loop for approach-point in (find-approach-points tmodel)
	  as entry-edge = (find-entry-edge-for-approach-point tmodel approach-point)
	  as entry-node = (find-circulation-node-for-edge entry-edge tmodel cmodel)
	  as approach-node = (make-circulation-node-for-approach-point
			     (point-x approach-point) (point-y approach-point) approach-point)
	  do (add-node cmodel approach-node)
	(connect-circulation-nodes entry-node approach-node cmodel)))


(defun find-cached-path (start end)
  ;; find shortest one (otherwise take first of (paths-to-node end start)
  (shortest-saved-path end start))


(defun path-from-node-x-to-node-y (x y tmodel)
  ;; cmodel arg makes more sense, but tmodel is consistent with other methods
  (let ((cmodel (circulation-model tmodel)))
    (dolist (node (nodes cmodel))
      (init-for-path-search node))
    (shortest-path (find-all-paths x y cmodel))
    #+ignore
    (find-shortest-path x y cmodel)))

(defmacro path-from-start-to-end-node (start end tmodel check-cache-p)
  `(let* (;(cmodel (circulation-model ,tmodel))
	  (start-node ,start)
	  (end-node ,end))
     (when (and start-node end-node)
       (or (when ,check-cache-p (find-cached-path start-node end-node))
	   (path-from-node-x-to-node-y start-node end-node ,tmodel)))))

;; Find circulation nodes

;; Need a way to handle markers; treat as special symbols for now. 
;; What about more than one approach point?  Later...

(defmethod find-circulation-node ((x design-element) tmodel)
  (find-circulation-node-for-element x tmodel))

(defmethod find-circulation-node ((x edge) tmodel)
  (find-circulation-node-for-edge x tmodel))

(defmethod find-circulation-node ((x symbol) tmodel)
  (find-circulation-node (find-design-element x (design-model tmodel)) tmodel))

(defmethod find-circulation-node ((x (eql 'private-area)) tmodel)
  (find-circulation-node-for-edge (find-private-area-entry-point-edge tmodel) tmodel))

(defmethod find-circulation-node ((x (eql 'approach-point)) tmodel)
  (find-circulation-node-for-approach-point tmodel))

(defmethod find-circulation-node ((x territory) tmodel)
  (find-or-make-circulation-node-for-territory x tmodel))


;; Shortest path between x and y
;;   x and y can be design elements, points, or markers (points not implemented yet)

(defmethod path-from-x-to-y ((x basic-circulation-node) (y basic-circulation-node) tmodel)
  (path-from-start-to-end-node x y tmodel t))

(defmethod path-from-x-to-y (x y tmodel)
  (path-from-x-to-y (find-circulation-node x tmodel) (find-circulation-node y tmodel)
		    tmodel))

;; All paths with unique (and valid) signatures between x and y

(defmethod paths-from-x-to-y ((x basic-circulation-node) (y basic-circulation-node) tmodel)
  (path-from-start-to-end-node x y tmodel t)
  (paths-to-node y x))

(defmethod paths-from-x-to-y (x y tmodel)
  (paths-from-x-to-y (find-circulation-node x tmodel) (find-circulation-node y tmodel)
		     tmodel))

(defmethod paths-from-x-to-y ((x (eql 'approach-points)) y tmodel)
  (let ((start-nodes (find-circulation-nodes-for-approach-points tmodel))
	(end-node (find-circulation-node y tmodel)))
    (loop for node in start-nodes
	  append  (paths-from-x-to-y node end-node  tmodel))))


;; Topologically shortest paths between x and y

(defun shortest-physical-path (from to tmodel)
  ;; just for consistency with shortest-topological-path
  (path-from-x-to-y from to tmodel))

(defun shortest-physical-paths (from to tmodel)
  ;; just for consistency with shortest-topological-path
  (shortest-paths (paths-from-x-to-y from to tmodel)))

(defun topological-length (path)
  (length (path-signature path)))

(defun shortest-topological-path (from to tmodel)
  (shortest-path (paths-from-x-to-y from to tmodel) #'topological-length))

(defun shortest-topological-paths (from to tmodel)
  (shortest-paths (paths-from-x-to-y from to tmodel) #'topological-length))



;; Path intersection

#+ignore
(defun path-intersects-center-of-space-p (path space
			     &optional (tmodel (default-territory-model (design-model space))))
  (edges-intersect-physical-center-rectangle-p (edges path) space tmodel))


;; Path circuity

(defun path-direction-changes (path)
  (loop for (seg1 seg2) on (path-edges-from-start-to-end path)
	when (and seg1 seg2)
	  collect (segment-direction-change seg1 seg2)))

(defun nonzero-direction-changes (path)
  (loop for change in (path-direction-changes path)
	when (nonzero-direction-change change)
	  collect change))

(defun nonzero-direction-change (x)
  (>= (abs x) .50))

(defun circuitousp (path)
  (>= (length (nonzero-direction-changes path)) 2))


;; Finding entrance edge into destination space:  assumes path is between centers of spaces,
;; so penultimate link in path leads to destination space entrance edge

(defmethod link-to-destination-space-entrance ((path path))
  ;; assumes path between 2 (centers of) spaces
  ;; to be safe really should pass in space and check edges
  (cadr (edges path)))

(defmethod destination-entrance-edge ((path path) &optional region)
  (let ((region (or region (car (territories (end-node path))))))
    (loop with openings = (void-edges region)
	  for node in (nodes path)
	  as edge = (corresponding-edge node)
	  when (member edge openings)
	    return edge)))