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

(in-package :mu)

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

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

;; (require "HEAD-EYE" "b:>xbus-devices>interfaces>unet>head-eye>driver")

;;; make sure that package unet is defined before loading code that uses it
(eval-when (compile load eval)
  (if (not (find-package 'unet))
      (defpackage unet)))

(defvar *max-galvo-value* 4095.)
(defvar *half-max-galvo-value* (ceiling *max-galvo-value* 2))

;;; This file contains code for setting the head position and mirror angles on the
;;; Head-Eye system.

;;; Head parameters: pan and tilt
;;; Mirror parameters (galvonometer channels):
;;;	right outer, right inner, left outer, left inner

;;; Right outer....1
;;; Right inner....3
;;;  Left outer....0
;;;  Left inner....2
;;;
;;; Use the constants he::*right-outer-galvo*, he::*right-inner-galvo*,
;;; he::*left-outer-galvo*, he::*left-inner-galvo* rather than hardwiring the
;;; channel values into the code.
;;;
;;; Camera parameters: iris, focus, zoom

;;; In case everything freezes up, try (unet::micro-init).

;;;******************************************************************************************
;;; structures

(defstruct EYE
  inner-mirror
  outer-mirror
  iris
  focus
  zoom)

(defstruct HEAD
  pan
  tilt)

(defstruct TRACKER
  (head (make-head) :type head)
  (left-eye (make-eye) :type eye)
  (right-eye (make-eye) :type eye))

;;;******************************************************************************************
;;; head and mirrors

;;; set the head and mirror positions to match the specified values
;(defun SET-HEAD-AND-MIRROR-POSITIONS
;       (&optional
;	(head-pan *head-pan*)
;	(head-tilt *head-tilt*)
;	(right-outer-galvo-count *right-outer-galvo-count*)
;	(right-inner-galvo-count *right-inner-galvo-count*)
;	(left-outer-galvo-count *left-outer-galvo-count*)
;	(left-inner-galvo-count *left-inner-galvo-count*))
;  (set-head-position head-pan head-tilt)
;  (set-mirror-positions (list right-outer-galvo-count right-inner-galvo-count
;			      left-outer-galvo-count left-inner-galvo-count))
;;  (update-head-and-mirror-positions)		;update globals
;  (format t "~%Moving head and mirrors to desired position...")
;  (print-head-and-mirror-positions)
;  (format t "~%Done."))

;(defmacro SET-HEAD-AND-MIRROR-POSITIONS-FROM-LIST (position-list)
;  `(apply #'set-head-and-mirror-positions ,position-list))

;(defmacro SET-HEAD-AND-MIRROR-POSITIONS-FROM-DISK (&rest rest)
;  `(read-head-and-mirror-positions-from-disk ,@rest :move-head-and-mirrors t))

;(defun SAVE-HEAD-AND-MIRROR-POSITIONS-TO-DISK
;       (&key (directory *calibration-data-directory*)
;	(filename "head-and-mirror-positions"))
;  ;; Update head and mirror position globals and print them out.
;  (print-head-and-mirror-positions)
;  (let ((pathname (string-append directory filename ".bin")))
;    (format t "~%Saving head and mirror positions to file ~S..." pathname)
;    (sys:dump-forms-to-file
;      (fs:pathname pathname)
;      (list `(setq *head-pan* `,*head-pan*
;		   *head-tilt* `,*head-tilt*
;		   *right-outer-galvo-count* `,*right-outer-galvo-count*
;		   *right-inner-galvo-count* `,*right-inner-galvo-count*
;		   *left-outer-galvo-count* `,*left-outer-galvo-count*
;		   *left-inner-galvo-count* `,*left-inner-galvo-count*)
;	    `(print-head-and-mirror-positions))
;      `(:package mu))
;    (format t "~%Done.")))

;(defun READ-HEAD-AND-MIRROR-POSITIONS-FROM-DISK
;       (&key (directory *calibration-data-directory*)
;	(filename "head-and-mirror-positions") move-head-and-mirrors)
;  (let ((pathname (string-append directory filename ".bin")))
;    (format t "~%Reading head and mirror positions from file ~S..." pathname)
;    (load pathname))
;  (if move-head-and-mirrors
;      (set-head-and-mirror-positions *head-pan*
;				       *head-tilt*
;				       *right-outer-galvo-count*
;				       *right-inner-galvo-count*
;				       *left-outer-galvo-count*
;				       *left-inner-galvo-count*)
;      (format t "~%To move the head/mirrors to this position, use set-head-and-mirror-positions or call this function with :move-head-and-mirrors t."))
;  (list *head-pan*
;	*head-tilt*
;	*right-outer-galvo-count*
;	*right-inner-galvo-count*
;	*left-outer-galvo-count*
;	*left-inner-galvo-count*))

;(defun HEAD-AND-MIRROR-POSITIONS ()
;;  (update-head-and-mirror-positions)
;  (list *head-pan*
;	*head-tilt*
;	*right-outer-galvo-count*
;	*right-inner-galvo-count*
;	*left-outer-galvo-count*
;	*left-inner-galvo-count*))

;(defun PRINT-HEAD-AND-MIRROR-POSITIONS ()
;  (print-head-position)
;  (print-mirror-positions))

;;;******************************************************************************************
;;; head only

;(defun PRINT-HEAD-POSITION ()
;;  (update-head-position)			;make sure that globals are correct
;  (format t "~%Head pan is ~D, head tilt is ~D." *head-pan* *head-tilt*))

;;;******************************************************************************************
;;; mirrors only

(defun INCREMENT-GALVO-POSITION (galvo-chan increment)
  (let ((galvo-position (galvo:galvo-position galvo-chan)))
    (galvo:set-position (+ galvo-position increment) galvo-chan)))

(defmacro DECREMENT-GALVO-POSITION (galvo-chan increment)
  `(increment-galvo-position ,galvo-chan (- ,increment)))

(defun LIST-OF-GALVO-COUNTS ()
  (galvo:galvo-position he::*right-outer-galvo*)
  (galvo:galvo-position he::*right-inner-galvo*)
  (galvo:galvo-position he::*left-outer-galvo*)
  (galvo:galvo-position he::*left-inner-galvo*))

(defun SET-MIRROR-POSITIONS (list-of-galvo-counts)
  (galvo:set-position (first list-of-galvo-counts) he::*right-outer-galvo*)
  (galvo:set-position (second list-of-galvo-counts) he::*right-inner-galvo*)
  (galvo:set-position (third list-of-galvo-counts) he::*left-outer-galvo*)
  (galvo:set-position (fourth list-of-galvo-counts) he::*left-inner-galvo*))

(defun SET-INNER-MIRRORS-45-DEGREES ()
  (galvo:set-position *half-max-galvo-value* he::*right-inner-galvo*)
  (galvo:set-position *half-max-galvo-value* he::*left-inner-galvo*))

(defun SET-OUTER-MIRRORS-45-DEGREES ()
  (galvo:set-position *half-max-galvo-value* he::*right-outer-galvo*)
  (galvo:set-position *half-max-galvo-value* he::*left-outer-galvo*))

(defun SET-ALL-MIRRORS-45-DEGREES ()
  (set-inner-mirrors-45-degrees)
  (set-outer-mirrors-45-degrees))

(defun CHANGE-FIXATION-DISTANCE (counts-inward-of-current-counts)
  (decrement-galvo-position he::*left-outer-galvo* counts-inward-of-current-counts)
  (increment-galvo-position he::*right-outer-galvo* counts-inward-of-current-counts))

(defun CHANGE-FIXATION-DIRECTION (counts-inward-of-current-counts)
  (increment-galvo-position he::*left-outer-galvo* counts-inward-of-current-counts)
  (increment-galvo-position he::*right-outer-galvo* counts-inward-of-current-counts))

(defun CHANGE-VERTICAL-DISPARITY (counts-inward-of-current-counts)
  (increment-galvo-position he::*left-inner-galvo* counts-inward-of-current-counts)
  (increment-galvo-position he::*right-inner-galvo* counts-inward-of-current-counts))

(defun CHANGE-VERTICAL-DIRECTION (counts-inward-of-current-counts)
  (decrement-galvo-position he::*right-inner-galvo* counts-inward-of-current-counts)
  (increment-galvo-position he::*left-inner-galvo* counts-inward-of-current-counts))

;;; Grab a frame from the selected channel, then draw crosshairs if appropriate.
;;; Called from set-mirrors-by-hand only.
(defun SHOW-FRAME-AND-CROSSHAIRS (channel crosshairs? window)
  (send spe:*color-screen* :start-camera :channel channel)
  (send spe:*color-screen* :stop-camera)
  (if crosshairs? (draw-crosshairs window)))

#||
;;; This is what I'd like to implement below.
~%      P ==> PRECISE positioning~%~%For iris, focus, and zoom, the default is to change both eyes.  CONTROL => left eye only, META => right eye only.~% i or I ==> change IRIS~% f or F ==> change FOCUS~% z or Z ==> change ZOOM~%~%      
||#

(defun TELL-USER-HOW-TO-SET-MIRRORS-BY-HAND ()
  (format t "~%~%Legend:

 v or V ==> change VERGENCE ANGLE
 s or S ==> change VERTICAL SKEW
      u ==> move gaze UP
      d ==> move gaze DOWN
      l ==> move gaze LEFT
      r ==> move gaze RIGHT

Change the selected eye:
      L ==> select left eye
      R ==> select right eye

Change the selected view(s):
      CONTROL-l ==> left view (and left eye)
      CONTROL-r ==> right view (and right eye)
      b ==> both views

Change the color map:
      n ==> normal
      w ==> weird (for setting the iris)

Adjust lens with keys:  
      RIGHT-SHIFT = zoom in, RIGHT-META = zoom out, 
      RIGHT-SYMBOL and RIGHT-SUPER adjust focus,  
      REPEAT = open iris, RIGHT-HYPER = close iris.

      c ==> COARSE positioning~%      f ==> FINE positioning

      HELP ==> reprint instructions
      q ==> QUIT~%~%"))

;;; WEG 10/13/87 - changed &optional to &keyword, added choice of whether to alternate
;;; views or show only one and whether to show crosshairs
(defun SET-MIRRORS-BY-HAND (&key reset-to-2048? (increment 20) (show :both) (crosshairs? t))
  (tell-user-how-to-set-mirrors-by-hand)
  (let ((unison-movement-increment increment)
	(opposing-movement-increment (floor increment 2))
	(eye he::*left-eye*))
    (if reset-to-2048?
	(set-all-mirrors-45-degrees))
    (grey:with-grey-screen-protected (spe:*color-screen* :window window)
      (loop until (tv:key-state #\q) do
	(if (or (eql show :both) (eql show :left))
	    (show-frame-and-crosshairs (he::eye-camera-channel he::*left-eye*) crosshairs? window))
	(if (or (eql show :both) (eql show :right))
	    (show-frame-and-crosshairs (he::eye-camera-channel he::*right-eye*) crosshairs? window))
	(he::read-keys-and-move-lens eye)
	(cond ((and (tv:key-state #\b) (not (eq show :both)))
	       (format t "~%Showing both views.") (setq show :both))
	      ((and (tv:key-state :shift) (tv:key-state #\v))	;V = vergence
	       (change-fixation-distance (- opposing-movement-increment)))
	      ((tv:key-state #\v)
	       (change-fixation-distance opposing-movement-increment))
	      
	      ;; l = move gaze left, L = select left eye, CONTROL-L = select left view
	      ((tv:key-state #\l)
	       (cond ((tv:key-state :shift)
		      (cond ((eq eye he::*right-eye*)
			     (format t "~%Selecting the left eye.")
			     (setq eye he::*left-eye*))))
		     ((tv:key-state :control)
		      (cond ((not (eq show :left))
			     (format t "~%Selecting the left view and left eye.")
			     (setq show :left)
			     (setq eye he::*left-eye*))))
		     (t (change-fixation-direction unison-movement-increment))))
	      
	      ;; r = move gaze right, R = select right eye, CONTROL-R = select right view
	      ((tv:key-state #\r)
	       (cond ((tv:key-state :shift)
		      (cond ((eq eye he::*left-eye*)
			     (format t "~%Selecting the right eye.")
			     (setq eye he::*right-eye*))))
		     ((tv:key-state :control)
		      (cond ((not (eq show :right))
			     (format t "~%Selecting the right view and right eye.")
			     (setq show :right)
			     (setq eye he::*right-eye*))))
		     (t (change-fixation-direction (- unison-movement-increment)))))
	      
	      ((and (tv:key-state :shift) (tv:key-state #\s))	;S = skew
	       (change-vertical-disparity opposing-movement-increment))
	      ((tv:key-state #\s)
	       (change-vertical-disparity (- opposing-movement-increment)))
	      ((tv:key-state #\u)
	       (change-vertical-direction (- unison-movement-increment)))
	      ((tv:key-state #\d)
	       (change-vertical-direction unison-movement-increment))
	      ((tv:key-state #\help) (tell-user-how-to-set-mirrors-by-hand))
	      ((tv:key-state #\n) (spe:linear-map))
	      ((tv:key-state #\w) (spe:inverse-spectral-map :white 255))
	      ((tv:key-state #\c)
	       (setq unison-movement-increment 40 opposing-movement-increment 20)
	       (format t "~%COARSE"))
	      ((tv:key-state #\f)
	       (setq unison-movement-increment 10 opposing-movement-increment 5)
	       (format t "~%FINE")))))
    (zl:send *terminal-io* :clear-input))
  nil) ; <-- It is important to return NIL, believe me.  Why?  I can't remember.

;;; Evaluate this into the buffer:
(defun PRINT-GALVO-COUNTS (&optional (your-own-format-string ""))
  (format nil "~%;;;  ~a~%;;;  *right-outer-galvo-count* = ~a~%;;;  *right-inner-galvo-count* = ~a~%;;;  *left-inner-galvo-count* = ~a~%;;;  *left-outer-galvo-count* = ~a" your-own-format-string *right-outer-galvo-count* *right-inner-galvo-count* *left-inner-galvo-count* *left-outer-galvo-count*))

;;;  Example:  for the table top...
;;;  *right-outer-galvo-count* = 3038
;;;  *right-inner-galvo-count* = *left-inner-galvo-count* = 2258
;;;  *left-outer-galvo-count* = 2318

"
;;;  feb 19 1987
;;;  *right-outer-galvo-count* = 2088
;;;  *right-inner-galvo-count* = 2088
;;;  *left-inner-galvo-count* = 2568
;;;  *left-outer-galvo-count* = 808"

(defun PRINT-MIRROR-POSITIONS ()
;  (update-mirror-positions)			;make sure globals are up to date
  (format t "~%Mirror positions are as follows:")
  (format t "~%    right outer = ~D" *right-outer-galvo-count*)
  (format t "~%    right inner = ~D" *right-inner-galvo-count*)
  (format t "~%    left outer = ~D" *left-outer-galvo-count*)
  (format t "~%    left inner = ~D" *left-inner-galvo-count*))

;(defun SET-LEFT-OUTER-MIRROR (value)
;  (setq *left-outer-galvo-count* value)
;  (galvo:set-position value *left-outer-galvo*)
;  (zl:process-sleep 60)
;  (grey::grab-frame 0 0 nil nil nil))

;(defun GIVE-INSTRUCTIONS-FOR-ESTABLISHING-INITIAL-HEAD-POSITION ()
;  (if (not zl-user::mouse-window) (zl-user::make-mouse-window))
;  (send zl-user::mouse-window ':expose-near '(:point 800 100))
;  (format
;    zl-user::mouse-window
;    "~& Establishing the initial head position:  ~2% There is a piece of red tape near the base of ~& the right-hand edge of the wall to your right.  ~& Move the head so as to place the image of that ~& piece in the upper right-hand corner of your ~& color-display screen.  ~& Use the SUPER, HYPER, META, and SYMBOL keys ~& to move the head.  "))

;;; Since track-with-head-only no longer uses a special mouse window, instructions
;;; can be sent directly to Lisp Listener output.  (WEG 1/8/88)
(defun GIVE-INSTRUCTIONS-FOR-ESTABLISHING-INITIAL-HEAD-POSITION ()
  (format t
    "~&Establishing the initial head position: ~2%There is a piece of red tape near the base of the right-hand edge of the wall to your right.  ~&Move the head so as to place the image of that piece in the upper right-hand corner of your color-display screen.  ~&Use the SUPER, HYPER, META, and SYMBOL keys to move the head.~%"))

;;;******************************************************************************************
;;; Hacks to save and restore position.  The servo loops for iris, focus, and zoom are not
;;; very precise.  Compensate by running them repeatedly until we get the right answer.
;;; I should really use defstruct here, but I don't have time to worry about this.

;;; Compensate for broken servo loop by running the command repeatedly.
(defun SET-MOTOR-POSITION (set-function read-function lens desired-position
			   &key (repeat 10) (initial-tolerance 0)
			   &aux (tolerance initial-tolerance))
  (format t "~%")
  (catch 'done					;exit when finished
    (loop do
      (dotimes (i repeat)			;with a fixed tolerance
	(let ((current-position (funcall read-function lens)))
	  (format t "~D " current-position)
	  ;; If the difference between the current position and the desired position is too great, try again.
	  (cond ((> (abs (- current-position desired-position)) tolerance)
		 (zl:process-sleep 10)		;pause to give the motors a little rest
		 (funcall set-function lens desired-position))
		(t (throw 'done nil)))))
      ;; increase the tolerance and try again
      (setq tolerance (if (zerop tolerance) 1 (1+ tolerance)))
      )))

;;; Parameters: pan, tilt, mirrors; iris, focus, zoom for both lenses.
(defun READ-HEAD-EYE-POSITION ()
  (list (he::get-left-eye-position)
	(he::get-right-eye-position)
	(he::get-pan-angle)
	(he::get-tilt-angle)
	(he::read-iris he::*left-lens*)
	(he::read-focus he::*left-lens*)
	(he::read-zoom he::*left-lens*)
	(he::read-iris he::*right-lens*)
	(he::read-focus he::*right-lens*)
	(he::read-zoom he::*right-lens*)))

;;; If precise is t, force the iris, focus, and zoom to be correct by running the
;;; commands repeatedly.
(defun SET-HEAD-EYE-POSITION (list &key (precise t))
  (declare (ignore precise))			;pao 4/14/89 16:52:09
  (apply #'he::set-left-eye-position (first list))
  (apply #'he::set-right-eye-position (second list))
  (he::set-pan-angle (third list))
  (he::set-tilt-angle (fourth list))
  
;  (set-motor-position #'he::set-iris #'he::read-iris he::*left-lens* (fifth list))
;  (set-motor-position #'he::set-focus #'he::read-focus he::*left-lens* (sixth list))
;  (set-motor-position #'he::set-zoom #'he::read-zoom he::*left-lens* (seventh list))
  
;  (set-motor-position #'he::set-iris #'he::read-iris he::*right-lens* (eighth list))
;  (set-motor-position #'he::set-focus #'he::read-focus he::*right-lens* (ninth list))
;  (set-motor-position #'he::set-zoom #'he::read-zoom he::*right-lens* (tenth list))
  )

;(defun SET-HEAD-EYE-POSITION (list &key (precise t))
;  (apply #'he::set-left-eye-position (first list))
;  (apply #'he::set-right-eye-position (second list))
;  (he::set-pan-angle (third list))
;  (he::set-tilt-angle (fourth list))
;
;  (he::set-iris he::*left-lens* (fifth list))  
;  (he::set-focus he::*left-lens* (sixth list))  
;  (he::set-zoom he::*left-lens* (seventh list))
;
;  (he::set-iris he::*right-lens* (eighth list))  
;  (he::set-focus he::*right-lens* (ninth list))  
;  (he::set-zoom he::*right-lens* (tenth list)))