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

;;; Paul E. Oppenheimer, 1987, 1988.

;;; To do:
;;; Make a defstruct to hold pan, tilt, 4 mirror positions, and 2 warping
;;; arrays.  (Look in b:>gillett>mike-utils7>head.lisp for these.)
;;; Write functions to save and restore them.
;;; Saving to disc should be optional.

(in-package :mu)

(*proclaim '(*defun fill-calibration-arrays))

(defun CALIBRATE-STEREO (&optional (size :4k))
;  (grey::straight-map)
  (when (null spe:*color-screen*) (spe:init-color-window))
  (spe:linear-map spe:*color-screen*)
;  (when (tv:mouse-y-or-n-p "Initialize home position of head?") (init-home))
  (give-instructions-for-putting-head-into-position-to-be-calibrated-part-1)
  (read-char)
  (if (not he::*mouse-window*) (he::make-mouse-window))
  (send he::*mouse-window* ':expose-near '(:point 800 100))  
  (give-instructions-for-putting-head-into-position-to-be-calibrated-part-2 he::*mouse-window*)
;  (zl-user::wg-track-with-head)
  (he:operate-manually)
  (give-instructions-for-setting-mirrors)
  (set-mirrors-by-hand)
  (explain-attaching-cm-and-*cold-booting)
  #+*lisp-hardware
  (cm:attach size)
  (*lisp:*cold-boot :initial-dimensions '(256 256))
  (fill-calibration-arrays)
; (save-warping-arrays "pig:<gillett.data>warp3")
  )

(defun GIVE-INSTRUCTIONS-FOR-PUTTING-HEAD-INTO-POSITION-TO-BE-CALIBRATED-PART-1 (&optional (stream tv:selected-window))
  (filling-output (stream)
    (format stream 
	    "~2%The calibration stimulus:~2%The calibration stimulus is a rectangular sheet of paper with a grid of black dots on it.  The dots form a five-by-five rectangle.  There are two such sheets:  a smaller one for calibrating the floor beneath the head, and a larger one for calibrating positions farther away from the head.  Get the appropriate calibration stimulus from room 914 (behind you).  Place it at the location you wish to calibrate.  Stabilize it appropriately.~2%When you have placed the calibration stimulus, type any character to continue.")))

(defun GIVE-INSTRUCTIONS-FOR-PUTTING-HEAD-INTO-POSITION-TO-BE-CALIBRATED-PART-2 (&optional (stream tv:selected-window))
  (filling-output (stream)
    (format stream 
	    "~2%Putting the head into position to be calibrated:~2%Now you want to center and orient the stimulus in the color display screen to your right.  The pattern has a center dot, which you should place in the center of your screen.  The edges of the pattern should parallel the edges of your screen.  You can use the same keys as before to pan and tilt the head.  Each camera looks through two mirrors.  The mirrors can be adjusted by moving the mouse.  As before, click any mouse button when you are satisfied with the placement of the head and mirrors.  ")))

(defun GIVE-INSTRUCTIONS-FOR-SETTING-MIRRORS (&optional (stream tv:selected-window))
  (filling-output (stream)
    (format stream
	    "~2%Vergence:~2%Now you are going to adjust the positions of the mirrors so as to make the two lines of sight converge.  The views from the left and right camera will be displayed alternately.  Adjustments are made by holding down keys; instructions will be displayed.  Upper-case letters adjust sense opposite to lower-case.  For example, S and SHIFT-S change the vertical skew in opposite directions.  There are two modes of adjustment:  coarse and fine.  Begin in coarse mode.  Get into coarse mode by holding down the indicated key.  Adjust direction of gaze first so that the center dot of the pattern is near the center of the color display screen.  Adjust skew to align the horizonal rows of dots.  Adjust vergence to align the vertical columns of dots.  Again adjust the direction of gaze so that the center dot of the pattern is near the center of the display screen (indicated by the crosshairs).  Now go to fine mode.  Continue adjusting until the center dot is stable.  Follow directions in order to quit.  ")))

(defun EXPLAIN-ATTACHING-CM-AND-*COLD-BOOTING (&optional (stream tv:selected-window))
  (filling-output (stream)
    (format stream
	    "Now you need the Connection Machine in order to fill the calibration arrays.  8192 processors of the connection machine will be attached.  ")))


(defstruct (eye-pos (:constructor make-eye-pos-internal
		     (eye inner-mirror outer-mirror zoom focus iris)))
  (eye :left)
  inner-mirror
  outer-mirror
  zoom
  focus
  iris)

(defun save-left-eye-pos ()
  (make-eye-pos-internal :left
			 (he:get-left-inner-mirror-angle)
			 (he:get-left-outer-mirror-angle)
			 (he:read-zoom he:*left-lens*)
			 (he:read-focus he:*left-lens*)
			 (he:read-iris he:*left-lens*)))


(defun save-right-eye-pos ()
  (make-eye-pos-internal :right
			 (he:get-right-inner-mirror-angle)
			 (he:get-right-outer-mirror-angle)
			 (he:read-zoom he:*right-lens*)
			 (he:read-focus he:*right-lens*)
			 (he:read-iris he:*right-lens*)))

(defun set-left-eye-pos (pos &optional (verbose nil))
  ;; Ignore the eye slot.
  (when verbose (write-string "Mirrors..."))
  (he:set-left-inner-mirror-angle (eye-pos-inner-mirror pos))
  (he:set-left-outer-mirror-angle (eye-pos-outer-mirror pos))
  (when verbose (write-string " Iris..."))
  (he:set-iris he:*left-lens* (eye-pos-iris pos))
  (when verbose (write-string " Zoom..."))
  (he:set-zoom he:*left-lens* (eye-pos-zoom pos))
  (when verbose (write-string " Focus..."))
  (he:set-focus he:*left-lens* (eye-pos-focus pos)))

(defun set-right-eye-pos (pos &optional (verbose nil))
  ;; Ignore the eye slot.
  (when verbose (write-string "Mirrors..."))
  (he:set-right-inner-mirror-angle (eye-pos-inner-mirror pos))
  (he:set-right-outer-mirror-angle (eye-pos-outer-mirror pos))
  (when verbose (write-string " Iris..."))
  (he:set-iris he:*right-lens* (eye-pos-iris pos))
  (when verbose (write-string " Zoom..."))
  (he:set-zoom he:*right-lens* (eye-pos-zoom pos))
  (when verbose (write-string " Focus..."))
  (he:set-focus he:*right-lens* (eye-pos-focus pos)))

(defun set-eye-pos (pos)
  (ecase (eye-pos-eye pos)
    (:left (set-left-eye-pos pos))
    (:right (set-right-eye-pos pos))))

(defstruct he-pos
  tilt
  pan
  left-eye
  right-eye)

(defun save-he-pos ()
  (make-he-pos :tilt (he:get-tilt-angle)
	       :pan (he:get-pan-angle)
	       :left-eye (save-left-eye-pos)
	       :right-eye (save-right-eye-pos)))


(defun set-he-pos (pos &optional (verbose t) (watch nil))
  (when watch (send color:color-screen :start-camera :channel 2))
  (when verbose (format t "~&Tilt..."))
  (he:set-tilt-angle-and-wait (he-pos-tilt pos))
  (when verbose (write-string " Pan..."))
  (he:set-pan-angle-and-wait (he-pos-pan pos))
  (when verbose (format t "~%  Left eye... "))
  (set-left-eye-pos (he-pos-left-eye pos) verbose)
  (when watch (send color:color-screen :start-camera :channel 3))
  (when verbose (format t "~%  Right eye... "))
  (set-right-eye-pos (he-pos-right-eye pos) verbose)
  (when watch (send color:color-screen :stop-camera)))