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

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

;;;*****************************************************************************
;;; CHANGE HISTORY
;;;
;;; 9/25/87  WEG  Changed the package to mu.  Recompiled for Release 7.
;;;*****************************************************************************

(in-package :mu)

;;; This file contains code for dealing with the weird 576.-by-454. arrays returned by 
;;; the frame-grabber.

;;; This file contains the incredibly useful function MOVE-FRAMES.  It
;;; takes the bizarre 576-by-454 arrays used by Noble Larson's frame grabber and
;;; puts them into nice 512x512, 256x256, 128x128 or 64x64 arrays, like the kind
;;; we're all used to around here.

(*proclaim '(*defun warp-with-router!!))

(defmacro MAKE-FRAME-RASTER (&rest rest)
  `(vu:make-raster-array *frame-width* *frame-height* :element-type '(mod 256) ,@rest))

;;; don't allocate these arrays unless needed, to avoid paging (WEG 10/27/87)
;(defvar *left-512* (make-array '(512 512) :element-type '(mod 256)))
;(defvar *right-512* (make-array '(512 512) :element-type '(mod 256)))
(defvar *left-512* nil)
(defvar *right-512* nil)
(defvar *left-256* (make-array '(256 256) :element-type '(mod 256)))
(defvar *right-256* (make-array '(256 256) :element-type '(mod 256)))
(defvar *left-128* (make-array '(128 128) :element-type '(mod 256)))
(defvar *right-128* (make-array '(128 128) :element-type '(mod 256)))
(defvar *left-64* (make-array '(64 64) :element-type '(mod 256)))
(defvar *right-64* (make-array '(64 64) :element-type '(mod 256)))

(defvar *LEFT-ARRAY* (make-frame-raster))
(defvar *RIGHT-ARRAY* (make-frame-raster))

;;; obsolete because of lxb-color
;(defun MOVE-GREY-ARRAY-TO-*LEFT-ARRAY* ()
;  (vu::copy-raster grey::grey-array :to-raster *left-array*))
;
;(defun MOVE-GREY-ARRAY-TO-*RIGHT-ARRAY* ()
;  (vu::copy-raster grey::grey-array :to-raster *right-array*))

(defun GRAB-LEFT (&optional wait?)
  (declare (special he:*left-eye*))
;  (grey::grab-frame 0 0 nil nil wait?)
;  (move-grey-array-to-*left-array*)
  (spe:grab-frame :dest *left-array*
		  :channel #+symbolics (he:eye-camera-channel he:*left-eye*)
		           #-symbolics 3
		  :wait wait?))

(defun GRAB-RIGHT (&optional wait?)
  (declare (special he:*right-eye*))
;  (grey::grab-frame 1 0 nil nil wait?)
;  (move-grey-array-to-*right-array*)
  (spe:grab-frame :dest *right-array*
		  :channel #+symbolics (he:eye-camera-channel he:*right-eye*)
		   	   #-symbolics 7
		  :wait wait?))

;;; ************************************************** CONVERT ^^ VV
  

#+lucid
(defun GRAB-STEREO-PAIR-TWIDDLE
       (&key (crosshairs? t) (wait-for-user-to-hit-space-bar t) (interval 10))
  (declare (ignore crosshairs? wait-for-user-to-hit-space-bar interval))
  (grab-left)
  (grab-right))

#+symbolics
(defun GRAB-STEREO-PAIR-TWIDDLE
       (&key (crosshairs? t) (wait-for-user-to-hit-space-bar t) (interval 10))
  (grey:with-grey-screen-protected (spe:*color-screen* :window window)
    (when wait-for-user-to-hit-space-bar
      (format t "~%Hit space bar to grab frames when the scene is set up the way you want it.~%")

      (loop until (listen) do
	(send spe:*color-screen* :start-camera
	      :channel (he:eye-camera-channel he:*left-eye*))
	(send spe:*color-screen* :stop-camera)
	(if crosshairs? (draw-crosshairs window))
	(zl:process-sleep interval)
	(send spe:*color-screen* :start-camera
	      :channel (he:eye-camera-channel he:*right-eye*))
	(send spe:*color-screen* :stop-camera)
	(if crosshairs? (draw-crosshairs window))
	(zl:process-sleep interval)
	    finally
	      (read-char)
	      ))
    (grab-left)
    (grab-right)))

#||
(defun MOVE-FRAMES (&optional (to-size 512)
		    with-averaging? upside-down? (l? t) (r? t) (from-size 512))
  (if l?
      (force-grabbed-image-into-a-square
	(left-array-of-size to-size) with-averaging? upside-down? from-size *left-array*))
  (if r?
      (force-grabbed-image-into-a-square
	(right-array-of-size to-size) with-averaging? upside-down? from-size *right-array*)))
||#

(defun MOVE-FRAMES (&optional (to-size 512)
		    with-averaging? upside-down? (l? t) (r? t) (from-size 512))
  to-size
  (if l?
      (force-grabbed-image-into-a-square
	*left-array* *left-256* with-averaging? upside-down? from-size))
  (if r?
      (force-grabbed-image-into-a-square
	*right-array* *right-256* with-averaging? upside-down? from-size)))

(defun LEFT-ARRAY-OF-SIZE (size)
  (case size
    ((nil) *left-array*)
    (576 *left-array*)
    (512 *left-512*)
    (256 *left-256*)
    (128 *left-128*)
    (64 *left-64*)))

(defun RIGHT-ARRAY-OF-SIZE (size)
  (case size
    ((nil) *right-array*)
    (576 *right-array*)
    (512 *right-512*)
    (256 *right-256*)
    (128 *right-128*)
    (64 *right-64*)))

(*defunc BILINEAR-SUBSAMPLING (left!! right!!)
  "Using fancy (i.e., slow) bilinear subsampling, extract 256x256 left and right arrays
from the central 454x454 portions of the left and right frames."
    (*set left!! (cmv::subsample-frame!! *left-array*)
	  right!! (cmv::subsample-frame!! *right-array*))
    (cmv::*read-raster-from-cm-grid *left-256* left!!)
    (cmv::*read-raster-from-cm-grid *right-256* right!!)
    )						;save the sampled left view

(*defunc FAST-SUBSAMPLING (left!! right!!)
  "Extract 256x256 left and right arrays from 512x512 central portions of the left and
right frames.  (Since the frame height is only 454, this results in overflow beyond the
boundary, which we fill in with black pixels.)"
    (move-frames 256 *average?*)
    (*set left!! (cmv::write-raster-to-cm-grid!! *left-256*)
	  right!! (cmv::write-raster-to-cm-grid!! *right-256*)))

(proclaim '(*defun warp-with-router!!))

(*defun *GRAB-AND-REGISTER-STEREO-IMAGES (dest-left!! dest-right!!
						       &key stop-and-show?
						       save-warped-right-view
						       already-grabbed
						       wait-for-user-to-hit-space-bar)
	 (declare (special *grabbed-pair*))
	 (require-256x256-CM-grid)	 
	 (when (not already-grabbed)
	   ;; page in the left and right frame arrays for faster grabbing
	   ;;(vu:page-in-rasters *left-array* *right-array*)
	   (grab-stereo-pair-twiddle :wait-for-user-to-hit-space-bar
				     wait-for-user-to-hit-space-bar))
	 (if *bilinear-subsampling?*
	     (bilinear-subsampling dest-left!! dest-right!!)
	     (fast-subsampling dest-left!! dest-right!!))
	 (setq *grabbed-pair* (list *left-256* *right-256*))
	 (cond (*display?*
		(spe:clear-window spe:*color-window*)
		(spe:show-color spe:*color-window* (car *grabbed-pair*)
				:to-x *stereo-x0-offset* :to-y *stereo-y-offset*)
		(spe:show-color spe:*color-window* (cadr *grabbed-pair*)
				:to-x *stereo-x1-offset* :to-y *stereo-y-offset*)))
	 (if stop-and-show? (pause))
	 (if *warp?* (*set dest-right!! (warp-with-router!! dest-right!! *x-warp-array* *y-warp-array*)))
	 (if save-warped-right-view (cmv::*read-raster-from-cm-grid *right-256* dest-right!!))
	 (cond (*display?*
		(*show-pvar dest-left!! :x *stereo-x0-offset* :y *stereo-y-offset*)
		(*show-pvar dest-right!! :x *stereo-x1-offset* :y *stereo-y-offset*)
		(if stop-and-show? (pause)))))