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

;;; -*- Mode: LISP; Base: 10; Package: 2DG; Syntax: Common-Lisp -*-
(in-package '2DG)

(export '(gaussian-smooth-uniform-theta-s *default-gaussian-cutoff* gaussian-convolve-1d
          1-2-1-convolve spring-2d-smooth gaussian make-1d-gaussian-filter normalize-filter
          derivative-theta-s derivative-vector clemens-smooth dejag-2d-smooth find-jag-run fill-jag-run))
						
(defvar *DEFAULT-GAUSSIAN-CUTOFF* .01)

(defun GAUSSIAN-SMOOTH-UNIFORM-THETA-S (theta-s sigma &key (end-case :truncate))
  "We smooth a theta-s space graph with 
   a Gaussian with the appropriate sigma.  By 'uniform', we mean that
   the theta-s s's all increase uniformly, i.e., we know the tangent
   at uniform intervals in the curve."
  (let ((new-nums (gaussian-convolve-1d (theta-s-thetas theta-s) sigma end-case)))
    (if (null new-nums) nil
	(let* ((new-length (car (array-dimensions new-nums)))
	       (new-theta-s (make-theta-s new-length)))
	  (loop for i from 0 to (1- new-length)
		doing
	    (set-theta-s-theta new-theta-s i (aref new-nums i)))
	  (loop for i from (/ (- (theta-s-length theta-s) new-length) 2)
		for j from 0 to (1- new-length)
		doing
	    (set-theta-s-s new-theta-s j (theta-s-s theta-s i)))
	  new-theta-s))))

