;;; This software is being provided to you, the LICENSEE, by the
;;; Massachusetts Institute of Technology (M.I.T.), and the NEC Research
;;; Institute (NECI) under the following license.  By obtaining, using
;;; and/or copying this software, you agree that you have read,
;;; understood, and will comply with these terms and conditions:
;;; 
;;; Permission to use, copy, modify and distribute, including the right to
;;; grant others rights to distribute at any tier, this software and its
;;; documentation for research purposes only and without fee or royalty is
;;; hereby granted, provided that you agree to comply with the following
;;; copyright notice and statements, including the disclaimer, and that
;;; the same appear on ALL copies of the software and documentation,
;;; including modifications that you make for internal use or for
;;; distribution:
;;; 
;;; Copyright 1995 by the Massachusetts Institute of Technology.  All
;;; rights reserved.  Copyright 1995 by NEC Research Institute Inc. All
;;; rights reserved
;;; 
;;; THIS SOFTWARE IS PROVIDED "AS IS", AND M.I.T. and NECI MAKE NO
;;; REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.  By way of example,
;;; but not limitation, M.I.T. and NECI MAKE NO REPRESENTATIONS OR
;;; WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY PARTICULAR PURPOSE OR
;;; THAT THE USE OF THE LICENSED SOFTWARE OR DOCUMENTATION WILL NOT
;;; INFRINGE ANY THIRD PARTY PATENTS, COPYRIGHTS, TRADEMARKS OR OTHER
;;; RIGHTS.
;;; 
;;; The name of the Massachusetts Institute of Technology or M.I.T., or
;;; NEC Research Institute, or NECI may NOT be used in advertising or
;;; publicity pertaining to distribution of the software.  Title to
;;; copyright in this software and any associated documentation shall at
;;; all times remain with M.I.T. and NECI, and USER agrees to preserve
;;; same.
;;; 

;;; -*- Package: GROUP; Mode: LISP; Base: 10; Syntax: Common-Lisp -*-

(in-package group)

;;; Given a convex-group, this file will find point features.

;;; In a convex group the lines are arranged so that the second point of the
;;; last line should join the first point of the next to last line ...
;;; and the second point of the first line joins the first point of the last line.

;;; Whether a line segment is stable depends on the strings that produce it.  Put this off 
;;; for now.

;;; Suppose the lines are colinear, or parallel and almost colinear.  
;;; Think of this as arising from: ____________|----|___________
;;; The colinear lines will be in the same convex group, and their endpoints
;;; will be stable, so if they're close enough together, there should be a stable 
;;; corner.  

;;; COMMENTS
;;;   The method of computing line-segment-angle-error seems bad.

;;; To Do
;;;   If a line doesn't contribute to a corner, consider its neighboring lines.
;;;   Fix unstable corner problem from way back.
;;;   If only a small part of a long line appears in a cg, the presence of this line
;;;     is probably unstable, and so the line should not contribute to any corners.

(defvar *very-large-number* 999999999999)

