;;; 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)

(defvar *FIRST-T-THEN-NIL*)
(setq *FIRST-T-THEN-NIL* (list t nil))
(rplacd (cdr *FIRST-T-THEN-NIL*) (cdr *FIRST-T-THEN-NIL*))
;;; This makes *FIRST-T-THEN-NIL* a list of (t nil nil nil nil ...)

;;; SET-ANGLE-ARRAY can be fixed to handle angle change ambiguity only in case of colinearity.

;;; Removing overlapping sequences calls MAKE-CONVEX-GROUP which doesn't deal with new cg structure.
;;; Handle connected lines separately when determining convexity
;;; Relics that I think can be removed: a convex group begin closed, *positive-dot-threshold*, entering
;;;   four values in the LENGTH-ARRAY, when only the front of the first line and back of the second are
;;;   used.
;;; I think it would probably pay to store distance in the lookup table instead of the square of the distance.
;;;   this can be checked empirically.

(defvar *FULL-CONSTRAINTS* t)

(defvar *POSITIVE-DOT-THRESHOLD* -2)
(defvar *ABS-POSITIVE-DOT-THRESHOLD* 2)
(defvar *CONVEX-ADDS*)
(defvar *CONVEX-TOTAL*)
(defvar *SQUARED-IGNORABLE-DISTANCE* 4 "The square of the length of a connecting line in a group for which we ignore convexity")
(defvar *MAX-SQUARED-GAP* 40000 "We don't consider adding line 2 to a cg ending in line 1 if the gap between them is
                                 bigger than this."
	)

