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

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

(in-package :mu)

;;; Display utilities for stereo code.  Note: this file is completely unrelated to Mike's
;;; old file with a similar name.  It contains new code.  (WEG 11/11/87)

;;;**************************************************************************************

;;; Prepare to use the display.  Turn off any active cameras, set up a grey-scale color
;;; map, and expose and clear the display window.
(defun DISPLAY-PREPARATION ()
  #-symbolics
  (progn
    (spe:linear-map)
    (spe::map-window spe:*color-window*)
    (spe:clear-window spe:*color-window*))    
  #+symbolics
  (progn
    (send spe:*color-screen* :stop-camera)
    (spe:linear-map)
    (send spe:*color-window* :expose)
    (send spe:*color-window* :clear-window)))

;;; Display the results of stereomatching.  Depth!! contains the depth data and loc!!
;;; specifies the locations at which the depth data is valid. 
(*defunc *SHOW-DEPTH (depth!! loc!! &optional (x 0) (y 0) (use-color t) (pause nil))
  (if use-color
      (progn
	;; Set up an inverse spectrum, blue to red.  Color value 0 is black so that we can
	;; clearly distinguish depth data from the background.
	(spe:inverse-spectral-map :black 0)
	;; Avoid displaying depth values in black by normalizing the disparity values to
	;; the range 1 - 255.
	(*let ((display!!
		 (if!! (context-rectangle!! *top-edges-clip* *left-edges-clip*
					    *right-edges-clip* *bottom-edges-clip*)
;		       (rescale-pvar!! depth!! 1 255)


		       (let ((scale (/ 254.0 (- (or *max-disparity-for-display* *fd*)
						(or *min-disparity-for-display* *id*)))))
			 (min!! (!! 255) 
				(max!! (!! 1) 
				       (1+!!
					 (round!!
					   (*!!
					     (-!! depth!!
						  (!! (or *min-disparity-for-display* *id*)))
					     (!! scale)))
					     ))))
		       (!! 0))))
	  (declare (type (field-pvar 8) display!!))
	  (*show-pvar (if!! loc!! display!! (!! 0)) :x x :y y :normalize? nil)))
      (*show-pvar (if!! loc!! depth!! (!! 0)) :x x :y y))
  (if pause (pause)))

