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

;;; Copyright (c) 1987, Massachusetts Institute of Technology
;;; Authors: Mike Drumheller, Walter E. Gillett

(in-package :mu)

;;;****************************************************************************************
;;;; Functions for local support operations.

;;; Standard local support uses square, uniformly weighted neighborhoods for speed and
;;; convenience.  Theoretically we should use circularly-symmetric Gaussian-weighted
;;; neighborhoods.  I have experimented with both uniformly-weighted circular neighborhoods
;;; and Gaussian weighting - the results weren't very interesting.  The code for uniform
;;; circular neighborhoods has been archived in old-stereo.

;;; If necessary, truncate the score to fit and record the shift count so that we can shift
;;; all scores by the same amount when done.  Make sure that (1+ score) will fit, not just
;;; the score itself.  This tolerance is needed in some cases.
(*defun *TRUNCATE-SCORE
	(score!! accum truncated-length disparity bits-dropped max-bits-dropped)
  (let* ((score-bits (integer-length (1+ (*max score!!))))
	 (bits-to-drop (if (plusp (- score-bits truncated-length))
			   (- score-bits truncated-length)
			   0)))
    (setf (aref bits-dropped disparity) bits-to-drop)
    (*set accum (if (plusp bits-to-drop)
		    (ash!! score!! (!! (- bits-to-drop)))
		    score!!))
    (max max-bits-dropped bits-to-drop)))	;return the current max-bits-dropped

;;; called only from *square-support-grey
(*defun *EQUALIZE-AVERAGE-INTENSITIES (left!! right!!)
  (let* ((left-average (average left!!))
	 (right-average (average right!!))
	 (dark!! (if (< left-average right-average) left!! right!!)))
    (format t "~%Average intensity of left image is ~D, average intensity of right image is ~D."
	    left-average right-average)
    (format t "~%The darker image will be made brighter to equalize the average intensities.")
    (*set dark!!
	  (min!! (+!! dark!! (!! (abs (round (- left-average right-average)))))
		 (!! (1- (expt 2 (pvar-length dark!!))))))
    ))

(*defun dog!! (ex-gaussian!! in-gaussian!! ex-scale in-scale)
  "To compute the dog from the gaussians, with these scale factors,
   we shift the two gaussians right by scale bits and then take the difference-
   BUT we do it in the following convoluted way to keep as many significant bits as poss."
  (let (bits-of-scale-left)
    (*let ((diff!!
	     (cond ((= ex-scale in-scale)
		    (setf bits-of-scale-left ex-scale)
		    (-!! ex-gaussian!! in-gaussian!!))
		   ((> ex-scale in-scale)
		    (setf bits-of-scale-left in-scale)
		    (-!! (ash!! ex-gaussian!! (!! (- in-scale ex-scale))) in-gaussian!!))
		   (t
		    (setf bits-of-scale-left ex-scale)
		    (-!! ex-gaussian!! (ash!! in-gaussian!! (!! (- ex-scale in-scale))))))))
      (ash!! diff!! 
	     (!!
	       (min 0 (- *grey-feature-bits* (+ 1 cmv::*pixel-bits* bits-of-scale-left))))))))

(*defun GREY-PATCH-DIFF!! (left!! left-average!! right!! right-average!! ivd fvd
				  in-image-scale ex-image-scale)
  (*let* (left-dog!!
	   right-dog!!
	   ;; the scale parameters are actually #bits to shift right...
	   diff!!
	   )
    (declare (type (signed-pvar *grey-feature-bits*) left-dog!! right-dog!!))
	   
      (*set left-dog!! (dog!! left!! left-average!! ex-image-scale in-image-scale))

      (*set right-dog!! (dog!! right!! right-average!! ex-image-scale in-image-scale)) 
	 
    (*set diff!! (if *square?* (square!! (-!! left-dog!! right-dog!!))
		     (abs!! (-!! left-dog!! right-dog!!))))
    ;; For each pixel, compute the minimum difference over the vertical disparity range.
    (if (not (zerop ivd))
	(*slide-pvar right-dog!! 0 (- ivd)))	  ;set initial position (if necessary)
    (loop for vd from ivd to fvd do
      (*set diff!! (min!! diff!! (abs!! (-!! left-dog!! right-dog!!))))
      (*slide-pvar right-dog!! 0 -1))
    diff!!))