(defvar *MAX-CONNECTED-COLINEAR-LINE-DEVIATION* 6
  "Two connected lines are considered colinear if a line connecting their endpoints is no more than
   this value from any point on the lines.")

(defvar *COLINEAR-LINE-POINT-ERROR* 6)

(defvar *CONVEX-POSITIONAL-ERROR* 5)

(defvar *MAX-ANGLE-LINE-ERROR* (/ *short-pi* 15))

(defvar *MAX-ANGLES-CONVEX-GROUP* (+ *short-2pi* (* 2 *max-angle-line-error*)))

(export '(convex-lines convex-lines-from-edges))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; MATCH INFORMATION ABSTRACTION
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;; These macro definitions have to come before they are used.

(defun MAKE-MATCH-INFORMATION (compat-array dist-array match-array length-array angle-array)
  (list compat-array dist-array match-array length-array angle-array))

(defmacro MAKE-COMPATIBILITY-ARRAY (len)
;  `(make-array (list ,len ,len) :element-type 'boolean :initial-element NIL))
  `(make-array (list ,len ,len) :element-type 'bit :initial-element 0))

(defmacro MAKE-DISTANCE-ARRAY (len)
  `(make-array (list ,len ,len) :initial-element 0 :element-type '(unsigned-byte 32)))

(defmacro MAKE-MATCH-ARRAY (len)
  `(make-array (list ,len) :initial-element NIL))

(defmacro MAKE-ANGLE-ARRAY (len)
  `(make-array (list ,len ,len) :initial-element NIL))

;;; A good way to save space here might be to use a hash table, and only enter lengths
;;; not equal to (0 0).
(defmacro MAKE-LENGTH-ARRAY (len)
  `(make-array (list ,len ,len) :initial-element NIL))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; See below for more of this abstraction
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;; What is CONVEX-LINES? really supposed to compute, and does it?
;;; I think we want all convex lines that satisfy k, but in addition
;;; to that we want to rule out subsets, supersets, and duplicates if possible.
;;; - Duplicates:  It's inherent in my algorithm that we may build up to n
;;;   copies of a convex group with n lines, because we will try each line
;;;   as a starting point.  
;;; - If the sequence (... A B C) is convex and satisfies the gap constraint,
;;;   and so does the sequence (... A C), then we don't want to consider both possiblities,
;;;   if possible.  If we did, we would get every subset of a valid convex group that is also
;;;   valid.  We have to consider whichever of the two has a better gap ratio, because it might
;;;   be that only that one leads to a convex group.  Our current code considers that case, 
;;;   which means that a superset convex group with a weaker gap ratio might be omitted.
;;;   This is done by never considering (... A B C) if (... A C) has a better ratio.
;;;     On the other hand, if (... A B C) has the better ratio, than we don't consider 
;;;     (... A C) subsequently, but it's possible that we have already considered it.
;;;   If we always wanted to get the largest convex groups, a more conservative approach 
;;;     would be to always consider (... A B C), but exclude (... A C) when it has a lower
;;;     gap ratio.
;;; - Finally, we may get subsets because even though we exclude (... A C) when it has a 
;;;   lower gap ratio, we don't exclude (C ... A).

;;; Proposed changes to this:
;;;   I think we might want to define the output by saying that if two groups meet the gap
;;;   criteria, and one is a superset of the other, we want the one with the better gap ratio.
;;;   We already do this in many cases anyway.  This just means fixing the "finally" above.

(defun CONVEX-LINES (lines strings k)		; &optional c-array)
  "This routine should return every collection of line segments that meet the following criteria:
   Traversing a path from the first point of ls1 to the second point to the first point of ls2 ...
   should lead to no concavities.  If the length of this path is P, and the length of the segments
   is L, then L/P > k.
   Additionally, we do not include a line in a convex group if eliminating it would produce a higher
   ratio of L/P, and we try to avoid forming groups that are subsets of other groups.
  "
;  (when c-array
;    (let ((c-array-1d (make-array (list (reduce #'* (array-dimensions c-array))) :displaced-to c-array)))
;      (fill c-array-1d nil)))
    (when (null strings)
      (setq strings (loop repeat (length lines) collecting nil)))
  (let* ((c-lines (make-c-lines lines strings)) ; a list of c-lines and a list of their reverse
	 (len (length c-lines)))
    (let ((compat-array (make-compatibility-array len))
		(dist-array (make-distance-array len))
		(length-array (make-length-array len))
		(angle-array (make-angle-array len))
		(match-array (make-match-array len)))
      (let ((c-line-matches (make-c-line-matches c-lines k compat-array dist-array length-array angle-array match-array)))
	(convex-backtrack c-lines c-line-matches k)))))

(defun CONVEX-LINES-FROM-EDGES-SPLIT (edges k &optional (m 1))
  (multiple-value-bind (lines strings)
      (split-and-merge (edge-image-strings edges) (* 2dg::*string-line-break-max-err* m)
		       (* 2dg::*string-line-break-max-ave-err* m))
    (convex-lines lines strings k)))

(defun CONVEX-LINES-FROM-EDGES (edges k &optional (thresh 2))
  (let* ((lines (edges-to-line-segments edges thresh))
         (strings (loop for line in lines collecting nil)))
    (convex-lines lines strings k)))

;;; When there is nothing more to add to a cg, we don't test whether a superset of the cg 
;;;   with a better gap ratio was accepted.
(defun CONVEX-BACKTRACK (c-lines matches k)
  (let ((done? nil)
	(current-convex-group (make-initial-convex-group (car c-lines) k))
	(next-base-line (cdr c-lines))
	(convex-groups nil)
	(search-list (list (get-c-line-match-list matches (car c-lines))))
	(min-gap-ratio-list (list k))  ; the min gap ratio a group at the level must have.  
	(excluded-lists (list nil)))		; For eliminate subsets
    (labels ((backtrack ()
	       (if (null current-convex-group)
		   (setf done? t)
		   (progn
		     (let ((removed-line (car (convex-group-lines current-convex-group))))	; For eliminate subsets
		       (when (convex-group-acceptable? current-convex-group k matches)
			 (let* ((finished-copy (copy-to-answer current-convex-group k matches))
			        (finished-gap (convex-group-gap-ratio finished-copy)))
;	       (format t "~%finished-gap = ~A, min-gap-ratio-list = ~A"
;        	       finished-gap min-gap-ratio-list)
			   (when (or (not *FULL-CONSTRAINTS*) (< (car min-gap-ratio-list) finished-gap))
			     ;; Is this cg's gap ratio better than any accepted cg that is a superset of it?
			     ;; If yes, keep it, and update the min-gap-ratio of it's subsets.
			     (push finished-copy convex-groups)
			     (loop for k-list on (cdr min-gap-ratio-list)
				   doing
			       (rplaca k-list (max (car k-list) finished-gap))))))
		       (setf current-convex-group (pop-convex-group! current-convex-group k))
		       (pop search-list)
		       (pop min-gap-ratio-list)
		       (pop excluded-lists)	; For elim sub
		       (loop for xl on excluded-lists doing (push removed-line (car xl)))	; for elim sub
		       ))) )
	     (next ()
	       (if search-list
		   (let ((c-line (pop-c-line-match-list current-convex-group (car search-list))))
		     (if (null c-line) nil
			 (if
			   (or (member c-line (convex-group-lines current-convex-group) :test #'c-lines-same?)
			       (and *FULL-CONSTRAINTS* (member c-line (car excluded-lists))))	; For elim sub
			   (next)
			   c-line)))
		   (let ((new-base-line (pop next-base-line)))
		     ;; This is the case where current group had one line, and nothing more to add to it.
		     (when new-base-line
		       (push (get-c-line-match-list matches new-base-line) search-list)
		       (push k min-gap-ratio-list))			   
		     new-base-line)))
	     (add-line (c-line)
	       ;; The test about adding the line has already added it to the cg structure.
	       (push nil excluded-lists)	; For elim sub
	       (push k min-gap-ratio-list)
	       (push (get-c-line-match-list matches c-line) search-list)))
      (loop until done?
	    doing
	(let ((next-c-line (next)))
	  (if (null next-c-line)
	      (backtrack)
	      (if (null current-convex-group)
		  (setf current-convex-group (make-initial-convex-group next-c-line k))
		  (when (add-c-line-convex-group?! current-convex-group next-c-line matches k)
		    (add-line next-c-line))))))
      convex-groups)))

(defun MAKE-C-LINE-MATCHES (c-lines k &optional c-array d-array l-array a-array m-array)
  "Like the old version, but: it will account for error, it will allow lines to be
   broken if parts of them can be in a convex group, and it will be simpler and slower.
   The output of this routine is a compatibility array, a length array, an angle array, and a match array.  
     The match-array tells us, for a given c-line, all other lines that could be added
       next to a cg containing it, and the distance squared to each of these lines.
   The compat-array, given 2 c-lines should give the distance squared from the end of the
     first line to the start of the second line, after taking into account any reductions
     in either line that might be necessary to maintain convexity,
     (or a nil marker if they can not be mutually convex), 
   The angle array indicates the change in angle when going from one c-line to the next.
   The length array tells us that in going from c-line-1 to c-line-2, we must reduce the
     length of the front of c-line-1 and the rear of c-line-2 by a given amount (or 0).
     This table contains some null value when there is no convexity.
  "
  (let* ((len (length c-lines))
	 (compat-array (if c-array c-array (make-compatibility-array len)))
	 (dist-array (if d-array d-array (make-distance-array len)))
	 (length-array (if l-array l-array (make-length-array len)))
	 ;; entries here are of the form ((d11 d12) (d21 d22))
	 (angle-array (if a-array a-array (make-angle-array len)))
	 (match-array (if m-array m-array (make-array (list len))))
         (match-info (make-match-information compat-array dist-array match-array length-array angle-array)))
    (loop for first? in *FIRST-T-THEN-NIL*
          for c-line-1-ptr = (if first? c-lines (cddr c-line-1-ptr))
	  for index1-offset from 0 by 2 below len
	  doing
      (loop for first? in *FIRST-T-THEN-NIL*
            for c-line-2-ptr = (if first? (cddr c-line-1-ptr) (cddr c-line-2-ptr))
	    for index2-offset from (+ 2 index1-offset) by 2 below len
	    doing
	(let ((convexity-reductions (c-lines-convexity (car c-line-1-ptr) (car c-line-2-ptr) k)))
	  (loop for c-line-1 in c-line-1-ptr
		for index1 from index1-offset to (1+ index1-offset)
		for c-reds in (list (list (first convexity-reductions) (third convexity-reductions))
 			             (list (second convexity-reductions) (fourth convexity-reductions)))
		doing
	    (loop for c-line-2 in c-line-2-ptr
		  for index2 from index2-offset to (1+ index2-offset)
		  for c-red in c-reds
		  ;; c-red = ((c-line-1-front-reduction c-line-2-rear-reduction)
		  ;;           (c-line-2-front-reduction c-line-1-rear-reduction)
		  doing
	      (when (car c-red)
		(add-c-lines-to-tables c-line-1 c-line-2 index1 index2 (car c-red) match-info))
	      (when (cadr c-red)
		(add-c-lines-to-tables c-line-2 c-line-1 index2 index1 (cadr c-red) match-info))
	      (when (caddr c-red)
		(set-c-lines-compatible index1 index2 match-info)
                (set-angle-array (match-info-angle-array match-info) index1 index2 c-line-1 c-line-2 (car c-red))
		(set-angle-array (match-info-angle-array match-info) index2 index1 c-line-2 c-line-1 (cadr c-red)))
	      )))))
	       (loop for i from 0 to (1- len)
		     doing
		 (setf (aref match-array i) (sort (aref match-array i) #'< :key #'car)))
    match-info))

(defun ADD-C-LINES-TO-TABLES (cl1 cl2 index1 index2 c-red match-info)
  (let ((dist (distance-squared-between-convex-c-lines cl1 cl2 c-red))
;; Change above routine
	(length-reductions (cons (* (car c-red) (c-line-length cl1)) (* (cadr c-red) (c-line-length cl2)))))
    (when (reduced-lines-long-enough? cl1 cl2 length-reductions)
      (setf (aref (match-info-distance-array match-info) index1 index2) (round dist))
      (when (< dist *max-squared-gap*)
	(push (cons dist cl2) (aref (match-info-match-array match-info) index1)))
      (set-length-reductions match-info index1 index2 length-reductions))))

(defun REDUCED-LINES-LONG-ENOUGH? (cl1 cl2 c-lengths)
  "c-length = ((length-rear-reduction-cl1 length-front-reduction-cl1) and same for cl2.
   If to make convexity, the length of a line has to be reduced to no more than:
   *CONVEX-POSITIONAL-ERROR* we figure that (a) it's not worth the effort of using it,
   and (b), nearly connected lines can always be reduced to this point.
 
   NOTE: This routine is on hold, because the change made that avoids accounting for
   error except for colinearity may make this unnecessary.
  "
  (ignore-it cl1 cl2 c-lengths)
  t)

(defun IGNORE-IT (&rest x)
  x
  nil)

(defun DISTANCE-SQUARED-BETWEEN-CONVEX-C-LINES (cl1 cl2 c-red)
  "Return the distance squared from the place in cl1 to the place in cl2 where
   a convex connection is made (this will usually be the second point in cl1 and 
   the first point in cl2).
  "
  (distance-squared-between-point-and-point
    (point-along-c-line cl1 (- 1 (first c-red)))
    (point-along-c-line cl2 (second c-red))))

(defun POINT-ALONG-C-LINE (cl frac)
  (cond ((= 0 frac) (c-line-point-1 cl))
	((= 1 frac) (c-line-point-2 cl))
	(t
	 (add (c-line-point-1 cl) (multiply (c-line-vector cl) (* (c-line-length cl) frac))))))

(defun COLINEAR-CONVEXITY (cl1 cl2)
  (multiple-value-bind (colinear? reverse-cl1? reverse-cl2?)
      ;; If COLINEAR? than reverse the c-lines as indicated means colinearity occurs going
      ;; from the first c-line to the second. 
      (c-lines-colinear cl1 cl2)
    (if colinear?
	(cond ((and (not reverse-cl1?) (not reverse-cl2?))
	       ;; Colinearity for (1 2 3 4) (4 3 2 1)
	       (list (list (list 0 0) nil t) nil nil (list nil (list 0 0) t)))
	      ((and reverse-cl1? (not reverse-cl2?))
	       ;; Colinearity for (2 1 3 4) (4 3 1 2)
	       (list nil (list (list 0 0) nil t) (list nil (list 0 0) t) nil))
	      ((and (not reverse-cl1?) reverse-cl2?)
	       ;; Colinearity for (1 2 4 3) (3 4 2 1)
	       (list nil (list nil (list 0 0) t) (list (list 0 0) nil t) nil))
	      (t
	       ;; Colinearity for (2 1 4 3) (3 4 1 2)
	       (list (list nil (list 0 0) t) nil nil (list (list 0 0) nil t))))
	nil)))

(defun C-LINES-COLINEAR (cl1 cl2)
  "If not, return nil, if yes, return (values t reverse-cl1? reverse-cl2?) indicating what
   reversals of the lines are necessary to have colinearity when going from line 1 to 2.
   My test here is whether a vector at the center of each line can be rotated within error
   bounds so that the entire line is first entirely on one side of the vector, then entirely
   on the other side.  In addition, the lines must have similar angles, and must be entirely
   in front of each other (ie not to the side of each other).  This last test rules out
   lines that are parallel, and near each other.
  "
  (let ((total-error (+ (c-line-angle-error cl1) (c-line-angle-error cl2)))
	(a-diff (mod (- (c-line-angle cl1) (c-line-angle cl2)) *short-pi*)))	
    (if (and (or (< a-diff total-error) (< (- *short-pi* a-diff) total-error))
	     (line-colinear-wrt-line cl1 cl2)
	     (line-colinear-wrt-line cl2 cl1))
	(let ((dist1 (dot (c-line-vector cl1) (diff (c-line-point-1 cl2) (c-line-point-1 cl1))))
	      (dist2 (dot (c-line-vector cl1) (diff (c-line-point-2 cl2) (c-line-point-1 cl1))))
	      (len (c-line-length cl1)))
	  ;; len = distance from cl1-point1 to cl1-point2
	  (if (and (< dist1 *CONVEX-POSITIONAL-ERROR*) (< dist2 *CONVEX-POSITIONAL-ERROR*))
	      (values t t (if (< dist1 dist2) t nil))
	      (if (and (< (- len *CONVEX-POSITIONAL-ERROR*) dist1) (< (- len *CONVEX-POSITIONAL-ERROR*) dist2))
		  (values t nil (if (< dist1 dist2) nil t))
		  nil)))
	nil)))

(defun LINE-COLINEAR-WRT-LINE (cl1 cl2)
  (flet ((sign-with-error (dot1 dot2)
	   ;; If the two dot products can both be positive, or both be negative, with allowed error,
	   ;; return -1, 0, or 1, otherwise return nil.  -1 means they can both be neg., 1 means
	   ;; they can both be positive, 0 means they can both be either.
	   (let ((dot1-sign (if (< dot1 (- *CONVEX-POSITIONAL-ERROR*)) -1
				(if (< *CONVEX-POSITIONAL-ERROR* dot1) 1 0)))
		 (dot2-sign (if (< dot2 (- *CONVEX-POSITIONAL-ERROR*)) -1
				(if (< *CONVEX-POSITIONAL-ERROR* dot2) 1 0))))
	     (if (= -1 (* dot1-sign dot2-sign)) nil
		 (if (= 0 dot1-sign)
		     dot2-sign
		     dot1-sign))))
	 (signs-different? (s1 s2)
	   ;; Are values from above routine indicative that line 2 can be on opposite sides of 1?
	   (/= 1 (* s1 s2))))
    (let* ((center1 (c-line-center cl1))
	   (normal-clock-1 (c-line-normal-rotated-clockwise cl1))
	   (normal-counter-1 (c-line-normal-rotated-counterwise cl1))
	   (pt21 (c-line-point-1 cl2))
	   (pt22 (c-line-point-2 cl2))
	   (vec11 (diff pt21 center1))
	   (vec12 (diff pt22 center1))
	   (dot11-clock (dot normal-clock-1 vec11))
	   (dot12-clock (dot normal-clock-1 vec12))
	   (dot11-counter (dot normal-counter-1 vec11))
	   (dot12-counter (dot normal-counter-1 vec12)))
      (let ((same-sign-with-error-1-clock? (sign-with-error dot11-clock dot12-clock)))
	(if (null same-sign-with-error-1-clock?)
	    nil
	    (let ((same-sign-with-error-1-counter? (sign-with-error dot11-counter dot12-counter)))
	      (if (null same-sign-with-error-1-counter?)
		  nil
		  (signs-different? same-sign-with-error-1-clock? same-sign-with-error-1-counter?))))))))

(defun ANGLES-CONVEX? (diff error)
  (< (- diff error) *short-pi*))

(defun MAX-ANGLE-LINE-ERROR (pt1 pt2)
  (min *max-angle-line-error*
       (/ *COLINEAR-LINE-POINT-ERROR*
	  (distance pt1 pt2))))

(defun C-LINES-CONVEXITY (cl1 cl2 k)
  (if *FULL-CONSTRAINTS*
      (c-lines-convexity-full cl1 cl2 k)
      (c-lines-convexity-simple cl1 cl2)))
;  (format t "~%cl1 = ~A, cl2 = ~A~%full   = ~A~%simple = ~A"
;	  cl1 cl2 (c-lines-convexity-full cl1 cl2 k) (c-lines-convexity-simple cl1 cl2)))

(defun C-LINES-CONVEXITY-SIMPLE (cl1 cl2)
      (let* ((norm1 (c-line-normal cl1))
	     (norm2 (c-line-normal cl2))
	     (vec11 (diff (c-line-point-1 cl2) (c-line-point-1 cl1)))
	     (vec12 (diff (c-line-point-2 cl2) (c-line-point-1 cl1)))
	     (vec21 (diff (c-line-point-1 cl2) (c-line-point-2 cl1)))
	     ;; vecij is the vector from point i of line 1 to point j of line 2.
	     (dot11 (dot norm1 vec11))
	     (dot12 (dot norm1 vec12))
	     ;; dotij is the distance from line i to point j of line not-i in the direction normal to line i.
	     (dot21 (- (dot norm2 vec11)))
	     ;; The - is there because vec11 is in the wrong direction for this computation
	     (dot22 (- (dot norm2 vec21))))
	(cond ((and (~<= 0 dot11) (~<= 0 dot12) (~<= 0 dot21) (~<= 0 dot22))
	       (list '((0 0) (0 0) t) '(nil nil nil) '(nil nil nil) '(nil nil nil)))
	      ((and (~>= 0  dot11) (~>= 0  dot12) (~<= 0 dot21) (~<= 0 dot22))
	       (list '(nil nil nil) '((0 0) (0 0) t) '(nil nil nil) '(nil nil nil)))
	      ((and (~<= 0 dot11) (~<= 0 dot12) (~>= 0  dot21) (~>= 0  dot22))
	       (list '(nil nil nil) '(nil nil nil) '((0 0) (0 0) t) '(nil nil nil)))
	      ((and (~>= 0  dot11) (~>= 0  dot12) (~>= 0  dot21) (~>= 0  dot22))
	       (list '(nil nil nil) '(nil nil nil) '(nil nil nil) '((0 0) (0 0) t)))
	      (t (list '(nil nil nil) '(nil nil nil) '(nil nil nil) '(nil nil nil))))))

(defun C-LINES-CONVEXITY-FULL (cl1 cl2 k)
  "This function takes two c-lines as arguments.  It returns four results:
     (cl1 cl2) (reversed-cl1 cl2) (cl1 reversed-cl2) (reversed-cl1 reversed-cl2)
   For each of these cases, we will return
   ((d12 d21) (d22 d11) t/nil).  dij Means that for c-line i, the 
   front (j = 1) or back (j = 2) should be reduced in length by dij, in order to
   accomodate convexity, and provide the best addition to the gap ratio of the group.  
   If the lines are just convex, these lengths will be 0.
   (d12 d21) for example, can be NIL if the lines are not mutually convex, or if they
   are convex, but adding the second line would only hurt the gap ratio of the first.
   The third value in the list is T or NIL, indicating whether convexity is possible
   at all between the two lines.  This is needed because we also want to be able to check
   if the CL1 can be added to a group when CL2 is the group's first line.

  We want to reduce the length of a line not just when it's necessary for convexity,
  but when doing so will improve the gap ratio.

  NEW NOTES: My new strategy is to not consider angular error when finding convexity
  between parts of lines.  This will greatly simplify the routines below.  I do still
  want to find possible colinearity, allowing for angular error.

  COLINEARITY: Suppose we have four points, colinear, in line from left to right, 1,2,3,4.
  One line has points 1 & 2, the other has 3 & 4.  A convex group can traverse the lines
  in the order: (1,2,3,4) or (4,3,2,1).  Other convex interpretations are possible, but they
  will never be as good, since they will all involve angle changes of pi or 2pi while covering
  the same ground, and with longer gaps.
  This is the only case where (d12 d21) might have a value, while (d22 d11) is NIL.
  "
  (or (colinear-convexity cl1 cl2)
      (let* ((norm1 (c-line-normal cl1))
	     (norm2 (c-line-normal cl2))
	     (vec11 (diff (c-line-point-1 cl2) (c-line-point-1 cl1)))
	     (vec12 (diff (c-line-point-2 cl2) (c-line-point-1 cl1)))
	     (vec21 (diff (c-line-point-1 cl2) (c-line-point-2 cl1)))
	     ;; vecij is the vector from point i of line 1 to point j of line 2.
	     (dot11 (dot norm1 vec11))
	     (dot12 (dot norm1 vec12))
	     ;; dotij is the distance from line i to point j of line not-i in the direction normal to line i.
	     (dot21 (- (dot norm2 vec11)))
	     ;; The - is there because vec11 is in the wrong direction for this computation
	     (dot22 (- (dot norm2 vec21))))
	(list (bidirectional-convexity-from-dot-products dot11 dot12 dot21 dot22 cl1 cl2 k nil nil)
	      (bidirectional-convexity-from-dot-products dot11 dot12 dot21 dot22 cl1 cl2 k t nil)
	      (bidirectional-convexity-from-dot-products dot11 dot12 dot21 dot22 cl1 cl2 k nil t)
	      (bidirectional-convexity-from-dot-products dot11 dot12 dot21 dot22 cl1 cl2 k t t)))))

(defun BIDIRECTIONAL-CONVEXITY-FROM-DOT-PRODUCTS (dot11 dot12 dot21 dot22 cl1 cl2 k rev-cl1? rev-cl2?)
  (let ((d11 (* (if rev-cl2? dot12 dot11) (if rev-cl1? -1 1)))
	(d12 (* (if rev-cl2? dot11 dot12) (if rev-cl1? -1 1)))
	(d21 (* (if rev-cl1? dot22 dot21) (if rev-cl2? -1 1)))
	(d22 (* (if rev-cl1? dot21 dot22) (if rev-cl2? -1 1))))
    (if (or (and (< d11 g:*epsilon*) (< d12 g:*epsilon*))
	    (and (< d21 g:*epsilon*) (< d22 g:*epsilon*)))
	(list nil nil nil)
	(let ((cl1-pt1 (if rev-cl1? (c-line-point-2 cl1) (c-line-point-1 cl1)))
	      (cl1-pt2 (if rev-cl1? (c-line-point-1 cl1) (c-line-point-2 cl1)))
	      (cl2-pt1 (if rev-cl2? (c-line-point-2 cl2) (c-line-point-1 cl2)))
	      (cl2-pt2 (if rev-cl2? (c-line-point-1 cl2) (c-line-point-2 cl2)))
	      (cl1-vec (if rev-cl1? (c-line-reverse-vector cl1) (c-line-vector cl1)))
	      (cl2-vec (if rev-cl2? (c-line-reverse-vector cl2) (c-line-vector cl2)))
	      (cl1-rvec (if rev-cl1? (c-line-vector cl1) (c-line-reverse-vector cl1)))
	      (cl2-rvec (if rev-cl2? (c-line-vector cl2) (c-line-reverse-vector cl2))))
	  (list (convexity-from-dot-products d11 d12 d21 d22 cl1-pt2 cl2-pt1 cl1-rvec cl2-vec k (c-line-length cl1)
					     (c-line-length cl2))
		(convexity-from-dot-products d21 d22 d11 d12 cl2-pt2 cl1-pt1 cl2-rvec cl1-vec k (c-line-length cl2)
					     (c-line-length cl1))
		t)))))

(defun CONVEXITY-FROM-DOT-PRODUCTS (dot11 dot12 dot21 dot22 cl1-p2 cl2-p1 cl1-vec cl2-vec k cl1-length cl2-length)
  ;; Note: cl1-vec is reversed, ie each vec is the vector from the connecting
  ;; point to the other.
  ;; We look to see how much, if any, of the end of the first line and the start of the second line
  ;; should be cut off to (1) maintain the convexity of the two lines, and (2) improve their gap
  ;; ratio.  If the two lines are convex, then if cutting any part of both lines improves the gap
  ;; ratio, we know the gap ratio is better with just the first line.  So if one of the lines must
  ;; be truncated to make them convex, we must do that first.
  (flet ((frac-cutoff (dot1 dot2) (if (and (minusp dot1) (plusp dot2)) (/ (abs dot1) (- dot2 dot1))
				      (if (and (minusp dot1) (minusp dot2)) 1 0)))
	 (dist-cutoff (dist cl-len) (if (< dist 0) 0 (/ dist cl-len))))
    (let ((convexity-cutoff-2 (frac-cutoff dot11 dot12))
	  (convexity-cutoff-1 (frac-cutoff dot22 dot21))
	  ;; convexity-cutoff-i is the fraction of line i that must be removed to make the lines convex.
	  )
      (when (not (zerop convexity-cutoff-1))
	(setq cl1-p2 (add cl1-p2 (multiply cl1-vec (* cl1-length convexity-cutoff-1)))
	      cl1-length (* (- 1 convexity-cutoff-1) cl1-length)))
      (when (not (zerop convexity-cutoff-2))
	(setq cl2-p1 (add cl2-p1 (multiply cl2-vec (* cl2-length convexity-cutoff-2)))
	      cl2-length (* (- 1 convexity-cutoff-2) cl2-length)))
      (let ((dist2 (line-distance-maximizing-gap-ratio cl1-p2 cl2-p1 cl2-vec k))
	    ;; This is the distance along cl2 from its first point to the point where the two lines
	    ;; should be connected to maximize the gap ratio.
	    )
	;; The gap ratio is worst at -infinity and +infinity, and improves from each of them to dist2.
	(if (< cl2-length dist2)
	    nil
	    ;; this means that the gap ratio improves continuously as we use less of cl2.
	    (let ((dist1 (line-distance-maximizing-gap-ratio cl2-p1 cl1-p2 cl1-vec k)))
	      (if (< cl1-length dist1)
		  nil
		  (if (no-connection-better? dist1 dist2 cl1-p2 cl2-p1 cl1-vec cl2-vec k)
		      nil
		      (list (round-near-0 (+ convexity-cutoff-1 (* (- 1 convexity-cutoff-1) (dist-cutoff dist1 cl1-length))))
			    (round-near-0 (+ convexity-cutoff-2
					     (* (- 1 convexity-cutoff-2) (dist-cutoff dist2 cl2-length)))))))))))))

(defun ROUND-NEAR-0 (x)
  (if (~zerop x) 0 x))

(defun NO-CONNECTION-BETTER? (dist1 dist2 cl1-p2 cl2-p1 cl1-vec cl2-vec k)
  (if (and (plusp dist1) (plusp dist2))
      t
      (if (or (plusp dist1) (plusp dist2))
	  (let ((first-point (if (plusp dist1) (add cl1-p2 (multiply cl1-vec dist1)) cl1-p2))
		(second-point (if (plusp dist2) (add cl2-p1 (multiply cl2-vec dist2)) cl2-p1)))
	    (if (plusp dist1)
		(if (plusp (line-distance-maximizing-gap-ratio first-point second-point cl2-vec k))
		    t
		    nil)
		(if (plusp (line-distance-maximizing-gap-ratio second-point first-point cl1-vec k))
		    t
		    nil)))
	  nil)))

(defun LINE-DISTANCE-MAXIMIZING-GAP-RATIO (p1 p2 v k)
    (let ((current-epsilon g:*epsilon*))
  (unwind-protect
    (progn
      (setq g:*epsilon* .01)
    (let* ((k- (/ (- 1 k) k))
	   (x1 (point-x p1))
	   (y1 (point-y p1))
	   (x2 (point-x p2))
	   (y2 (point-y p2))
	   (vx (point-x v))
	   (vy (point-y v))
	   (c1 (- (g:sq vx) (g:sq k-)))
	   (c2 (- (g:sq vy) (g:sq k-)))
	   (c3 (* 2 vx vy))
	   (x3 (- x1 x2))
	   (y3 (- y1 y2))
	   (a (+ (* c1 (g:sq vx))
		 (* c2 (g:sq vy))
		 (* c3 vx vy)))
	   (b (- (+ (* 2 c1 x3 vx)
		    (* 2 c2 y3 vy)
		    (* c3 x3 vy)
		    (* c3 y3 vx))))
	   (c (+ (* c1 (g:sq x3))
		 (* c2 (g:sq y3))
		 (* c3 x3 y3)))
	   (-b (- b))
	   (bb (g:sq b))
	   (4ac (* 4 a c))
	   (sqrt-bb-4ac (if (g:~= bb 4ac) 0 (sqrt (- bb 4ac))))
	   (2a (* 2 a))
	   (d1 (/ (- -b sqrt-bb-4ac) 2a))
	   (d2 (/ (+ -b sqrt-bb-4ac) 2a))
	   (valid-answers
	     (loop for d in (list d1 d2)
		   for exp1 = (* vx (- x3 (* d vx)))
		   for exp2 = (* vy (- y3 (* d vy)))
		   when (or (g:~<= 0 (+ exp1 exp2))
			    (g:~= exp1 (- exp2)))
		     ;; (g:~<= 0 (+ (* vx (- x3 (* d vx))) (* vy (- y3 (* d vy)))))
		     collect d)))
      (if (or (null (cdr valid-answers)) (reduce #'g:~= valid-answers))
	  (if (g:~zerop (car valid-answers)) 0 (car valid-answers))
	  (error "Found two answers for point maximizing gap ratio"))))
    (setq g:*epsilon* current-epsilon))))
      

(defun MAKE-C-LINES (line-segments strings)
  (loop for ls in line-segments
	for string in strings
	for cl = (make-c-line ls)
	for rev-cl = (make-c-line (make-line-segment (line-segment-point-2 ls) (line-segment-point-1 ls)))
	for index from 0 by 2
	for angle-error = (min *max-angle-line-error* (line-segment-angle-error ls *convex-positional-error*))
	for vec = (unit (diff (line-segment-point-2 ls) (line-segment-point-1 ls)))
	for angle = (atan (point-y vec) (point-x vec))
	  collect cl into cls
	  collect rev-cl into cls
	do
	  (setf (c-line-vector cl) vec)
	  (setf (c-line-reverse-vector cl) (multiply vec -1))
	  (setf (c-line-normal cl) (normal vec))
	  (setf (c-line-length cl) (distance (line-segment-point-1 ls) (line-segment-point-2 ls)))
	  (setf (c-line-index cl) index)
	  (setf (c-line-underlying-points cl) string)
	  (setf (c-line-center cl) (average-points (list (line-segment-point-1 ls) (line-segment-point-2 ls))))
	  (setf (c-line-normal-rotated-clockwise cl) (rotate-point (c-line-normal cl) (- *short-2pi* angle-error)))
	  (setf (c-line-normal-rotated-counterwise cl) (rotate-point (c-line-normal cl) angle-error))
	  (setf (c-line-angle cl) angle)
	  (setf (c-line-angle-error cl) angle-error)

	  (setf (c-line-vector rev-cl) (c-line-reverse-vector cl))
	  (setf (c-line-reverse-vector rev-cl) (c-line-vector cl))
	  (setf (c-line-normal rev-cl) (multiply (c-line-normal cl) -1))
	  (setf (c-line-length rev-cl) (c-line-length cl))
	  (setf (c-line-index rev-cl) (1+ index))
	  (setf (c-line-underlying-points rev-cl) string)
	  (setf (c-line-center rev-cl) (c-line-center cl))
	  (setf (c-line-normal-rotated-clockwise rev-cl) (multiply (c-line-normal-rotated-clockwise cl) -1))
	  (setf (c-line-normal-rotated-counterwise rev-cl) (multiply (c-line-normal-rotated-counterwise cl) -1))
	  (setf (c-line-angle rev-cl) (let ((rev-ang (+ angle *short-pi*)))
					(if (<= rev-ang *short-pi*)
					    rev-ang
					    (- rev-ang *short-2pi*))))
	  (setf (c-line-angle-error rev-cl) angle-error)
	  finally (return cls)))

(defun C-LINES-EQUAL (cl1 cl2)
  (let ((diff (- (c-line-index cl1) (c-line-index cl2))))
    (and (< (abs diff) 2)
	 (or (= 0 diff)
	     (if (< 0 diff)
		 (oddp (c-line-index cl1))
		 (evenp (c-line-index cl1)))))))

(defun ADD-C-LINE-CONVEX-GROUP?! (convex-group c-line matches k)
  "Combine functions of add-line-to-convex? and add-c-line-convex-group!.
   If c-line should be added, add it.
   I have a new theory: if the total of the angles turned in the polygon is not more than 360 degrees,
     and no angle is less than 0 than the polygon is convex.  
  "
  (let ((first-line (convex-group-first-line convex-group))
	(last-line (convex-group-last-line convex-group)))
    (when (and (not (convex-group-closed convex-group))
	       (c-lines-compatible c-line first-line matches))
      ;; Is the c-line convex to the first line in the convex group?  We don't care about the distance.
      (let ((angle-1 (c-lines-turning-angle last-line c-line matches))
	    (angle-2 (c-lines-turning-angle c-line first-line matches)))
	(when (angles-acceptable? (convex-group-total-angles convex-group) angle-1 angle-2)
	  ;; Ok, at this point, we know that the lines are all convex.			   
	  (let ((dist (sqrt (c-lines-distance-squared last-line c-line matches))))
	    ;; Additional gap in convex-group
	    (destructuring-bind (last-front-reduction c-line-rear-reduction)
		(lengths-reduced last-line c-line matches)
		(when (line-length-worth-adding? convex-group dist last-front-reduction k c-line matches)
		  (add-c-line-convex-group! convex-group c-line k last-front-reduction c-line-rear-reduction
					    dist angle-1)
		  t))))))))

(defun LINE-LENGTH-WORTH-ADDING? (cg dist last-front-reduction k c-line matches)
  (if *FULL-CONSTRAINTS* (line-length-worth-adding?-full cg dist last-front-reduction k c-line matches)
      t))

(defun LINE-LENGTH-WORTH-ADDING?-FULL (cg dist last-front-reduction k c-line matches)
  "We know that the distance to the new line is less than the maximum allowable gap.
   We need to check three things:
      First, if the last line in the cg has its length reduced by the new c-line,
        this reduces the allowable gap, so we must make sure the added line is still ok.
      Second, we must check whether the cg with the new c-line wouldn't be better
        if we excluded the last line in the original cg.  If so, we want to avoid
        adding this c-line now, and add it later, after we backtrack.
      Third, the last c-line in the cg isn't wiped out by a reduction in its front from
        the new line.
   We are not taking into account how the new line might effect the first line in the cg.
   Our rule of thumb is to ignore that effect until the cg is done.
  "
;  (when (= 4 (length (convex-group-lines cg))) (break))
  (and (or (zerop last-front-reduction)
	   (and 
	     (new-gap-allowable? (- (convex-group-length cg) last-front-reduction)
				 (convex-group-gap-length cg)
				 dist
				 k)
	     ;; First test
	     (< last-front-reduction (convex-group-last-line-effective-length cg)))
           ;; Third test
	   )
       (or (null (cdr (convex-group-lines cg)))
	   ;; If cg has only one group, don't check second condition.
	   (let ((new-line-reduces-second-last-line
		   (car (lengths-reduced (convex-group-second-last-line cg) c-line matches))))
	     ;; If this value is nil, it means the new line is incompatible with the second to last line.
	     ;; This is possible because allowing for error might make the last line compatible with both,
	     ;; while they are not mutually compatible.
	     ;; Or, adding the new line to the second to last directly might produce a worse gap ratio,
	     ;; while we get a better gap ratio when the last line is present.
	     ;; Either way, we want to add the new line.
	     (or (null new-line-reduces-second-last-line)
		  (line-improves-gap-ratio
		    (- (convex-group-last-line-effective-length cg) last-front-reduction)
		    ;; length the last line contributes 
		    (convex-group-last-gap cg)
		    dist
		    (sqrt (c-lines-distance-squared (convex-group-second-last-line cg) c-line matches))
		    (convex-group-second-last-front-reduced cg)
		    ;; length new c-line would take off the cg without this line
		    new-line-reduces-second-last-line
		    ;;  length this line takes off the rest of the cg.
		    k))))))

(defun LINE-IMPROVES-GAP-RATIO (effective-line-length line-gap-1 line-gap-2 gap-without-line
				group-reduction-with-line group-reduction-without-line k)
  (if *FULL-CONSTRAINTS* (line-improves-gap-ratio-full effective-line-length line-gap-1 line-gap-2 gap-without-line
						       group-reduction-with-line group-reduction-without-line k)
      t))

(defun LINE-IMPROVES-GAP-RATIO-FULL (effective-line-length line-gap-1 line-gap-2 gap-without-line
				group-reduction-with-line group-reduction-without-line k)
  "We want to test (>= (increment in line length from line)
                      (* (/ k (- 1 k)) increment in gap from line.
  "
  (>= (+ effective-line-length group-reduction-without-line (- group-reduction-with-line))
     (* (/ k (- 1 k))
	(- (+ line-gap-1 line-gap-2 gap-without-line)))))

(defun ANGLES-ACCEPTABLE? (group-angle-total angle-to-new-line angle-from-new-line)
  (< (+ group-angle-total angle-to-new-line angle-from-new-line)
     *MAX-ANGLES-CONVEX-GROUP*))


;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Convex Group Stuff
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun UPDATE-CONVEX-GROUP-MAX-GAP! (cg k)
; (format t "~%convex-group-length = ~A" (convex-group-length cg))
  (setf (convex-group-max-gap-squared cg)
	(square (max 0 (- (/ (* (convex-group-length cg) (- 1 k)) k) (convex-group-gap-length cg))))))
  ;; (/ len (+ len total-gap)) >= k

(defun CONVEX-GROUP-LAST-NORMAL (cg)
  (c-line-normal (convex-group-last-line cg)))

(defun NEW-GAP-ALLOWABLE? (cg-length cg-gap new-gap k)
  (>= (* (- 1 k) cg-length) (* k (+ cg-gap new-gap))))

(defun CONVEX-GROUP-ACCEPTABLE? (cg k matches)
  "Could we tie off this group now?  We know it's convex, or we wouldn't have gotten this far, so
   we must just check that the remaining gap isn't too big.
   This means also taking acount of any shortening of the first or last line by the other one.
   Also, we check whether we'd get a better gap ratio by dropping the last line.
  "
  (if (null (cdr (convex-group-lines cg)))
      (<= k .5)
      (let ((reds (lengths-reduced (convex-group-last-line cg) (convex-group-first-line cg) matches)))
	(if (null reds)
	    nil
	    ;; This means that the cg's gap ratio would be better without its first line.
	    (let ((last-front-reduction (car reds))
		  (first-rear-reduction (cadr reds))
		  (new-gap-squared (c-lines-distance-squared (convex-group-last-line cg)
							     (convex-group-first-line cg)
							     matches)))
	      (and (if (and (zerop last-front-reduction) (zerop first-rear-reduction))
		       (<= new-gap-squared (convex-group-max-gap-squared cg))
		       (and (< last-front-reduction (convex-group-last-line-effective-length cg))
			    (< (+ first-rear-reduction (cadr (car (last (convex-group-length-reductions cg)))))
			       (c-line-length (convex-group-first-line cg)))
			    ;; Check that the reductions from considering the first and last lines won't wipe
			    ;;   out either of them.
			    (new-gap-allowable? (- (convex-group-length cg) last-front-reduction first-rear-reduction)
						(convex-group-gap-length cg) (sqrt new-gap-squared) k)))
		   (if (null (cddr (convex-group-lines cg)))
		       ;; If there are only two lines in the group, we don't check if it would be better with one line.
		       t
		       (line-improves-gap-ratio
			 (- (convex-group-last-line-effective-length cg) last-front-reduction)
			 (convex-group-last-gap cg)
			 (sqrt new-gap-squared)
			 (distance-between-point-and-point (c-line-point-2 (convex-group-second-last-line cg))
							   (convex-group-first-point cg))
			 (+ first-rear-reduction (convex-group-second-last-front-reduced cg))
			 (reduce #'+ (lengths-reduced (convex-group-second-last-line cg) (convex-group-first-line cg) matches))
			 k))))))))

(defun COPY-TO-ANSWER (cg k matches)
  "Given a convex group, it returns a structure that we want to save"
  (let ((new-cg (copy-cg cg)))
    (destructuring-bind (last-front-reduction first-rear-reduction)
	(if (null (cdr (convex-group-lines new-cg)))
	    (list 0 0)
	    (lengths-reduced (convex-group-last-line new-cg) (convex-group-first-line new-cg) matches))
      (let ((new-gap (sqrt (c-lines-distance-squared (convex-group-last-line new-cg) (convex-group-first-line new-cg)
						     matches))))
	(when (or (plusp last-front-reduction) (plusp first-rear-reduction))
	  (decf (convex-group-length new-cg) (+ last-front-reduction first-rear-reduction))
	  (decf (convex-group-last-line-effective-length new-cg) last-front-reduction)
	  (rplaca (cdr (first (convex-group-length-reductions new-cg))) last-front-reduction)
	  (rplaca (car (last (convex-group-length-reductions new-cg))) first-rear-reduction)
	  )
	(incf (convex-group-gap-length new-cg) new-gap)
	(push new-gap (convex-group-gaps new-cg))))					
    (update-convex-group-max-gap! new-cg k)
    new-cg))

(defun COPY-CG (cg)
  (let ((cg (copy-convex-group cg)))
    (setf (convex-group-lines cg) (copy-list (convex-group-lines cg)))
    (setf (convex-group-gaps cg) (copy-list (convex-group-gaps cg)))
    (setf (convex-group-length-reductions cg) (copy-tree (convex-group-length-reductions cg)))
    (setf (convex-group-angles cg) (copy-list (convex-group-angles cg)))
    cg))

(defun MAKE-INITIAL-CONVEX-GROUP (c-line k)
  (let ((cg (make-convex-group-internal :last-line c-line
					:last-point (c-line-point-2 c-line)
					:lines (list c-line)
					:length (c-line-length c-line)
					:gap-length 0
					:total-angles 0
					:first-line c-line
					:first-point (c-line-point-1 c-line)
					:closed nil
					:gaps nil
					:last-line-effective-length (c-line-length c-line)
					:length-reductions (list (list 0 0))
					)))
    (update-convex-group-max-gap! cg k)
    cg))

(defun MAKE-CONVEX-GROUP (c-lines k)
  (let ((cg (make-convex-group-internal)))
    (setf (convex-group-first-point cg) (c-line-point-1 (car c-lines)))
    (setf (convex-group-first-line cg) (car c-lines))
    (loop for first? in *FIRST-T-THEN-NIL*
          for prev-point = (if first? nil (c-line-point-2 cl))
          for cl in c-lines
	  for cl-upoints = (c-line-underlying-points cl)
	  for next-point = (if first? nil (c-line-point-1 cl))
	  for gap = (if first? 0 (distance prev-point next-point))
	  sum (c-line-length cl) into len
	  sum gap into gap-total
	  append cl-upoints into upoints
	  doing
	  (push cl (convex-group-lines cg))
	  (push gap (convex-group-gaps cg))
	  finally (progn (setf (convex-group-length cg) len)
			 (setf (convex-group-gap-length cg) gap-total)
			 (setf (convex-group-last-line cg) cl)
			 (setf (convex-group-last-point cg) (c-line-point-2 cl))
			 (setf (trait-underlying-points cg) upoints)
			 (update-convex-group-max-gap! cg k) ))
    cg))

(defun MOCK-CONVEX-GROUP-FROM-LINE-SEGMENTS (lss)
  (let ((cg (make-convex-group-internal)))
    (loop for ls in (reverse lss)
	  for cl = (make-c-line ls)
	  doing
      (push cl (convex-group-lines cg)))
    cg))

(defun MAKE-SIMPLE-CONVEX-GROUP (c-lines)
  (let ((cg (make-convex-group-internal)))
    (setf (convex-group-lines cg) c-lines)
    cg))

(defun CONVEX-GROUP-SECOND-LAST-LINE (cg)
  (cadr (convex-group-lines cg)))

(defun CONVEX-GROUP-SECOND-LAST-FRONT-REDUCED (cg)
  (car (second (convex-group-length-reductions cg))))

(defun CONVEX-GROUP-LAST-GAP (cg)
  (car (convex-group-gaps cg)))

(defun ADD-C-LINE-CONVEX-GROUP! (cg cl k last-front-reduction c-line-rear-reduction dist angle)
; (format t "~%Adding, cg = ~A" cg)
    (setf (convex-group-last-line-effective-length cg)
	  (- (c-line-length cl) c-line-rear-reduction))
    (incf (convex-group-length cg) (- (convex-group-last-line-effective-length cg)
					  last-front-reduction))
    (rplaca (cdr (first (convex-group-length-reductions cg)))
	    last-front-reduction)
    (push (list c-line-rear-reduction 0) (convex-group-length-reductions cg))
    (incf (convex-group-gap-length cg) dist)
    (push cl (convex-group-lines cg))
    (push dist (convex-group-gaps cg))
    (setf (convex-group-last-line cg) cl)
    (setf (convex-group-last-point cg) (c-line-point-2 cl))
    (incf (convex-group-total-angles cg) angle)
    (push angle (convex-group-angles cg))
;    (when (< (distance-squared-between-point-and-point (convex-group-first-point cg) last-point)
;	     *squared-ignorable-distance*)
;      (setf (convex-group-closed cg) t))    
    (update-convex-group-max-gap! cg k)
; (format t "~%cg = ~A" cg)
 )

(defun POP-CONVEX-GROUP! (cg k)
  "Remove the most recently added c-line from the group.  If this would leave none, return nil"
; (format t "~%Popping, cg = ~A" cg)
  (let ((cl (pop (convex-group-lines cg)))
	(last-gap (pop (convex-group-gaps cg)))
	(last-reductions (pop (convex-group-length-reductions cg))))
    (if (null (convex-group-lines cg))
	nil
	(progn (setf (convex-group-last-line cg) (car (convex-group-lines cg)))
	       (setf (convex-group-last-point cg) (c-line-point-2 (convex-group-last-line cg)))
	       (decf (convex-group-length cg) (- (c-line-length cl) (car last-reductions)))
	       ;; the CADR of last-reductions should always be 0.
	       (rplaca (cdr (first (convex-group-length-reductions cg))) 0)
	       (setf (convex-group-last-line-effective-length cg)
		     (- (c-line-length (convex-group-last-line cg))
			(car (first (convex-group-length-reductions cg)))))
	       (decf (convex-group-gap-length cg) last-gap)
	       (decf (convex-group-total-angles cg) (pop (convex-group-angles cg)))
	       (setf (convex-group-closed cg) nil)
	       (update-convex-group-max-gap! cg k)
; (format t "~%cg = ~A" cg)
	       cg))))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; MATCH INFORMATION ABSTRACTION
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun SET-LENGTH-REDUCTIONS (m i1 i2 reds)
  "reds = cons of 2 floats indicating length reductions."
  (setf (aref (match-info-length-array m) i1 i2) reds))

(defun MATCH-INFO-DISTANCE-ARRAY (mi) (second mi))
(defun MATCH-INFO-MATCH-ARRAY (mi) (third mi))
(defun MATCH-INFO-LENGTH-ARRAY (mi) (fourth mi))
(defun MATCH-INFO-ANGLE-ARRAY (mi) (fifth mi))

(defun SET-C-LINES-COMPATIBLE (index1 index2 match-info)
;  (setf (aref (car match-info) index1 index2) t)
;  (setf (aref (car match-info) index2 index1) t))
  (setf (aref (car match-info) index1 index2) 1)
  (setf (aref (car match-info) index2 index1) 1))

(defun LENGTHS-REDUCED (cl1 cl2 matches)
  (let ((lens (aref (fourth matches) (c-line-index cl1) (c-line-index cl2))))
    (if lens (list (car lens) (cdr lens))
	nil)))

(defun GET-C-LINE-MATCH-LIST (matches c-line)
  (list (aref (third matches) (c-line-index c-line))))

(defun POP-C-LINE-MATCH-LIST (convex-group match-list)
  (let ((match (pop (car match-list))))
    (if (null match) nil
	(if (< (car match) (convex-group-max-gap-squared convex-group))
	    (cdr match)
	    nil))))

(defun C-LINES-COMPATIBLE (cl1 cl2 matches)
  "Returns the squared distance if they are compatible"
;  (aref (car matches) (c-line-index cl1) (c-line-index cl2)))
  (= 1 (aref (car matches) (c-line-index cl1) (c-line-index cl2))))

(defun C-LINES-DISTANCE-SQUARED (cl1 cl2 matches)
  (aref (second matches) (c-line-index cl1) (c-line-index cl2)))

(defun SET-ANGLE-ARRAY (angle-array index1 index2 cl1 cl2 c-red) 
  (setf (aref angle-array index1 index2)
	(let ((a-diff (mod (- (c-line-angle cl1) (c-line-angle cl2)) *short-2pi*))
	      ;; Going clockwise, the angle decreases, then jumps from -pi to pi.
	      (a-error (+ (c-line-angle-error cl1) (c-line-angle-error cl2))))
	  ;; There's some potential ambiguity about whether an angle difference of nearly
	  ;; *short-2pi* reflects that big an angle change, or a small negative angle change which
	  ;; is allowed by assuming some error.  To differentiate between these, basically if
	  ;; the second line is in front of the first, it must be a small negative change,
	  ;; otherwise a large change.
	  (if (< a-diff (- *short-2pi* a-error))
	      a-diff
	      (if (null c-red)
		  ;; if the lines aren't really compatible, the second isn't in front of the first and colinear.
		  a-diff
		  (if (< (dot (c-line-vector cl1)
			      (diff (point-along-c-line cl2 (* (cadr c-red) (c-line-length cl2)))
				    (point-along-c-line cl1 (- 1 (* (car c-red) (c-line-length cl1))))))
			 (- *CONVEX-POSITIONAL-ERROR*))
		      ;; If the real end of cl1 is more than error bounds in front of the real start of cl2:
		      a-diff
		      (- a-diff *short-2pi*)))))))

(defun C-LINES-TURNING-ANGLE (cl1 cl2 matches)
  (aref (fifth matches) (c-line-index cl1) (c-line-index cl2)))

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;; Post-process answers
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(defun REMOVE-DUPLICATE-ANSWERS (answers &optional (ignorable-length 0))
  (let ((c-lines&reds (loop for a in answers
			    collecting
			      (loop for cl in (convex-group-lines a)
				    for red in (convex-group-length-reductions a)
				    for long-enough? = (< ignorable-length (- (c-line-length cl) (reduce #'+ red)))
				    when long-enough? collect cl into cls
				    when long-enough? collect red into reds
				    finally (return (list cls reds))))))
    (loop for answer in answers
	  for cls&reds in c-lines&reds
	  for i from 0
	  appending
	    (when (loop for cls&reds2 in c-lines&reds
			for j from 0
			always
			  (if (< i j)
			      (not (line-proper-subset (car cls&reds) (cadr cls&reds) (car cls&reds2) (cadr cls&reds2)))
			      (if (< j i)
				  (not (line-subset (car cls&reds) (cadr cls&reds) (car cls&reds2) (cadr cls&reds2)))
				  t)))
	      (list answer)))))

(defun COMBINE-OVERLAPPING-ANSWERS (answers k &optional (thresh .75))
  "When 2 groups have a common sequence of lines accounting for a given percentage of their total length,
   just keep this common sequence, throwing out the groups."
;  (let ((answers (loop for a in answers collecting (list (convex-group-length a) a))))
    ;; Each answer is (len convex-group) so that when we build a sub-answer we may retain the longest 
  (loop until (null answers)
	with res = nil
	for a1 = (pop answers)   ; Note that this can't be for a1 in answers, since things get pushed on answers.
	doing
    (loop for a2 in answers
	  for overlap-ans = (convex-group-overlapping-sequence a1 a2 k thresh)
	  doing
      (when overlap-ans
	(setf answers (remove a2 answers))
	(push overlap-ans answers)
	(return))
          finally (push a1 res))
    finally (return res)))

(defun CONVEX-GROUP-OVERLAPPING-SEQUENCE (cg1 cg2 k overlap-thresh)
  (let* ((cls1 (convex-group-lines cg1))
	(cls2 (convex-group-lines cg2))
	(min-seq-length (* (max (apply #'+ (mapcar #'c-line-length cls1))
				(apply #'+ (mapcar #'c-line-length cls2)))
			   overlap-thresh))
	(seqs (eq-sequences-circular cls1 cls2)))
      (loop for seq in seqs
	    doing
	(when (< min-seq-length (apply #'+ (mapcar #'c-line-length seq)))
;(format t "Sequence long enough")
	  (let ((cg (make-convex-group (reverse seq) k)))
;(format t "~%Dist = ~A, max-gap = ~A, group = ~A"
;	(distance-squared-between-point-and-point (convex-group-first-point cg) (convex-group-last-point cg))
;	(convex-group-max-gap-squared cg) cg)
	    (when (< (distance-squared-between-point-and-point (convex-group-first-point cg) (convex-group-last-point cg))
		     (convex-group-max-gap-squared cg))
;(format t "Gap ok")
	      (return cg)))))))
	
(defun EQ-SEQUENCES-CIRCULAR (lst1 lst2)
  (labels ((eq-sequence-2nd-circular (lst1 lst2)
	     ;; Find sequence beginning at start of lst1
	     (let ((mem (member (car lst1) lst2)))
	       (when mem
		 (loop for i1 in lst1
		       for i2 in (append mem lst2)
		       until (not (eq i1 i2))
		       collect i1 into res
		       finally (return (values res (- (length lst2) (length mem))))))))
	   (eq-sequences-2nd-circular (lst1 lst2)
	     ;; Find all sequences in first lst that don't wrap around from end to beginning
	     (if (null lst1) nil
		 (let ((seq (eq-sequence-2nd-circular lst1 lst2)))
		   (if (null seq)
		       (eq-sequences-2nd-circular (cdr lst1) lst2)
		       (cons seq (eq-sequences-2nd-circular (nthcdr (length seq) lst1) lst2)))))))
    (multiple-value-bind (seq seq-start)
	(eq-sequence-2nd-circular lst1 lst2)
      (if (null seq)
	  (eq-sequences-2nd-circular (cdr lst1) lst2)
	  (multiple-value-bind (seq-front seq-front-start)
	      (eq-sequence-2nd-circular (reverse lst1) (reverse lst2))
	    (if (or (null seq-front)
		    (/= seq-front-start (mod (1- (- (1- (length lst2)) seq-start)) (length lst2))))
		(cons seq (eq-sequences-2nd-circular (nthcdr (length seq) lst1) lst2))))))))

(defun PROPER-SUBSET-EQ (set1 set2)
  (and (loop for e1 in set1 always (member e1 set2))
       (< (length set1) (length set2))))

(defun SUBSET-EQ (set1 set2)
  (loop for e1 in set1 always (member e1 set2)))

(defun LINE-PROPER-SUBSET (lines1 reductions1 lines2 reductions2)
  (and (< (length lines1) (length lines2))
       (line-subset lines1  reductions1 lines2 reductions2)))

(defun LINE-SUBSET (lines1 reductions1 lines2 reductions2)
  "We have two lists of c-lines, and two matching lists of reductions in their length, from
   the front and back.  It's these reduced lines we want to compare, making sure that
   for every line in LINES1 there's a superset of that line in LINES2.
  "
  (loop for line1 in lines1
	for red1 in reductions1
	always
	(loop for line2 in lines2
	      for red2 in reductions2
	      thereis
	      (reduced-line-subset line1 red1 line2 red2))))

(defun REDUCED-LINE-SUBSET (line1 red1 line2 red2)
  (and (eq line1 line2)
       (<= (car red2) (car red1))
       (<= (cadr red2) (cadr red1))))