;;; Zoom the field pvar from the quadrant with upper corner <x y> to fill the machine,
;;; expanding by a factor of two.  If the parameter replicate is true, then zooming just
;;; replicates pixels.  Otherwise the zooming is connectivity-preserving: if two pixels
;;; were originally 8-connected, they remain eight-connected after zooming.
(*defunc ZOOM-FIELD!! (field!! x-bound y-bound &key (replicate t))
  (*let ((zoomed-field!! (!! 0))
	 (x!! (self-address-grid!! (!! 0)))
	 (y!! (self-address-grid!! (!! 1)))
	 s!! se!! e!!)
    `(declare (type (field-pvar ,(pvar-length field!!)) zoomed-field!! s!! se!! e!!))
    ;; Take the chunk of disparity!! bounded by x-bound <= x < x-bound+128,
    ;; y-bound <= y < y-bound+128 and expand it by a factor of 2 to fill the CM.
    (*when (and!! (>=!! x!! (!! x-bound)) (<!! (-!! x!! (!! x-bound)) (!! 128))
		  (>=!! y!! (!! y-bound)) (<!! (-!! y!! (!! y-bound)) (!! 128)))
      (*set s!! (cmv::south!! field!!))
      (*set se!! (cmv::southeast!! field!!))
      (*set e!! (cmv::east!! field!!))
      (*let ((dest-x!! (ash!! (-!! x!! (!! x-bound)) (!! 1)))
	     (dest-y!! (ash!! (-!! y!! (!! y-bound)) (!! 1))))
	(*pset :logior field!! zoomed-field!! (cube-from-grid-address!! dest-x!! dest-y!!)
	       :collision-mode :no-collisions)
	(loop for pvar!! in (list s!! se!! e!!) do
	   (*pset :logior pvar!! pvar!! (cube-from-grid-address!! dest-x!! dest-y!!)
			  :collision-mode :no-collisions))))
    (*when (and!! (evenp!! x!!) (evenp!! y!!))
      (if replicate
	  ;; replicate pixels
	  (progn (setf (cmv::south!! zoomed-field!!) zoomed-field!!)
		 (setf (cmv::southeast!! zoomed-field!!)  zoomed-field!!)
		 (setf (cmv::east!! zoomed-field!!) zoomed-field!!))
	  ;; for any direction in which there was a neighbor, interpolate a new neighbor
	  (progn (setf (cmv::south!! zoomed-field!!) s!!)
		 (setf (cmv::southeast!! zoomed-field!!) se!!)
		 (setf (cmv::east!! zoomed-field!!) e!!))))
    zoomed-field!!))

;;; Draw depth data, zoomed by a factor of two.  The disparities are assumed to have
;;; been rescaled so that all values are odd and lie in the range 1 to 255, inclusive.
(*defunc *DRAW-ZOOMED-DEPTH-DATA (image-input image-output disparity!! i j &key replicate)
  (declare (type (field-pvar 8) disparity!!))
  (*let ((zoomed-disp!! (zoom-field!! disparity!! (* i 128) (* j 128) :replicate replicate)))
    (declare (type (field-pvar 8) zoomed-disp!!))
    (*transpose zoomed-disp!!)			;switch to raster coords
    (let* ((display-rows-missing (floor (- 512 *frame-height*) 2))
	   (display-cols-extra (floor (- *frame-width* 512) 2))
	   (x-start 0) (y-start 0)
	   (x-end (+ x-start 256)) (y-end (+ y-start 256))
	   (array-row-offset (- (* j 256) display-rows-missing))
	   (array-col-offset (+ (* i 256) display-cols-extra)))
      (if (= j 0) (setq x-start (+ x-start display-rows-missing)
			array-row-offset 0))
      (if (= j 1) (setq x-end (- x-end display-rows-missing)))
      ;; Read in the relevant part of image-input, merge image-input and the stereo data,
      ;; then write the merged image to image-output.
      (*let ((image!! (!! 0)))
	(declare (type (field-pvar 8) image!!))
	;; read in the image
	(fastio::*write-array-to-cm-grid image-input image!!
				:array-row-offset array-row-offset
				:array-col-offset array-col-offset
				:x-start x-start :y-start y-start
				:x-end x-end :y-end y-end)
	;; Merge the image and stereo data.  Clear the low bit of the image to obtain grey scale
	;; output (the color map will be grey-and-inverse-spectral).
	(*set image!! (if!! (zerop!! zoomed-disp!!)
			    (logand!! image!! (!! 254))
			    zoomed-disp!!))
	;; now display the merged image and stereo data
	(fastio::*read-array-from-cm-grid image-output image!!
			       :array-row-offset array-row-offset
			       :array-col-offset array-col-offset
			       :x-start x-start :y-start y-start
			       :x-end x-end :y-end y-end)))))

;;; Display the full left view (not downsampled) with superimposed stereo data.  The stereo
;;; data can be either the depth data or isodisparity contours.  Note: x and y refer to CM
;;; coordinates, not screen raster coordinates.  Also note that we use *right-array* to
;;; avoid having to create a new display array.
(*defunc *SHOW-FULL-LEFT-VIEW-WITH-STEREO-DATA
	 (full-left-view-to-show disparity-map!! disparity-loc!! pause replicate
				 &aux (display-cols-extra (floor (- *frame-width* 512) 2)))
  (declare (ignore pause))			;we may use this in the future
  (format t "~%Zooming isodisparity contours...")
;  (tv:noting-progress ("Zooming isodisparity contours")
    (cond ((not full-left-view-to-show)
	   (spe:grab-frame :dest *left-array* :channel 0 :wait nil)
	   (setq full-left-view-to-show *left-array*)))
    ;; Set the color map so that even values are grey and odd values are spectral.
    ;; The depth data will be displayed in color using the 128 odd grey
    ;; values.  Normalize to a range of 128, then transform to odd grey values.
    (*let ((rescaled-disparity!! (!! 0)))
      (declare (type (field-pvar 8) rescaled-disparity!!))
      (*if disparity-loc!!
	   (progn
	     (*set rescaled-disparity!! (rescale-pvar!! disparity-map!! 0 127 0))
	     (*set rescaled-disparity!! (1+!! (ash!! rescaled-disparity!! (!! 1)))))
	   (*set rescaled-disparity!! (!! 0)))
      ;; Note that we no longer need disp-loc!!, since a value of 0 in zoomed-map!!  indicates that
      ;; there is no depth data at that point.  Expand and display the data, one fourth at a time.
      (loop for i from 0 to 1 do
	(loop for j from 0 to 1 do
	  (*draw-zoomed-depth-data full-left-view-to-show *right-array* rescaled-disparity!! i j :replicate replicate))))
    ;; Clear the portion of the output array that doesn't contain stereo data.

;;; ************************************************** CONVERT

    display-cols-extra
;    (vu:zero-raster *right-array* 0 0 display-cols-extra)
;    (vu:zero-raster *right-array* (+ 512 display-cols-extra) 0 display-cols-extra)
    (spe:clear-window spe:*color-window*)
    (spe:grey-and-inverse-spectral-map)
    (spe:show-color spe:*color-window* *right-array*)
    )
;)

#+symbolics
(defun save-stereo-display (&optional (depth-data? nil))
  ;; Assumes it's displayed in the center of the screen
  (let ((new (make-raster-array 256 256 :element-type '(unsigned-byte 8)))
	(from-x (if depth-data? *stereo-x0-offset* (floor (- *frame-width* 256) 2)))
	(from-y (if depth-data? *stereo-y-offset* (floor (- *frame-height* 256) 2))))
    (send spe:*color-window* :bitblt-from-sheet tv:alu-seta 256 256 from-x from-y new 0 0)
    new))

#+symbolics
(defun show-saved-display (array &optional (to-x 0) (to-y 0))
  (multiple-value-bind (ww hh) (decode-raster-array array)
    (send spe:*color-window* :bitblt tv:alu-seta ww hh array 0 0 to-x to-y)))

;;; Display the downsampled left view with stereo data superimposed.  The stereo data
;;; can be either the depth data or isodisparity contours.
(*defunc *SHOW-LEFT-VIEW-WITH-STEREO-DATA
	 (image disparity-map!! disparity-loc!! pause)
  (ignore pause)				;we may use this in the future
  (format t "~%Superimposing isodisparity contours...")
;  (tv:noting-progress ("Superimposing isodisparity contours")
    (let ((x-offset (floor (- *frame-width* 256) 2))
	  (y-offset (floor (- *frame-height* 256) 2)))
      (spe:clear-window spe:*color-window*)
      (cmv::with-temp-pvars
	(let ((image!! (*lx:allocate-field-pvar 8))
	      (disp!! (*lx:allocate-field-pvar 8)))
	  ;; Read in the image (to save stack space, we discarded it earlier).
	  (fastio:*write-raster-to-cm-grid image image!! :transpose-in-cm t)
	  ;; Normalize the image to prepare it for displaying, using only the disparity
	  ;; values that coincide with contours.
	  (*when disparity-loc!! (*set disp!! (rescale-pvar!! disparity-map!!)))
	  ;; Set up the color map to mix grey scale and color.  Even slots are grey,
	  ;; odd slots are spectral.
	  (spe:grey-and-inverse-spectral-map)
	  ;; Turn on bit 0 in the disparity data to get color and turn off bit 0 in the
	  ;; image to get grey scale.
	  (*set disp!! (logior!! disp!! (!! 1)))
	  (*set image!! (logand!! image!! (!! 254)))
	  ;; display the image with contours superimposed
	  (*show-pvar (if!! disparity-loc!! disp!! image!!)
		      :x x-offset :y y-offset :normalize? nil)))))
;)

;;; to do: generalize to handle raster or pvar arguments
(*defunc *SHOW-RASTER-WITH-1BIT-OVERLAY
	 (raster bit &key
		 clear				;clear screen first
		 (x-offset (floor (- *frame-width* 256) 2))
		 (y-offset (floor (- *frame-height* 256) 2))
		 (ignore-border 0)
		 (red spe:*overlay-red*) (green spe:*overlay-green*) (blue spe:*overlay-blue*)
		 &aux bit!!)
  (cmv::with-temp-pvars
    (cond ((not (pvarp bit))
	   (setq bit!! (*lx:allocate-field-pvar 1))
	   (fastio:*write-raster-to-cm-grid bit bit!!))
	  (t (setq bit!! bit)))
    (if clear (spe:clear-window spe:*color-window*))
    (spe:overlay-map :red red :green green :blue blue)
    (setq bit!! (fastio:coerce-boolean-to-bit!! bit!!))
    (*let ((image (fastio:write-raster-to-cm-grid!! raster)))
      (*set image (deposit-byte!! image (!! 0) (!! 1) bit!!))
      (*show-pvar image :x x-offset :y y-offset :normalize? nil :ignore-border ignore-border))))

;;; *** old code ***
;  (cmv::with-temp-pvars
;    ;; put raster up on grey screen
;    (cond
;      ;; the order of these cases matters, since a pvar is also an array
;      ((pvarp raster) (*show-pvar raster :x x-offset :y y-offset))
;      ((arrayp raster) (spe::show-color spe:*color-window* raster
;				:to-x x-offset :to-y y-offset))
;      (t (error "Bogus raster argument ~A to *show-raster-with-1bit-overlay: must be either an array or a pvar." raster)))
;    (vu::zero-raster (grey::grey-array 0))	;clear the low bit of the grey screen
;    ;; set up 1 bit color overlay
;    (grey::overlay-color-map *overlay-red* *overlay-green* *overlay-blue*)
;    (cond
;      ((pvarp bit) (*show-pvar bit :x x-offset :y y-offset
;			       :display-raster (grey::grey-array 0)
;			       :normalize? nil
;			       :ignore-border ignore-border))
;      ((arrayp bit) (vu::copy-raster bit :to-raster (grey::grey-array 0)
;				     :to-x x-offset :to-y y-offset))
;      (t (error "Bogus raster argument ~A to *show-raster-with-1bit-overlay: must be either an array or a pvar." raster)))))

;;; Display a stereo anaglyph.  The upper four bits of the left image are mapped into the red channel
;;; and the upper four bits of the right image are mapped into the blue channel.  This function works
;;; for 1-bit random dot stereograms as well.
(*defunc *SHOW-ANAGLYPH (&key left!! right!! (left *left-256*) (right *right-256*)
			      (download t))		;if true, download arrays to pvars
  (spe:blue-and-red-map)
  (cmv::with-temp-pvars
    (if download (setq left!! (fastio:write-raster-to-cm-grid!! left)
		       right!! (fastio:write-raster-to-cm-grid!! right)))
    (if (= (pvar-length left!!) 1)
	(setq left!! (*lx:allocate-field-pvar 8 :initial-value (if!! (nzerop!! left!!) (!! 255) (!! 0)))
	      right!! (*lx:allocate-field-pvar 8 :initial-value (if!! (nzerop!! right!!) (!! 255) (!! 0)))))
    (*show-pvar
      (+!! (ash!! (load-byte!! left!! (!! 4) (!! 4)) (!! 4))
	   (ash!! (load-byte!! right!! (!! 4) (!! 4)) (!! 0)))
      :normalize? nil
      )))

(*defunc *SHOW-DEPTHS-FROM-BOTH-VIEWS
	 (disparity-map!! disp-locations!! pause)
  "Show the depth map as it appears from both the left and right views."
  (*show-depth disparity-map!! disp-locations!!
	       *stereo-x0-offset* *stereo-y-offset* use-color nil)
; Temporarily comment out until I find out what happened to depth-for-right-view!!
; PAO 89mar08
;  (*show-pvar (depth-for-right-view!! disparity-map!! disp-locations!!)
;	      :x *stereo-x1-offset* :y *stereo-y-offset*)
  (if pause (pause)))

;;; hacked to explore principle of filling in occluded regions with nearby data
#||
(*defunc *SHOW-DEPTHS-FROM-BOTH-VIEWS
	 (disparity-map!! disp-locations!! pause)
  "Show the depth map as it appears from both the left and right views."
  (spe:inverse-spectral-map :black 0)
  (*show-depth disparity-map!! disp-locations!!
	       *stereo-x0-offset* *stereo-y-offset* use-color nil)
  (*let ((filled-disparity-map!! disparity-map!!))
    (*set filled-disparity-map!! (scan-grid!! filled-disparity-map!! 'copy!! :direction :forward :dimensions :grid-x :segment-pvar disp-locations!!))
    (*show-pvar filled-disparity-map!!
		 :x *stereo-x1-offset* :y *stereo-y-offset*)
    (*when (not!! filled-disparity-map!!) (*set filled-disparity-map!! (!! 0)))
    (*set disparity-map!! filled-disparity-map!!))
  (if pause (pause)))
||#

;;;*****************************************************************************************

;;; a greatly simplified version of function shift-edges in >mike-utils7>shift-edges,
;;; which is broken in rel 7
(*defunc *SHOW-STEREO-EDGE-ALIGNMENTS
	 (&key (left *left-256*) (right *right-256*) (sigma *sigma*)
	       (noise-percentage cmv::*noise-percentage*)
	       (id *id*) (fd *fd*) (ntimes 1))
  (*let ((l!! (cmv::canny-edges!! sigma (fastio:write-raster-to-cm-grid!! left) nil nil nil nil nil
				  :noise-percentage noise-percentage))
	 (r!! (cmv::canny-edges!! sigma (fastio:write-raster-to-cm-grid!! right) nil nil nil nil nil
				  :noise-percentage noise-percentage))
	 (xored-edges!! (!! 0)))
    (declare (type (field-pvar 1) l!! r!! xored-edges!!))
    (loop for i from 1 to ntimes do
      (loop for d from id to fd do
	(*set xored-edges!!
	      (logxor!! l!!
			(news-border!! r!! (!! 0) (- d) 0)))
	(*show-pvar xored-edges!!)))))