(*defun *SQUARE-SUPPORT-GREY (accums xdim ydim i-d f-d ivd fvd)
  "Compute the local support for a match via grey level correlation, specifically the sum of the absolute grey level
differences between patches in the left and right images."
  (ignore f-d ydim)				;square neighborhood, don't need ydim
  (*let* (left!!
	   right!! 
	   (left-average!! (!! 0))
	   (right-average!! (!! 0))
	   ;; Holds absolute difference between zero-mean versions of
	   ;; left image and (shifted) right image.  Note that zero-mean
	   ;; difference requires an extra bit.
	   diff!!
	   ;; Pvar to hold sum of absolute differences between the left
	   ;; and right images.  The absolute difference cannot exceed
	   ;; the maximum pixel value, so adding up a neighborhood of
	   ;; differences requires the same number of bits as adding up
	   ;; an image neighborhood.
	   score!!)
    (declare (type (field-pvar cmv::*smoothed-pixel-bits*) 
		   left!! left-average!! right!! right-average!!)
	     (type (field-pvar (1+ cmv::*smoothed-pixel-bits*))
		   diff!!)
	     (type (field-pvar (sum-field-nbhd-length left-average!! xdim))
		   score!!))
    (let (
	  (bits-dropped (make-array (length accums) :initial-element 0))
	  (max-bits-dropped 0)
	  (truncated-length (pvar-length (car accums)))
	  ;; To save bits, if *discard-bad-scores?* is true, clamp the
	  ;; patch correlation to a maximum value (if the correlation is
	  ;; very bad, we don't care exactly how bad.)  The maximum
	  ;; value corresponds to an average pixel difference over the
	  ;; patch of *max-average-pixel-diff*.
	  (max-allowed-score (if *discard-bad-scores?*
				 (* *max-average-pixel-diff* (square xdim))
				 nil))		;don't clamp the score
	  (max-actual-score 0)
	  in-image-scale			;scale factor from *binomial convolve
	  ex-image-scale
	  )					;keep track of the max score achieved



      (case *filter-type*
	(:gaussian
	  (setf ex-image-scale
		(round (log (cmv::*binomial-convolve *sigma* *left-image* left!!) 2)))
	  (cmv::*binomial-convolve *sigma* *right-image* right!!)
	  (setf in-image-scale ex-image-scale)	;we're gonna fake it!
	  )
	(:dog
	  (setf ex-image-scale
		(round (log (cmv::*binomial-convolve *ex-sigma* *left-image* left!!) 2)))
	  (cmv::*binomial-convolve *ex-sigma* *right-image* right!!)
	  
	  (setf in-image-scale
		(round (log
			 (cmv::*binomial-convolve *in-sigma* *left-image* left-average!!) 2)))
	  (cmv::*binomial-convolve *in-sigma* *right-image* right-average!!)
	  )
	)

;      (*equalize-average-intensities left!! right!!)	;make the average intensities of the two images equal
      (*slide-pvar right!! i-d 0)		;start at the initial disparity
      (*slide-pvar right-average!! i-d 0)
      (if *discard-bad-scores?* (format t "~%Discarding bad scores."))
      (if *use-1d-neighborhoods?* (format t "~&Using 1-d neighborhoods...~%"))
      ;; scan the disparity range, accumulating local support at each disparity
      (loop for accum in accums
	    for disparity from 0 do
	(*set diff!! 
	      (grey-patch-diff!! left!! left-average!! right!! right-average!! ivd fvd
				 in-image-scale ex-image-scale))
	(*set score!! (sum-field-nbhd!! diff!! xdim))
	(setq max-actual-score (max (*max score!!) max-actual-score))
	;; If *discard-bad-scores?* is true, clamp the scores to a maximum value of max-allowed-score.
	;; All scores at or above max-allowed-score are losers.
	(when *discard-bad-scores?*
	  (*when (>!! score!! (!! max-allowed-score)) (*set score!! (!! max-allowed-score))))
	;; if necessary, truncate the score to fit and record the shift count so that we can shift
	;; all scores by the same amount when done
	(setq max-bits-dropped
	      (*truncate-score score!! accum truncated-length disparity bits-dropped max-bits-dropped))
	(*slide-pvar right!! 1 0)
	(*slide-pvar right-average!! 1 0)
	)
      (format t "~%Dropped ~D bits from the scores to save memory." max-bits-dropped)
      ;; Now align the scores so that they will be comparable, and invert them because we are trying to
      ;; minimize the sum of differences.  If we are discarding scores, then a score of max-allowed-score
      ;; should become 0 after inverting.  If we are not discarding scores, then a score of
      ;; max-actual-score should become 1 after inverting.  (By leaving room for one higher score value
      ;; *truncate-score makes sure that this doesn't cause the score to overflow.)
      (let ((max-shifted-score
	      (if (and *discard-bad-scores?* (>= max-actual-score max-allowed-score))
		  (ash max-allowed-score (- max-bits-dropped))
		  (1+ (ash max-actual-score (- max-bits-dropped))))))
	(loop for accum in accums
	      for disparity from 0 do
	  (*set accum (ash!! accum (!! (- (aref bits-dropped disparity) max-bits-dropped))))
	  ;; invert: subtract from max possible value
	  (*set accum (-!! (!! max-shifted-score) accum))))
      )))

;;; A nasty hack.  The problem is that normalized scores increase near the boundary because
;;; the number of edges decreases, so only a few edges need to match to produce a high score.
;;; Large, bogus scores require more bits.  Here we arbitrarily fill in the edge map so that
;;; every other pixel outside of the clipping boundary is an edge.  This makes left-sum!! and
;;; right-sum!! high and keeps the scores low.  Got it?
;;;
;;; Called only by *square-support-normalized.
(*defunc HACKED-EDGES!! (edge!!)
  (*let ((bit!! (if!! (context-rectangle!! *top-edges-clip* *left-edges-clip*
					   *right-edges-clip* *bottom-edges-clip*)
		      edge!! (random!! (!! 2)))))
    (declare (type (field-pvar 1) bit!!))
    bit!!))

;;; Scan the disparity range, accumulating local support at each disparity over a square
;;; neighborhood.  If requested, normalize the scores according to the number of edges in the left
;;; and right image neighborhoods.

(*defunc *SQUARE-SUPPORT-NORMALIZED
	 (support-planes accums l-features!! r-features!! xdim ydim i-d f-d)
  (ignore f-d ydim)				;square neighborhood, don't need ydim
  (cmv::with-temp-pvars
    (let* ((left-sum!! (*lx:allocate-field-pvar (integer-length (square xdim))))
	   (right-sum!! (*lx:allocate-field-pvar (integer-length (square xdim))))
	   ;; pvar to hold sum prior to normalization
	   (sum!! (*lx:allocate-field-pvar (sum-field-nbhd-length (car support-planes) xdim)))
	   (bits-dropped (make-array (length accums) :initial-element 0))
	   (max-bits-dropped 0)
	   (truncated-length (pvar-length (car accums)))
	   left-sum-max right-sum-max)
      ;; the number of edges in the local support neighborhood in each image
      (*set left-sum!! (sum-field-nbhd!! (hacked-edges!! l-features!!) xdim)) 
      (*set right-sum!! (sum-field-nbhd!! (hacked-edges!! r-features!!) xdim)) 
      (setq left-sum-max (*max left-sum!!)
	    right-sum-max (*max right-sum!!))
      (*slide-pvar right-sum!! i-d 0)
      ;; scan the disparity range, accumulating local support at each disparity
      (loop for sp in support-planes
	    for accum in accums
	    for disparity from 0 do
	(*set sum!! (sum-field-nbhd!! sp xdim))
	(*let ((score!! (if!! (and!! (nzerop!! left-sum!!) (nzerop!! right-sum!!))
			      (floor!! (*!! sum!! (!! (* left-sum-max right-sum-max)))
				       (*!! left-sum!! right-sum!!))
			      (!! 0))))
	  ;; if necessary, truncate the score to fit and record the shift count so that we can shift
	  ;; all scores by the same amount when done
	  (setq max-bits-dropped
		(*truncate-score score!! accum truncated-length disparity bits-dropped max-bits-dropped)))
	(*slide-pvar right-sum!! 1 0))
      (format t "~%Dropped ~D bits from the scores to save memory." max-bits-dropped)
      ;; now align the scores so that they will be comparable
      (loop for accum in accums
	    for disparity from 0 do
	(*set accum (ash!! accum (!! (- (aref bits-dropped disparity) max-bits-dropped)))))
      )))

(*defunc *SQUARE-SUPPORT-UNNORMALIZED (support-planes accums xdim ydim)
  (ignore ydim)					;square neighborhood
  ;; We are adding up width^2 elements of support-plane, where width = xdim is the
  ;; width of the square neighborhood to sum.
  (*let ((sum!! (!! 0)))
    (declare (type (field-pvar (+ (pvar-length (car support-planes))
				  (integer-length (square xdim))))
		   sum!!))
    (loop for sp in support-planes
	  for accum in accums
	  for disparity from 0
	  for bits-dropped = (make-array (length accums) :initial-element 0)
	  for max-bits-dropped = 0
	  for truncated-length = (pvar-length (car accums)) do
      (*set sum!! (sum-field-nbhd!! sp xdim))
      ;; if necessary, truncate the score to fit and record the shift count so that we can shift
      ;; all scores by the same amount when done
      (setq max-bits-dropped
	    (*truncate-score sum!! accum truncated-length disparity bits-dropped max-bits-dropped))
	  finally
	    (progn
	      (format t "~%Dropped ~D bits from the scores to save memory." max-bits-dropped)
	      ;; now align the scores so that they will be comparable
	      (loop for accum in accums
		    for disparity from 0 do
		(*set accum (ash!! accum (!! (- (aref bits-dropped disparity) max-bits-dropped)))))))
    ))

;;; Fills accums with local support scores.  Splitting neighborhoods is an experimental,
;;; computationally expensive option.  Matching grey levels is also experimental.
;;;
;;; Called by iterative-stereo.
(defun GATHER-LOCAL-SUPPORT (support-planes accums l-features!! r-features!! xdim ydim i-d f-d ivd fvd)
  (declare (special *split?*))
  (format t "~%Gathering local support ")
  (cond ((and (boundp '*split?*) *split?*)	;hack for splitting neighborhoods
	 (format t "over split neighborhoods...")
;	   (*square-split-local-support-operation support-planes accums xdim ydim clear?)
	 )
	(t (format t "over square neighborhoods~A..."
		   (if (and *normalize?* (not *match-grey?*)) " (normalizing scores)" ""))
	   (cond (*match-grey?* (*square-support-grey accums xdim ydim i-d f-d ivd fvd))
		 (*normalize?*
		  (*square-support-normalized support-planes accums l-features!! r-features!! xdim ydim i-d f-d))
		 ((*square-support-unnormalized support-planes accums xdim ydim))))))