(defun GAUSSIAN-CONVOLVE-1D (nums sigma end-case)
  "Convolve an array of numbers with a Gaussian.
   With end-case = :truncate, this will shorten the array; nil will be returned when
   the filter is bigger than the number vector."
  (ecase end-case
    (:truncate
      (let* ((filter (make-1d-gaussian-filter sigma *default-gaussian-cutoff*))
	     (filter-length--1 (1- (car (array-dimensions filter)))))
	(if (< (car (array-dimensions nums)) (car (array-dimensions filter))) nil
	    (let* ((cutoff (/ filter-length--1 2))
		   (new-nums (make-array (list (- (car (array-dimensions nums)) (* 2 cutoff))) :element-type 'float)))
	      (loop for i from cutoff to (- (1- (car (array-dimensions nums))) cutoff)
		    for new-i from 0
		    doing
		(setf (aref new-nums new-i)
		      (loop for j from 0 to filter-length--1
			    for num-index from (- i cutoff) to (+ i cutoff)
			    summing
			      (* (aref filter j) (aref nums num-index)))))
	      new-nums))))
    (:repeat					; Repeats the first and last numbers.
      (let* ((filter (make-1d-gaussian-filter sigma *default-gaussian-cutoff*))
	     (filter-length--1 (1- (car (array-dimensions filter))))
	     (filter-length--1/2 (/ filter-length--1 2)))
	(if (< (car (array-dimensions nums)) (car (array-dimensions filter))) nil
	    (let* ((new-nums (make-array (array-dimensions nums) :element-type 'float))
		   (num-len (car (array-dimensions nums)))
		   (1st-num (aref nums 0))
		   (last-num (aref nums (1- num-len))))
	      (loop for i from 0 to (1- num-len)
		    doing
		(setf (aref new-nums i)
		      (loop for j from 0 to filter-length--1
			    for k from (- i filter-length--1/2) to (+ i filter-length--1/2)
			    summing
			      (* (aref filter j) (if (< k 0) 1st-num
						     (if (<= num-len k)  last-num
							 (aref nums k)))))))
	      new-nums))))
    (:reflect
      (let* ((filter (make-1d-gaussian-filter sigma *default-gaussian-cutoff*))
	     (filter-length--1 (1- (car (array-dimensions filter))))
	     (filter-length--1/2 (/ filter-length--1 2)))
	(if (< (car (array-dimensions nums)) filter-length--1/2) nil
	    (let* ((reflected-nums (make-reflected-nums nums filter-length--1/2))
		   (new-nums (make-array (array-dimensions nums) :element-type 'float))
		   (num-len (car (array-dimensions nums))))
	      (loop for counter from 0 to (1- num-len)
		    for i from filter-length--1/2
		    doing
		(setf (aref new-nums counter)
		      (loop for j from 0 to filter-length--1
			    for k from (- i filter-length--1/2) to (+ i filter-length--1/2)
			    summing
			      (* (aref filter j) (aref reflected-nums k)))))
	      new-nums))))
    ))

(defun MAKE-REFLECTED-NUMS (nums reflection-length)
  "We assume that the reflection-length isn't greater than the number of nums"
  (let* ((num-length (car (array-dimensions nums)))
	 (new-nums (make-array (list (+ (* 2 reflection-length) num-length))
			      :element-type (array-element-type nums))))	
    (loop for i from 0 to (1- reflection-length)
	  for j from (1- reflection-length) downto 0
	  doing
      (setf (aref new-nums i) (aref nums j)))
    (loop for i from reflection-length
          for j from 0 to (1- num-length)
	  doing
      (setf (aref new-nums i) (aref nums j)))
    (loop for i from (+ reflection-length num-length)
	  for j from (1- num-length) downto (- num-length reflection-length)
	  doing
      (setf (aref new-nums i) (aref nums j)))
    new-nums))

(defun 1-2-1-CONVOLVE (nums end-case)
  ;"Convolve the numbers with a |1|2|1| template.
  (case end-case
    (:truncate
      (loop for last-num in nums
	    for num in (cdr nums)
	    for next-num in (cddr nums)
	    collecting
	    (/ (+ last-num num num next-num) 4.0)))))

(defun SPRING-2D-SMOOTH (points closed? n k &optional (smooth-points))
  "The goal of this routine is to compensate for error caused by pixelization.
  On each iteration, the smooth point is set to be the center of mass
  of its starting position (weighted by k) and its two smooth-point
  neighbors (each weighted by 1).  This is the equilibrium position
  the point would find if it were attached to the three other points
  with springs of appropriate stiffness.  End points remain fixed
  unless the curve is closed.  The optional smooth-points arg is
  useful when repeated applications are desired."
  (if (null (cddr points)) points
    (progn
      (unless smooth-points (setq smooth-points (mapcar #'copy-point points)))
      (loop repeat n
            doing
	(let* ((k+2 (+ k 2))
	       (spt-1   (if closed? (car (last smooth-points)) (first smooth-points)))
	       (spt     (if closed? (first smooth-points)      (second smooth-points)))
	       (spt-cdr (if closed? (nthcdr 1 smooth-points)   (nthcdr 2 smooth-points)))
	       (spt+1   (first spt-cdr))
	       (sx-1 (point-x spt-1))
	       (sy-1 (point-y spt-1))
	       (sx   (point-x spt))
	       (sy   (point-y spt))
	       (sx+1 (point-x spt+1))
	       (sy+1 (point-y spt+1)) )
	  (loop	for pt in (if closed? points (rest points))
		for x = (point-x pt)
		for y = (point-y pt)
		doing
	    (mutate-point-x spt (/ (+ (* k x) sx-1 sx+1) k+2))
	    (mutate-point-y spt (/ (+ (* k y) sy-1 sy+1) k+2))
	    (setq spt-cdr (cdr spt-cdr))
	    (when (null spt-cdr)
	      (if closed? (setq spt-cdr smooth-points) (return)) )
	    (setq spt spt+1 spt+1 (first spt-cdr)
		  sx-1 sx sx sx+1 sx+1 (point-x spt+1)
		  sy-1 sy sy sy+1 sy+1 (point-y spt+1) ))))
      smooth-points )))

(defun GAUSSIAN (sigma x)
  (* (/ 1 (* sigma (sqrt g:*2pi*)))
     (exp (/ (- (square x)) (* 2 (square sigma))))))

(defun MAKE-1D-GAUSSIAN-FILTER (sigma cutoff-ratio)
  "Returns a 1d array, that can be used for convolution. The array is
   as long as necessary to ensure that we represent enough of the gaussian.
   We stop when an entry in the filter would be less than the peak of the
   gaussian times the cutoff-ratio."
  (let* ((max-num (gaussian sigma 0))
	 (cutoff-num (* max-num cutoff-ratio)))
    (let ((nums
	    (loop for i from 1
		  for gi = (gaussian sigma i)
		  until (< gi cutoff-num)
		  collecting
		    gi)))
      (let ((filter (make-array (list (1+ (* 2 (length nums)))) :element-type 'float))
	    (center-index (length nums)))
	(setf (aref filter center-index) max-num)
	(loop for i from 1
	      for num in nums
	      doing
	  (setf (aref filter (+ center-index i)) num)
	  (setf (aref filter (- center-index i)) num))
	(normalize-filter filter)))))

(defun NORMALIZE-FILTER (filter)
  "Adjust the values of a filter so they sum to 1."
  (let ((last-index (1- (car (array-dimensions filter)))))
    (let ((sum (loop for i from 0 to last-index
		     summing
		     (aref filter i))))
      (loop for i from 0 to last-index
	    doing
	(setf (aref filter i) (/ (aref filter i) sum)))))
  filter)

(defun DERIVATIVE-THETA-S (theta-s)
  "This assumes that the s's in the theta-s graph are just integers"
  (let ((new-ts (make-theta-s (1- (theta-s-length theta-s)))))
    (loop for i from 0 to (- (theta-s-length theta-s) 2)
	  doing
      (set-theta-s-theta new-ts i (- (theta-s-theta theta-s (1+ i)) (theta-s-theta theta-s i)))
      (set-theta-s-s theta-s i i))
    new-ts))

(defun DERIVATIVE-VECTOR (vector)
  (let ((dvector (make-array (list (1- (car (array-dimensions vector))))
			     :element-type (array-element-type vector))))
    (loop for i from 0 to (- (car (array-dimensions vector)) 2)
	  doing
      (setf (aref dvector i) (- (aref vector (1+ i)) (aref vector i))))
    dvector))

(defun CLEMENS-SMOOTH (points closed? n k)
  (spring-2d-smooth (dejag-2d-smooth points closed?) closed? n k))

;;; Dejags in the image space.  Looks for horizontal or vertical runs
;;; with diagonals at each end and moves the points to lie on a straight
;;; line between the midpoints of the diagonals.  (Does not work if the
;;; points have been smoothed already).
(defun DEJAG-2D-SMOOTH (points closed? &optional (smooth-points (mapcar #'copy-point points)))
  (let* ((circ-s-points smooth-points)
	 (end-pt (first (last circ-s-points)))
	 (prev-dx) (prev-dy)
	 (hit-end?) )
    (when closed?
      (setq circ-s-points (copy-seq circ-s-points))	 ; Copies list, but uses same points.
      (setf (cdr (last circ-s-points)) circ-s-points) )	 ; Make list circular.
    ; Find a starting point.
    (if closed?
	(progn
	  (multiple-value-setq (circ-s-points ignore ignore prev-dx prev-dy ignore)
	    (find-jag-run circ-s-points end-pt) )
	  (setq end-pt (first circ-s-points)) )
	(multiple-value-setq (ignore ignore ignore prev-dx prev-dy ignore)
	  (find-jag-run circ-s-points end-pt) ))
    ; Fill the runs.
    (loop doing
      (multiple-value-setq (circ-s-points prev-dx prev-dy hit-end?)
	(fill-jag-run circ-s-points prev-dx prev-dy end-pt) )
      until hit-end? )
    smooth-points ))

;;; Takes a connected-point list and finds the first diagonal jump, or
;;; the first jump that is different from the run-jump once it has been
;;; established.  Returns six values: the cdr of the point list in which
;;; the first point is immediately after the "bad" jump, whether the run
;;; is vertical, the number of points in the run, the DX and DY of the
;;; "bad" jump, and whether the end point was detected.  Returns a
;;; run-len of 0 if a diagonal is found immediately (no run).  Returns a
;;; DX and DY of NIL if no "bad" jump is found to terminate the run.
(defun FIND-JAG-RUN (points &optional end-pt)
  (declare (values point-cdr vert-run? run-len last-dx last-dy hit-end?))
  (loop with run-dx = nil
	with run-dy = nil
	with hit-end? = nil
	for pt-cdr on points
	for run-len upfrom 0
	for pt1 = (first pt-cdr)
	for pt2 = (second pt-cdr)
	doing
    (unless pt2
      (return (values nil (when run-dx (= run-dx 0)) run-len nil nil t)) )
    (when (eq pt2 end-pt) (setq hit-end? t))
    (let ((dx (- (point-x pt2) (point-x pt1)))
	  (dy (- (point-y pt2) (point-y pt1))))
      (if run-dx
	  (when (or (not (= dx run-dx)) (not (= dy run-dy)))
	    (return (values (cdr pt-cdr) (= run-dx 0) run-len dx dy hit-end?)) )
	  (if (or (zerop dx) (zerop dy))
	      (setq run-dx dx run-dy dy)
	      (return (values (cdr pt-cdr) nil run-len dx dy hit-end?)) )))))

;;; Requires that the first point came right after a diagonal jump of
;;; PREV-DX PREV-DY.  Finds the next diagonal, and fills the run, if any.
;;; Returns the points after the diagonal and the jump of that diagonal.
(defun FILL-JAG-RUN (points prev-dx prev-dy end-pt)
  (declare (values point-cdr next-dx next-dy hit-end?))
  (multiple-value-bind (point-cdr vert-run? run-len next-dx next-dy hit-end?)
      (find-jag-run points end-pt)
;    (format t "~&~d pts -> ~d pts, prev (~d ~d), next (~d ~d), end? ~a"
;	    (length points) (length point-cdr) prev-dx prev-dy next-dx next-dy hit-end? )
    (when (> run-len 0)
      (unless next-dx
	(if prev-dx
	    (setq next-dx prev-dx next-dy prev-dy)
	    (setq next-dx 0 prev-dx 0 next-dy 0 prev-dy 0) ))
      (let ((step-delta)
	    (delta-0) )
	(if vert-run?
	    (setq step-delta (/ (+ next-dx prev-dx) 2.0 (1+ run-len))
		  delta-0 (/ (- step-delta prev-dx) 2.0) )
	    (setq step-delta (/ (+ next-dy prev-dy) 2.0 (1+ run-len))
		  delta-0 (/ (- step-delta prev-dy) 2.0) ))
	(loop
	  for pt in points
	  until (eq pt (first point-cdr))
	  for delta upfrom delta-0 by step-delta
	  doing
	  (if vert-run?
	      (incf (point-x pt) delta)
	      (incf (point-y pt) delta) ))))
    (values point-cdr next-dx next-dy hit-end?) ))

						
