;;; -*- Mode: Lisp; Syntax: Common-lisp; Package: MU; Base: 10.; -*-

;;; Copyright (c) 1987, Massachusetts Institute of Technology
;;; Author: Mike Drumheller

(in-package :mu)

;(*defunc unsigned-SQUARE-SUM!! (pvar diameter)
;  (assert (oddp diameter))
;  (let* ((half-diam (floor (/ (1- diameter) 2))))
;    (*let* ((copy pvar) (x-accumulator-pvar copy) (y-accumulator-pvar (!! 0)))
;      ;; First sum over the left half of the square, progressively
;      ;; getting values from more negative x's.  Note that we
;      ;; progress one x unit at a time, thereby allowing us always
;      ;; to use NEWS instead of the router.
;      (do ((x (- half-diam) (1+ x)))
;	  ((> x -1))
;	(*set copy (news-border!! copy (!! 0) -1 0))
;	(*set copy (*!! copy (!! (gaussian-formula (abs x) 100000))))
;	(*incf x-accumulator-pvar copy))
;      ;;Now do the right side of the square in the same fashion
;      (*set copy pvar)
;      (do ((x 1 (1+ x)))
;	  ((> x half-diam))
;	(*set copy #-(OR CM-5.0 CM-5-1) (pref-grid-relative!! copy (!! 1) (!! 0) :border-pvar (!! 0))
;		   #+(OR CM-5.0 CM-5-1) (news-border!! copy (!! 0) 1 0))
;	(*set copy (*!! copy (!! (gaussian-formula (abs x) 100000))))
;	(*incf x-accumulator-pvar copy))
;      ;; Now each x-accumulator-pvar value is the sum of every pixel with the
;      ;; same x address within diameter/2. 
;      (*incf y-accumulator-pvar x-accumulator-pvar)
;      (*set copy x-accumulator-pvar)
;      ;; Start summing in the y direction, using the same algorithm as for x.
;      ;; Do the bottom half of the square:
;      (do ((y (- half-diam) (1+ y)))
;	  ((> y -1))
;	(*set copy #-(OR CM-5.0 CM-5-1) (pref-grid-relative!! copy (!! 0) (!! -1) :border-pvar (!! 0))
;		   #+(OR CM-5.0 CM-5-1) (news-border!! copy (!! 0) 0 -1))
;	(*set copy (*!! copy (!! (gaussian-formula (abs y) 100000))))
;	(*incf y-accumulator-pvar copy))
;      ;; Do the top half of the square:
;      (*set copy x-accumulator-pvar)
;      (do ((y 1 (1+ y)))
;	  ((> y half-diam))
;	(*set copy #-(OR CM-5.0 CM-5-1) (pref-grid-relative!! copy (!! 0) (!! 1) :border-pvar (!! 0))
;		   #+(OR CM-5.0 CM-5-1) (news-border!! copy (!! 0) 0 1))
;	(*set copy (*!! copy (!! (gaussian-formula (abs y) 100000))))
;	(*incf y-accumulator-pvar copy))
;      y-accumulator-pvar)))

(*defunc UNSIGNED-SQUARE-SUM!! (pvar diameter)
  "For documentation see SQUARE-SUM!!"
  (assert (oddp diameter))
  (let* ((half-diam (floor (/ (1- diameter) 2))))
    (*let* ((copy pvar)
	    (x-accumulator-pvar copy)
	    (temp (!! 0))
	    (y-accumulator-pvar (!! 0)))
      (declare (type (field-pvar (pvar-length pvar)) copy)
	       (type (field-pvar (+ 1 (floor (+ (log diameter 2) (pvar-length pvar)))))
		     x-accumulator-pvar temp)
	       (type (field-pvar (+ 1 (floor (+ (* 2 (log diameter 2)) (pvar-length pvar)))))
		     y-accumulator-pvar))
      (do ((x (- half-diam) (1+ x)))
	  ((> x -1))
	(*set copy (news-border!! copy (!! 0) -1 0))
	(*incf x-accumulator-pvar copy))
      (*set copy pvar)
      (do ((x 1 (1+ x)))
	  ((> x half-diam))
	(*set copy  (news-border!! copy (!! 0) 1 0))
	(*incf x-accumulator-pvar copy))
      (*incf y-accumulator-pvar x-accumulator-pvar)
      (*set temp x-accumulator-pvar)
      (do ((y (- half-diam) (1+ y)))
	  ((> y -1))
	(*set temp (news-border!! temp (!! 0) 0 -1))

	(*incf y-accumulator-pvar temp))

      (*set temp x-accumulator-pvar)
      (do ((y 1 (1+ y)))
	  ((> y half-diam))
	(*set temp (news-border!! temp (!! 0) 0 1))

	(*incf y-accumulator-pvar temp))
      y-accumulator-pvar)))

;;; This function works, but it is much slower than unsigned-square-sum!! because
;;; scan-grid!! is badly implemented (at least, in rel 4.0).  Jim Little has
;;; implemented a function called *sum-nbhd that wins by scanning the grid in PARIS.
;(*defunc UNSIGNED-SQUARE-SUM-SCAN!! (pvar!! diameter
;					    &aux (half-diam (floor (/ (1- diameter) 2))))
;  (assert (oddp diameter))
;  (*let ((x!! (self-address-grid!! (!! 0)))
;	 (y!! (self-address-grid!! (!! 1)))
;	 (sum!! (!! 0)))
;    ;; Scan in the x direction to add up the pvar.
;    (*set sum!! (scan-grid!! pvar!! '+!! :dimension :x :direction :forward))
;    ;; Subtract the sum on the left edge from the sum on the right edge.
;    (*let ((left-x!! (max!! (!! 0) (-!! x!! (!! half-diam))))
;	   (right-x!! (min!! (!! (1- (dimension-size 0))) (+!! x!! (!! half-diam)))))
;      (*set sum!! (-!! (pref-grid!! sum!! left-x!! y!!)
;		       (pref-grid!! sum!! right-x!! y!!))))
;    ;; Scan in the y direction to add up the intermediate horizontal sums.
;    (*set sum!! (scan-grid!! sum!! '+!! :dimension :y :direction :forward))
;    ;; Subtract the sum on the top edge from the sum on the bottom edge.
;    (*let ((top-y!! (max!! (!! 0) (-!! y!! (!! half-diam))))
;	   (bottom-y!! (min!! (!! (1- (dimension-size 1))) (+!! y!! (!! half-diam)))))
;      (*set sum!! (-!! (pref-grid!! sum!! x!! top-y!!)
;		       (pref-grid!! sum!! x!! bottom-y!!))))
;    sum!!))

(*defunc TEST-UNSIGNED-SQUARE-SUM ()
  (*let ((temp (!! 1)))
    (declare (type (field-pvar 1) temp))
    (pref-grid (unsigned-square-sum!! temp 23) 100 100)))

(defun GAUSSIAN-FORMULA (x sigma)
  (/ (exp (- (/ (float (* x x)) (* 2 sigma sigma))))
     (* sigma (sqrt (* 2 3.1415927)))))

;;;  If HORIZONTAL? is t, then it does a 1d horizontal gaussian convolution.  If HORIZONTAL?
;;;  is nil, then it does a vertical gaussian convolution.
(*defunc 1D-G-CONV!! (image sigma horizontal? &optional (total-width (* 6 sigma)))
  (let ((w2 (floor total-width 2)))
    (*let ((result (!! 0)))
      (*incf result (*!! image (!! (gaussian-formula 0 sigma))))
      (*let ((temp image))
	(dotimes (i w2)
	  (*set temp
		(if horizontal?
		    (news-border!! temp (!! 0) 1 0)
		    (news-border!! temp (!! 0) 0 1)))
	  (*incf result (*!! temp (!! (gaussian-formula (1+ i) sigma))))))
      (*let ((temp image))
	(dotimes (i w2)
	  (*set temp
		(if horizontal?
		    (news-border!! temp (!! 0) -1 0)
		    (news-border!! temp (!! 0) 0 -1)))
	  (*incf result (*!! temp (!! (gaussian-formula (1+ i) sigma))))))
      result)))

(*defunc G-CONV!! (image sigma &optional (total-width (* 6 sigma)))
  (*let ((horiz (1d-g-conv!! image sigma t total-width)))
    (1d-g-conv!! horiz sigma nil total-width)))

(defun SUM-GAUSSIAN (sigma &optional (scale-factor 1.0) round? (total-width (* 6 sigma)))
  (let ((sum
	  (+ (gaussian-formula 0 sigma)
	     (* 2 (loop for i from 1 to (floor total-width 2)
			summing
			  (if round?
			      (round (* scale-factor (gaussian-formula i sigma)))
			      (* scale-factor (gaussian-formula i sigma))))))))
    sum))

;;; Someday I should go back and document this.
(defun GET-SCALE-FACTOR-FOR-SCALE-FACTOR
       (sigma scale-factor &optional (total-width (* 6 sigma)))
  (let ((sum (sum-gaussian sigma scale-factor t total-width)))
    (/ (float scale-factor) sum)))

;;; Use scale-factor for sigma1 (* <result-of-this> scale-factor) for sigma2
(defun GET-SCALE-FACTOR-TO-MAKE-TWO-DIFFERENT-SIGMAS-HAVE-SAME-AREA
       (sigma1 sigma2 scale-factor)
  (let ((sum1 (sum-gaussian sigma1 scale-factor t))
	(sum2 (sum-gaussian sigma2 scale-factor t)))
    (/ (float sum1) sum2)))

;;;  Strictly for unsigned integers.
;;;
;;;  If HORIZONTAL? is t, then it does a 1d horizontal convolution.  If HORIZONTAL? is nil,
;;;  then it does a vertical convolution.
(*defunc UNSIGNED-INTEGER-1D-CONV-WITH-ARRAY-MASK!! (image 1d-array horizontal?)
  (let ((mask-width (array-dimension 1d-array 0)))
    (if (not (oddp mask-width))
	(error
	  "Sorry, the 1d-array must have odd length.  (Otherwise it can't be centered.):  ~a"
	  1d-array))
    (let* ((mask-total-weight
	     (loop for i from 0 below mask-width summing (abs (aref 1d-array i))))
	   (w2 (floor mask-width 2)))
      (*let ((result (!! 0))
	     (temp image))
	(declare (type (field-pvar (+ (pvar-length image)
				      (1+ (floor (log mask-total-weight 2))))) result)
		 (type (field-pvar (pvar-length image)) temp))
	(*incf result (*!! image (!! (aref 1d-array w2))))
	(dotimes (i w2)
	  (let ((mask-val (aref 1d-array (+ w2 i 1))))
	    (*set temp
		  (if horizontal?
		      (news-border!! temp (!! 0) 1 0)
		      (news-border!! temp (!! 0) 0 1)))
	    (*incf result (*!! temp (!! mask-val)))))
	(*set temp image)
	(dotimes (i w2)
	  (let ((mask-val (aref 1d-array (- w2 i 1))))
	    (*set temp
		  (if horizontal?
		      (news-border!! temp (!! 0) -1 0)
		      (news-border!! temp (!! 0) 0 -1)))
	    (*incf  result (*!! temp (!! mask-val)))))
	result))))

(*defunc UNSIGNED-SEPARABLE-CONV!! (image 1d-array)
  (unsigned-integer-1d-conv-with-array-mask!!
    (unsigned-integer-1d-conv-with-array-mask!! image 1d-array t)
    1d-array nil))

(defun MAKE-1D-G-MASK-ARRAY (sigma scale-factor &optional (round? t))
  (let ((x-extent (1- (dotimes (i 10000.)
			(if (zerop (round (* scale-factor (gaussian-formula i sigma))))
			    (return i))))))
    (if (= x-extent (1- (1- 10000.)))
	(error "Something is seriously wrong here..."))
    (let* ((w (1+ (* 2 x-extent)))
	   (array (make-array w)))
      (loop for i from 0 below w
	    for x from (- x-extent) do
	(setf (aref array i) (if round?
				 (round (* scale-factor (gaussian-formula x sigma)))
				 (* scale-factor (gaussian-formula x sigma)))))
      array)))				       

(*defunc UNSIGNED-G-CONV!! (image sigma &optional (scale-factor 100))
  (unsigned-separable-conv!! image (make-1d-g-mask-array sigma scale-factor t)))

(defun MAKE-KERNEL-ELEMENT (rel-x rel-y weight)
  (list rel-x rel-y weight))
(defun KERNEL-X (kernel-element)
  (first kernel-element))
(defun KERNEL-Y (kernel-element)
  (second kernel-element))
(defun KERNEL-W (kernel-element)
  (third kernel-element))

(*defunc LIST-CONV!! (image kernel-list)
  (let* ((last-x 0)
	 (last-y 0)
	 dx dy)
    (*let ((temp image)
	   (result (!! 0)))
      (dolist (k kernel-list)
	(setq dx (- (kernel-x k) last-x)
	      dy (- (kernel-y k) last-y))
	(*set temp (get-grid!! temp dx dy))
	(*incf result (*!! (!! (kernel-w k)) temp))
	(setq last-x (kernel-x k) last-y (kernel-y k)))
      result)))

(*defunc UNIFORM-LIST-CONV!! (image kernel-list)
  (let* ((last-x 0)
	 (last-y 0)
	 dx dy)
    (*let ((temp image)
	   (result (!! 0)))
      (dolist (k kernel-list)
	(setq dx (- (kernel-x k) last-x)
	      dy (- (kernel-y k) last-y))
	(*set temp (get-grid!! temp dx dy))
	(*incf result temp)
	(setq last-x (kernel-x k) last-y (kernel-y k)))
      result)))

;;; W and H are user-specifiable because sometimes the user might know that
;;; only the central patch of the array is non-nil.
(*defunc ARRAY-CONV!!
	(image array &optional (w (array-dimension array 0)) (h (array-dimension array 1)))
  (if (not (and (oddp w) (oddp h)))
      (error "The `useful patch' in the array must have odd dimensions (not ~a ~a) 
              in order for this function to be well-defined"
	     w h)
      (let ((centerx (floor (array-dimension array 0) 2))
	    (centery (floor (array-dimension array 1) 2)))
	(*let ((accum (!! 0)))
	  (with-pvars-moving-in-rectangle ((moving-image image)) dx dy w h
	    (let ((weight (aref array (+ centerx dx) (+ centery dy))))
	      (if weight
		  (cond ((= 1 weight)
			 (*incf accum moving-image))
			((= -1 weight)
			 (*decf accum moving-image))
			(t
			 (*incf accum (*!! moving-image (!! weight))))))))
	  accum))))