(defvar *MAX-CORNER-INSTABILITY* 15)
(defvar *MAX-POINT-ERROR* 5)
(defvar *POINT-ERROR-FOR-CORNER-ANGLES* 5)
(defvar *GENERAL-CORNER-ERROR* 2)
(defvar *MERGEABLE-CORNER-DISTANCE* 2 "Corners may merge into one point if they are no more than
                                      this far from the point")
(defvar *MERGEABLE-CORNER-DISTANCE-SQUARED*
	(g:sq *MERGEABLE-CORNER-DISTANCE*))
(defvar *2-MERGEABLE-CORNER-DISTANCE-SQUARED*
	(g:sq (* 2 *MERGEABLE-CORNER-DISTANCE*)))
						
(defun CONVEX-GROUPS-WITH-SUPERSET-CORNERS (cgs &optional (min-size 0))
  "Even a cg has fewer than min-size corners, or its corners are a subset of the
   corners of another cg, than toss it out.  If many cgs have the same corners, keep
   only one of them.
  "
  (let ((big-cgs (remove-if #'(lambda (cg) (< (length (convex-group-corners cg)) min-size)) cgs)))
    (loop for cg1 in big-cgs
	  for i from 0
	  when (loop for cg2 in big-cgs
		     for j from 0
		     always
		       (if (< i j)
			   (not (list-of-convex-corners-proper-subset (convex-group-corners cg1) (convex-group-corners cg2)))
			   (if (< j i)
			       (not (list-of-convex-corners-subset (convex-group-corners cg1) (convex-group-corners cg2)))
			       t)))
	    collect cg1)))

(defun CONVEX-GROUPS-WITH-UNIQUE-CORNERS (cgs &optional (min-size 0))
  "Find the corners of each convex group, and only keep the ones that are truly different,
   and have more than MIN-SIZE corners.
  "
  (remdups (copy-list
	     (remove-if #'(lambda (cg) (< (length (convex-group-corners cg)) min-size)) cgs))
	   #'(lambda (cg1 cg2) (list-of-convex-corners-same (convex-group-corners cg1)
							  (convex-group-corners cg2)))
	   #'(lambda (cg1 cg2) (unordered-list-of-convex-corners-ordering
			       (convex-group-corners cg1)
			       (convex-group-corners cg2)))))

(defun UNORDERED-LIST-OF-CONVEX-CORNERS-ORDERING (list1 list2)
  "We have two lists of convex corners.  We want some canonical way of ordering them, so
   we can sort them and remove identical ones.
  "
  (let ((len1 (length list1))
	(len2 (length list2)))
    (if (< len1 len2) t
	(if (< len2 len1) nil
	    (let ((sorted-list1 (sort (copy-list list1) #'convex-corner-ordering))
		  (sorted-list2 (sort (copy-list list2) #'convex-corner-ordering)))
	      (loop for cc1 in sorted-list1
		    for cc2 in sorted-list2
		    doing
		(when (not (convex-corners-eq cc1 cc2))
		  (return
		    (if (convex-corner-ordering cc1 cc2)
			t
			nil)))
		    finally (return nil)))))))

(defun LIST-OF-CONVEX-CORNERS-SAME (list1 list2)
  "This fails if a list has the same convex corner more than once."
  (and (= (length list1) (length list2))
       (loop for cc in list1
	     always
	     (member cc list2 :test #'convex-corners-eq))))

(defun LIST-OF-CONVEX-CORNERS-SUBSET (list1 list2)
  "Is list 1 a subset of list 2?  It need not be a proper subset."
  (loop for cc in list1
	always (member cc list2 :test #'convex-corners-eq)))

(defun LIST-OF-CONVEX-CORNERS-PROPER-SUBSET (list1 list2)
  "Is list 1 a subset of list 2?  It need not be a proper subset."
  (and (< (length list1) (length list2))
       (loop for cc in list1
	     always (member cc list2 :test #'convex-corners-eq))))

(defun COMPUTE-CONVEX-GROUPS-CORNERS (cgs) ;  ht)
  (loop for cg in cgs
	doing
    (setf (convex-group-corners cg) (find-convex-group-corners cg)); ht))))
        finally (clear-memoized-function 'adjacent-lines-feature-point)))

;(defun FIND-CONVEX-GROUP-CORNERS (cg ht)
;  "ht is a hash table containing all unstable line segments"
;  (let* ((c-lines (remove-if (lambda (cl) (line-segment-unstable? (c-line-segment cl) ht))
;			     (cons (car (convex-group-lines cg)) (reverse (convex-group-lines cg))))))
;    (loop for cl1 in c-lines
;	  for cl2 in (cdr c-lines)
;	  for point-error = (adjacent-lines-feature-point cl1 cl2)
;	  when (and (cadr point-error) (< (cadr point-error) *max-corner-instability*))
;	    collect (make-convex-corner (car point-error) (c-line-segment cl1) (c-line-segment cl2)))))
;;	  finally (clear-memoized-function 'adjacent-lines-feature-point))))

(defun ADJACENT-LINES-FEATURE-POINT (cl1 cl2)
  "We have two lines in a sequence.  That is, ls1 = (p1-1 p1-2), ls2 = (p2-1 p2-2)
   and a connection is hypothesized between p1-2 and p2-1.
   We want to find the feature point, if any that
   is their vertex.  There are two issues.  First, do we want to say there is a feature
   point.  Reasons for saying there isn't one are:
     - Are the end points being connected stable?  Could a little error in the underlying
       edges change the end points a lot?  We can't judge this by just looking at the two
       lines, however.
     - Is the location of the vertex stable?
   Second, where is the vertex?
   In addition to these questions, we probably want to make this a memo-ized function, since many
   different convex groups will share the same line segment sequences.

   It's possible that one of the line segments should be truncated, for reasons of convexity.
   In that case, we locate the end point of the line at the intersection point when determining
   the two rays, although we use the full line length for determing the error.

   Reconsider what happens if the rays don't intersect after angle error is added
   (this might be a problem for the old code too).  I think this is OK because of symmetry.
  "
  (destructuring-bind (ls1 ls2 line-rays-should-intersect?)
      (convex-cls-line-segments cl1 cl2)
    (if (not line-rays-should-intersect?)
	(list nil nil)
	(let* ((point-error (relative-point-error ls1 ls2)))
	  (if (point-equal (line-segment-point-2 ls1) (line-segment-point-1 ls2))
	      (list (line-segment-point-2 ls1)
;		    (+ point-error *GENERAL-CORNER-ERROR*))
		    (connected-line-error ls1 ls2))
	      (let ((ip-error (ray-intersection-error
				(line-segment-point-2 ls1) (line-segment-point-1 ls2)
				(c-line-vector cl1)
				(c-line-reverse-vector cl2)
				point-error point-error
				(line-segment-angle-error (c-line-segment cl1))
				(line-segment-angle-error (c-line-segment cl2)))))
		(if (car ip-error) 
                    (list (float-point (car ip-error)) 
                          (+ *straight-line-approximation-extra-error* (cadr ip-error)))
                          ;; This extra error is our uncertainty about the overall position
                          ;; of the pair of end points, as opposed to our uncertainty about
                          ;; their relative position.
		    ip-error)))))))

(defun CONNECTED-LINE-ERROR (ls1 ls2)
  "If (line-segment-point-2 ls1) = (line-segment-point-1 ls2), then error is due to two factors.
   - error in edge detection.
   - error introduced by straight-line approximation.
  "
  (let ((forward-shift (amount-corner-can-shift ls1 ls2 :forward))
	(back-shift (amount-corner-can-shift ls1 ls2 :back)))
    (if (or (< (line-segment-length ls1) back-shift)
	    (< (line-segment-length ls2) forward-shift))
	*very-large-number*
	(max back-shift forward-shift))))

(defun CONVEX-CLS-LINE-SEGMENTS (cl1 cl2)
  (if (point-equal (c-line-point-2 cl1) (c-line-point-1 cl2))
      (list (c-line-segment cl1) (c-line-segment cl2) (c-line-point-2 cl1))
      (let* ((ip-unfloated (intersection-of-line-and-line (2dg:g-coerce (c-line-segment cl1) '2dg::line)
							  (2dg:g-coerce (c-line-segment cl2) '2dg::line))))
	(if (or (not (point-p ip-unfloated)) (c-lines-colinear cl1 cl2))
	    ;; c-lines are parallel
	    (list (c-line-segment cl1) (c-line-segment cl2) t)
	    (let* ((ip (if ip-unfloated (float-point ip-unfloated) nil))
		   (ip-in-front-line-1? (plusp (dot (c-line-vector cl1)
						    (diff ip (c-line-point-2 cl1)))))
		   (ip-in-front-line-2? (minusp (dot (c-line-vector cl2)
						     (diff ip (c-line-point-1 cl2)))))
		   (ip-in-back-line-1? 
                     (if ip-in-front-line-1? nil
                         (not (plusp (dot (c-line-vector cl1)
				     (diff ip (c-line-point-1 cl1)))))))
		   (ip-in-back-line-2? 
                     (if ip-in-front-line-2? nil
                         (not (minusp (dot (c-line-vector cl2)
				           (diff ip (c-line-point-2 cl2))))))))
	      (if (or ip-in-back-line-1? ip-in-back-line-2?)
		  (list nil nil nil)
		  ;; This means the intersection point is behind the lines.  This can occur
		  ;; when the angle between the lines is greater than pi.  We assume that
		  ;; no useful corner can come from such a situation.
		  (list (if ip-in-front-line-1?
			    (c-line-segment cl1)
			    (make-line-segment (c-line-point-1 cl1) ip))
			(if ip-in-front-line-2?
			    (c-line-segment cl2)
			    (make-line-segment ip (c-line-point-2 cl2)))
			t)))))))

(defun FLOAT-POINT (p)
  (make-point (float (point-x p)) (float (point-y p))))

(defun RELATIVE-POINT-ERROR (ls1 ls2)
  (let ((d (distance (line-segment-point-2 ls1) (line-segment-point-1 ls2))))
    (if (= d 0)
	(straight-line-approximation-error ls1 ls2)
	(min *max-point-error* (/ d 10.0)))))

(defun LINE-SEGMENT-ANGLE-ERROR (ls &optional (error *point-error-for-corner-angles*))
  (/ error (line-segment-length ls)))

;;; RAY-INTERSECTION-ERROR inefficient because p1 and p2 may be generated many times.
(defun RAY-INTERSECTION-ERROR (p1 p2 v1 v2 p1-error p2-error v1-error v2-error)
  "We are given two rays, in the form of their points, and vectors for each ray.
   There may be error in the locations of the points, up to p1-error, p2-error, in
   any direction.  There may be error in the vectors, which is given in terms of angle.
   We want to find the maximum amount of variation in the intersection point of the rays.
   That is, how much can their intersection point deviate from the error-free case.
   Return (error-free-intersection-point magnitude-of-error).
   (nil nil) is returned when error can be infinite.
  "
  (let* ((a1 (angle-of-vector v1))
	 (a2 (angle-of-vector v2))
	 (a-diff (abs-angle-difference a1 a2))
	 (a-total-error (+ v1-error v2-error)))
    (if (or (<= a-diff a-total-error) (<= (abs (- pi a-diff)) a-total-error))
	;; If rays could be parallel (pointing in same direction) intersection is unstable
	;; If rays could be colinear (pointing in opposite directions, they are often produced
	;; by a single broken line.
	'(nil nil)
	(let ((corner (intersection-of-line-and-line (make-line p1 (add p1 v1)) (make-line p2 (add p2 v2)))))
		;; (let ((ip (intersection-of-ray-and-ray p1 v1 p2 v2)))
		;;   (if ip ip (multiply (add p1 p2) .5)))))
	  (list corner
		  (loop for ip in (ray-intersections@extremal-error p1 p2 a1 a2 p1-error p2-error v1-error v2-error)
			maximize (distance corner ip)))))))

(defun INTERSECTION-OF-RAY-AND-RAY (p1 v1 p2 v2)
  (let ((intersection (intersection-of-line-and-line (make-line p1 (add p1 v1)) (make-line p2 (add p2 v2)))))
    (if (or (null intersection) (line-p intersection))
	;; If the rays are colinear or parallel, return the mid-point between the two ray end points.
	nil
	(if (and (plusp (dot v1 (diff intersection p1)))
		 (plusp (dot v2 (diff intersection p2))))
	    intersection
	    ;; If not, the rays don't intersect.
	    nil))))

(defun RAY-INTERSECTIONS@EXTREMAL-ERROR (p1 p2 a1 a2 p1-error p2-error v1-error v2-error)
  "Here, instead of vectors, the angles of the rays are given"
  (loop for a1-error in (list v1-error (- v1-error))
	for v1 = (unit-vector-from-angle (+ a1 a1-error))
	appending
	(loop for a2-error in (list v2-error (- v2-error))
	      for v2 = (unit-vector-from-angle (+ a2 a2-error))
	      appending
	      (ray-intersections@extremal-point-error p1 p2 v1 v2 p1-error p2-error))))

(defun RAY-INTERSECTIONS@EXTREMAL-POINT-ERROR (p1 p2 v1 v2 p1-error p2-error)
  "Assume vectors are unit vectors"
  (loop with v1-normal = (normal v1)
	for p1-error in (list (multiply v1-normal p1-error) (multiply v1-normal (- p1-error)))
	for p1+error = (add p1 p1-error)
	appending
	  (loop with v2-normal = (normal v2)
		for p2-error in (list (multiply v2-normal p2-error) (multiply v2-normal (- p2-error)))
		for p2+error = (add p2 p2-error)
		for ip = (intersection-of-ray-and-ray p1+error v1 p2+error v2)
		when ip collect ip
		when (null ip) append (list p1 p2))))

(defun ANGLE-OF-VECTOR (v)
  (atan (point-y v) (point-x v)))

(defun UNIT-VECTOR-FROM-ANGLE (a)
  (make-point (cos a) (sin a)))

(defun MERGE-CONVEX-GROUPS-NEARBY-CORNERS (cgs)
  (merge-nearby-convex-corners (flatten (mapcar #'convex-group-corners cgs)))
  (mapcar #'remove-duplicate-corners cgs)
  cgs)

(defun REMOVE-DUPLICATE-CORNERS (cg)
  (setf (convex-group-corners cg) (remove-duplicates (convex-group-corners cg)
						     :test #'(lambda (c1 c2) (eq (convex-corner-point c1)
									       (convex-corner-point c2))))))

(defun MERGE-NEARBY-CONVEX-CORNERS (convex-corners)
  (let ((list-of-replace-point-old-point
	  (sort
	    (loop for rep-neigh in (mergeable-corners-and-reps (remdups (copy-list convex-corners)
									#'convex-corners-eq
									#'convex-corner-ordering))
		  ;; This is a pt, and the convex corners that are within *MERGEABLE-CORNER-DISTANCE* of it.
		  appending
		    (loop for n in (cadr rep-neigh)
			  collecting
			    (list (car rep-neigh) (convex-corner-point n))))
	    #'point-ordering
	    :key #'cadr)))
; (format t "~%list-of-replace-point-old-corner = ~A" list-of-replace-point-old-point)
    (loop with sorted-convex-corners = (sort (copy-list convex-corners) #'convex-corner-ordering)
;  do (format t "~%~%sorted-convex-corners = ~A...~%list-of-replace-point-old-corner = ~A"
;	     (loop for c in sorted-convex-corners repeat 5 collecting c) list-of-replace-point-old-point)
          until (or (null sorted-convex-corners) (null list-of-replace-point-old-point))
	  for replace-point-point = (car list-of-replace-point-old-point)
	  doing					
      (if (point-equal (cadr replace-point-point)
		       (convex-corner-point (car sorted-convex-corners)))
	  (setf (convex-corner-point (pop sorted-convex-corners)) (car replace-point-point))
	  (if (point-ordering (cadr replace-point-point) (convex-corner-point (car sorted-convex-corners)))
	      (pop list-of-replace-point-old-point)
	      (pop sorted-convex-corners))))))

(defun MERGEABLE-CORNERS-AND-REPS (convex-corners)
  (loop for neigh in (convex-corner-neighborhoods convex-corners)
	when (cdr neigh)
	  append (convex-corner-neighborhood-reps neigh)))

(defun CONVEX-CORNER-NEIGHBORHOOD-REPS (neigh)
  (if (null neigh) nil
      (if (null (cdr neigh))
	  (list (list (convex-corner-point (car neigh)) neigh))
	  (if (null (cddr neigh))
	      (if (<= (distance-squared-between-point-and-point
			(convex-corner-point (car neigh))
			(convex-corner-point (cadr neigh)))
		      *2-mergeable-corner-distance-squared*)
		  (list (list (multiply (add (convex-corner-point (car neigh))
					     (convex-corner-point (cadr neigh)))
					.5)
			      neigh))
		  (list (list (convex-corner-point (car neigh)) (list (car neigh)))
			(list (convex-corner-point (cadr neigh)) (list (cadr neigh)))))
	      (let ((neigh-rep
		      (biggest-represented-neighborhood neigh)))
		(cons neigh-rep
		      (convex-corner-neighborhood-reps (remlist (cadr neigh-rep) neigh))))))))

(defun BIGGEST-REPRESENTED-NEIGHBORHOOD (neigh)
  (let ((best-radius-squared *very-large-number*)
	(biggest-neigh nil)
	(best-center nil)
	(size-biggest-neigh 0))
    (loop for perimeter-corners in (all-pairs-and-triples neigh)
	  for perimeter-points = (mapcar #'convex-corner-point perimeter-corners)
	  doing
      (let ((center (circle-center-from-perimeter-points perimeter-points)))
	(when center
	  (let ((radius-squared (distance-squared-between-point-and-point center (car perimeter-points))))
	    (when (<= radius-squared *2-mergeable-corner-distance-squared*)
	      (let* ((circle-neigh (loop for c in neigh
					 for p = (convex-corner-point c)
					 when (or (member c perimeter-corners)
						  (<= (distance-squared-between-point-and-point center p)
						      *2-mergeable-corner-distance-squared*))
					   collect c))
		     (size-circle-neigh (length circle-neigh)))
		(when (or (< size-biggest-neigh size-circle-neigh)
			  (and (= size-biggest-neigh size-circle-neigh)
			       (<= radius-squared best-radius-squared)))
		  (setq best-radius-squared radius-squared
			size-biggest-neigh size-circle-neigh
			best-center center
			biggest-neigh circle-neigh))))))))
    (list best-center biggest-neigh)))

(defun ALL-PAIRS-AND-TRIPLES (lst)
  (append
    (loop for i in lst
	  for rest on (cdr lst)
	  appending
	    (loop for j in rest
		  collecting
		    (list i j)))
    (loop for i in lst
	  for rest1 on (cdr lst)
	  appending
	    (loop for j in rest1
		  for rest2 on (cdr rest1)
		  appending
		    (loop for k in rest2
			  collecting
			    (list i j k))))))

(defun CIRCLE-CENTER-FROM-PERIMETER-POINTS (pts)
  "Assume there is a circle with PTS on the perimeter, and pts are unique, so only the first 3 matter."
  (let ((pts (remdups pts #'point-equal)))
    (if (null (cdr pts))
	(car pts)
	(if (null (cddr pts))
	    (multiply (add (car pts) (cadr pts)) .5)
	    (let* ((p1 (car pts))
		   (p2 (cadr pts))
		   (p3 (caddr pts))
		   (x1 (point-x p1))
		   (y1 (point-y p1))
		   (x2 (point-x p2))
		   (y2 (point-y p2))
		   (x3 (point-x p3))
		   (y3 (point-y p3)))
	      (if (= x1 x2 x3)
		  nil
		  (if (= x1 x2)
		      (circle-center-from-perimeter-points (list p1 p3 p2))
		      (let* ((k (/ (+ (g:sq x2) (- (g:sq x1)) (g:sq y2) (- (g:sq y1)))
				   (* 2 (- x2 x1))))
			     (m (* 2 (/ (- y2 y1) (- x2 x1))))
			     (denom (* 2 (+ (* x1 m) (- (* x3 m)) y3 (- y1)))))
			(if (= 0 denom)
			    nil
			    (let ((y (/ (+ (g:sq x3) (- (g:sq x1)) (- (* 2 k (- x3 x1))) (g:sq y3) (- (g:sq y1)))
					denom)))
			      (make-point (- k (* m y)) y)))))))))))

(defun CONVEX-CORNER-NEIGHBORHOODS (corners)
  "Find sets of points using the neighbor equivalence class, where two
   points are neighbors if they are no more than (* 2 *MERGEABLE-CORNER-DISTANCE*)
   pixels apart.
  "
  (let ((buckets (sort-corners-in-buckets corners (* 2 *MERGEABLE-CORNER-DISTANCE*))))
    (loop for index in (get-buckets-indices buckets)
	  when (access-corner-buckets buckets index)
	    append (bucket-neighborhoods buckets index))))

(defun BUCKET-NEIGHBORHOODS (buckets index)
  (loop collect (corner-neighborhood (pop-corner-buckets buckets index) buckets index)
        until (null (access-corner-buckets buckets index))))

(defun CORNER-NEIGHBORHOOD (c buckets index)
  "Assume that all buckets before (i j) are empty, or we would have started with a seed
   that was earlier on."
  (let* ((i (car index))
	 (j (cadr index))
	 (neigh (expand-equivalence-class
		 (list (list c i j))
		 (loop for ii from i to (1+ i)
		       appending
			 (loop for jj from j to (1+ j)
			       appending
				 (loop for c in (access-corner-buckets buckets (list ii jj))
				       collecting
					 (list c ii jj))))
		 #'(lambda (c&i1 c&i2)
		   (<= (distance-squared-between-point-and-point
			 (convex-corner-point (car c&i1))
			 (convex-corner-point (car c&i2)))
		       *2-mergeable-corner-distance-squared*)))))
    (loop for corner&index in neigh
	  for corner = (car corner&index)
	  for index = (cdr corner&index)
	  doing
      (delete-corner-from-bucket corner buckets index))
    (mapcar #'car neigh)))

(defun EXPAND-EQUIVALENCE-CLASS (class rest fun &optional (in-class-looked-at nil))
  "Return class plus all elements of rest, r1, such that (fun c1 r1) => t for some element, c1,
   such that c1 is in class or will be added to class."
  (let* ((to-add 
	   (loop for r in rest
		 when (member r class :test fun)
		   collect r)))
    (if to-add
	(expand-equivalence-class to-add (remlist to-add rest) fun (append class in-class-looked-at))
	(append class in-class-looked-at))))

(defun SORT-CORNERS-IN-BUCKETS (corners bucket-width)
  (let ((buckets (make-buckets (length corners))))
    (loop for corner in corners
	  for pt = (convex-corner-point corner)
	  doing
      (add-corner-to-bucket corner buckets
			    (list (floor (/ (point-x pt) bucket-width))
			          (floor (/ (point-y pt) bucket-width)))))
    buckets))

(defun ACCESS-CORNER-BUCKETS (buckets index)
  (gethash index buckets))

(defun DELETE-CORNER-FROM-BUCKET (corner buckets index)
  (delete corner (gethash index buckets)))

(defun ADD-CORNER-TO-BUCKET (corner buckets index)
  (push corner (gethash index buckets)))

(defun MAKE-BUCKETS (size)
  (make-hash-table :test #'equal :size size))

(defun POP-CORNER-BUCKETS (buckets index)
  (pop (gethash index buckets)))

(defun GET-BUCKETS-INDICES (buckets)
  (let ((indices nil))
    (maphash #'(lambda (key &rest ignore)
		 (push key indices))
	     buckets)
    indices))