;; -*- Mode: LISP; Syntax: Common-Lisp; Package: (USER); -*-
;***************************************************************
;*
;*  3-D GEOMETRY FOR LOW-LEVEL TURN-TAKING SYSTEM
;*
;*  Kris R. Thorisson
;*  
;*  HISTORY: 
;*  Begin 7/7 '94, in MCL
;*  Sept 28 - move to UNIX
;*  Feb '95 - calibration routines added
;*  May '95 - debugging gaze, body, head, routines
;*
;***************************************************************
;*
;* Explanation
;*   Contains the geometric analysis of the body for turn-taking.
;*

(in-package 'USER)

;(require "/ahi/ahi2/users/kris/3DGeo.lisp")
;(use-package 'cl-user)


;*********************
;*  SCREEN CLASS     *
;*********************

;The screen class contains a center in addtion...
 
(defclass CPlane (Explicit-Plane)
  (
   (center :accessor center :initarg :center :initform (vector 0.0 0.0 0.0))
   )
  )

(defun make-CPlane (offset dir1 dir2)
  "Return an explicit plane."
  (make-instance 'CPlane :direction1 dir1 :direction2 dir2 :offset offset))

;*************************
;*      ENVIRONMENT      *
;*************************
;                                     OFFSET              DIRECTION
(setf trunk-normal (make-line (vector 0.0 0.0 0.0)(vector 4.0 0.5 0.5))
      head-normal  (make-line (vector 0.0 0.0 0.0)(vector 1.0 0.0 0.0))
      eye-normal   (make-line (vector 0.0 0.0 0.0)(vector 1.0 2.0 0.0)))

 ;Initial values pos vectors for agent screen corners. (in tens of mm)
;(defvar a2 (vector 1560 860  550))
;(defvar b2 (vector 1200 1000 550))
;(defvar c2 (vector 1200 1000 830))

; NEW VALUES IN 320 ;2/12/96
;(defvar a2 (vector 1530 820 550))
;(defvar b2 (vector 1190 930 550))
;(defvar c2 (vector 1190 930 830))

(setf agent-screen (make-instance 'CPlane 
                     :offset (vector 1.0 0.0 0.0)   
                     :direction1 (vector 0.0 1.0 0.0)   
                     :direction2 (vector 0.0 0.0 1.0)
                     :center     (vector 1.0 0.5 0.5)))

 ;Initial values pos vectors for work screen corners. (in thousands of mm)
;(defvar a1  (vector 1967.5 -649.0 425.2))
;(defvar b1  (vector 1961.0 706.5 421.6))
;(defvar c1  (vector 1964.3 708.6 1459.0))
;(defvar ctr (vector 3930.15 58.55 1365.5))

; NEW VALUES IN 320 ;2/12/96
(defvar a1  (vector 1830.0 -641.52  425.2))
(setf   a1  (vector 1830.0 -641.52  425.2))
(defvar b1  (vector 1830.0  765.22  421.6)) ;y used to be 706.5; Current values are from J. Bers
(setf   b1  (vector 1830.0  765.22  421.6)) ;y used to be 706.5; 2/15 1996
(defvar c1  (vector 1830.0  765.03 1459.0)) ;y used to be 708.5; Don't know why I had 708 before.
(setf   c1  (vector 1830.0  765.03 1459.0)) ;y used to be 708.5; 


(setf work-screen (make-instance 'CPlane 
                      :offset nil
                      :direction1 nil
                      :direction2 nil
                      :center nil))

(setf w-s work-screen)

;********************
;********************
#|
                                    Work Screen       
                                            /|
                                          /  |
                                        /    |
                          agent-      /      | 
                          screen     |c1     |
                         c--------   |       |
         Z               |  o o   |  |       |
         ^               |   |    |  |       |
         |    Y          |  <_>   |  |       |
         |   /|          b--------a  |b1     |
         |  /                         \      |
         |_/                            \    |
       /___/|                             \  |
      |    ||----> X                        \|a1 
      |    |/ 
       ----
                                             
|#
;********************
;*   AGENT SCREEN   *
;********************

(defvar cali-stream nil)

(defun calibrate-agent-screen ()
  "This one expects pre-measured values."
  (let ((vector1 (vector 1530.0 820.0 550.0)) ;vector source-cube->point-a on screen
        (vector2 (vector 1190.0 930.0 550.0)) ;vector source-cube->point-b on screen
        (vector3 (vector 1190.0 930.0 830.0)) ;vector source-cube->point-c on screen
        (stop nil))
    (let ((a vector1)
	  (b (vector-sub vector2 vector1))
	  (c (vector-sub vector3 vector2)))
      (setf agent-screen (make-CPlane a b c))
      (setf (center agent-screen) (plane-center agent-screen))      
      )
    (print "Agent screen:") (describe agent-screen)
    ))

(defun calibrate-agent-screen2 ()
  "This one expects a connection through a socket."
  (let ((vector1 nil)
        (vector2 nil)
        (vector3 nil)
        (stop nil))
    (if (y-or-n-p "Need to re-open socket?")
	(setf cali-stream (open-calibration-socket)))
    (do ((foo 1 1))(stop)
      (princ "Place sensor cube at Lower Left corner of agent screen & hit RETURN.")
      (read-line)
      (setf vector1 (read-cal-data cali-stream))
      (princ "Vector 1: ")(princ vector1)(terpri)
      (princ "Place it at the Lower Right & hit RETURN.") (read-line)
      (setf vector2 (read-cal-data cali-stream))
      (princ "Vector 2: ")(princ vector2)(terpri)
      (princ "Place it at the Upper Right & hit RETURN.") (read-line)
      (setf vector3 (read-cal-data cali-stream))
      (princ "Vector 3: ")(princ vector3)(terpri)
      (let ((a vector1)
            (b (vector-sub vector2 vector1))
            (c (vector-sub vector3 vector2)))
        (setf agent-screen (make-CPlane a b c))
        (setf (center agent-screen) (plane-center agent-screen))      
        )
      (print "Agent screen:") (describe agent-screen)
      (unless (y-or-n-p "Redo (y/n)?") (setf stop t)))
    ))

(defun read-cal-data (socket)
  "If you need to read cube data from body model."
  (let ((values (make-array '(3))))
    (dotimes (count 4 1)
      (send-item 1 socket)
      (read-body-record))
    (describe (read-body-record))
    (setf temp (RA (body-data *socket-obj1*)))
    (print (stamp (body-data *socket-obj1*)))
    (print (head (body-data *socket-obj1*)))
    (dotimes (count 3 values)
      (setf (aref values count) (aref temp (+ 11 count))))))


;********************
;*   WORK SCREEN    *
;********************

(defvar *big-screen-center* nil)

(defun calibrate-work-screen ()
  (let ((v1 a1)
	(v2 b1)
	(v3 c1))
    (let* ((a v1)
	   (b (vector-sub v2 v1))
	   (c (vector-sub v3 v2)))
      (setf work-screen (make-CPlane a b c))
      (setf (center work-screen) (plane-center work-screen))))
  (setf *big-screen-center* (center work-screen))
  (print "WORK screen:") (describe work-screen))


;***********************************
;* INTERACTION-SPECIFIC GEOMETRY   *
;***********************************

(defmethod boolean-intersect ((line Line)(plane CPlane))
  "Returns a T if the line intersects the plane."
  (multiple-value-bind (wb wc)
                       (intersect line plane)
    (if (or (< wb 0) (> wb 1))      ;If the first scalar is off,
      nil                           ;return nil, else,
      (if (and (> wc 0) (< wc 1))   ;if the second is in (more than 0.0),
        T                           ;return T, otherwise
        nil))))                     ;return nil.

(defmethod line-plane-distance ((line Line)(plane CPlane))
  "Returns the shortest distance from the line to the plane's centerpoint."
  (let* ((i-point (intersect-to-point line screen))
         (plane-dir (vector-sub (center screen) i-point))    ;This is the hypotenuse.
         (angle (angle-between plane-dir (direction line)))
         (length (* (vector-length plane-dir) (sin angle))))  
    length))

(defmethod analog-facing ((line Line)(plane CPlane))
  "Returns the shortest dist from line to plane's center + angle in degs."
  (let ((dist (line-screen-distance line plane))
        (angle (rad-to-deg (angle-between line plane))))
    (if (= 0 (round angle))  ;If the angle is less than 0.5 deg.
      (setf dist (vector-length (vector-sub (offset line) (center plane)))))
    (values dist (round angle))
    ))


;********************************
;*  KNOWLEDGE-BASED FUNCTIONS   *
;********************************

;These would be functions like "facing-me? X" and "turned-to-me? X" 
; and "looking-at-me? X".  Or: "Facing x y" etc.

(defvar generic-limit 40 "Generic cone limit, in degs.")
(defvar head-limit 35 "Limit of the head cone, in degs.")  
(defvar trunk-limit 40 "Limit of the body cone, in degs.")
(defvar gaze-limit 20 "Limit of gaze, in degs.") ;a bit too narrow?

;All measures in mm.
(defvar gest-space-limit 300 "Limit of the gest-space sphere's radius, in mm.")
(defvar gest-space-offset 400 "Offset from the front plane of the trunk, in mm.")
(defvar gest-plane-offset 100 "Offset of gest-plane from front plane of trunk, in mm.")
(defvar hand-sphere-radius 180 "Sphere around wrist, defining coarse hand position, in mm.")  

(defmethod looking-at ((line Line)(plane CPlane))
  "Returns T if line (=gaze) intersects other plane."
  (boolean-intersect line plane))  ;Strictest of them.

;How about a function called "Foveates (line point)"
;that would return T if point falls within 2 deg radius of line.

#|
(defmethod intersect ((line Line)(plane CPlane))
  "Returns T if line intersects plane at a generic-limit angle."
  (let* ((diff (vector-sub (center plane)(offset line)))
        (angle (rad-to-deg (angle-between diff (direction line)))))
    (if (> angle generic-limit) 
      nil  ;else...
      T)
    ))
|#

(defmethod my-intersect ((line Line)(plane CPlane) angle-limit)
  "Returns T if center of plane is inside cone given by the angle limit."
  (let* ((diff (vector-sub (center plane)(offset line)))
        (angle (rad-to-deg (angle-between diff (direction line)))))
    (if (> angle angle-limit) 
      nil  ;else...
      T)
    ))

(defmethod 2D-intersect ((line Line)(plane CPlane) angle-limit)
  "Returns T if line intersects plane at the angle limit."
  (let* ((diff (vector-sub (center plane)(offset line)))
        (angle (rad-to-deg (angle-between diff (direction line)))))
    (if (> angle angle-limit) 
      nil  ;else...
      T)
    ))

(defmethod looking-me ((line Line)(plane CPlane))
;  (let ((dir (vector-reflect (direction line)))) ;I thought the cube was wrong...
;    (setf (direction line) dir)
    (my-intersect line plane gaze-limit)
  )

(defmethod my-intersect (first second third) ;A catch-all method
  ())

#|
(defmethod facing ((line Line)(plane CPlane))
  "Returns T if line (=head) is facing plane at a head-limit angle."
  (let* ((diff (vector-sub (center plane)(offset line)))
         (angle (rad-to-deg (angle-between diff (direction line)))))
    (if (> angle head-limit) 
      nil  ;else...
      T)
    ))
|#

(defmethod facing ((line Line)(plane CPlane))
;  (print 'offset)  (princ (offset line))
;  (print 'dir)     (princ (direction line))
  (my-intersect line plane head-limit))

;#|
(defmethod turned-to ((line Line)(plane CPlane))
  "Returns T if line (=trunk) is turned toward plane."
  (let* ((diff (vector-sub (center plane)(offset line)))
        (angle (rad-to-deg (angle-between diff (direction line)))))
    (if (> angle trunk-limit) 
      nil    ;else...
      T)
    ))
;|#

(defmethod turned-my-way ((plane CPlane)(line Line))
  (my-intersect line plane trunk-limit))

;(defmethod turned-to ((line1 Line)(line2 Line))
;  (print line1)(print line2)
;  (read-line))
;  )

#| 
(defmethod hand-in-gesture-space ((trunk Line)(hand-pos array))
  "Returns nil if hand is outside sphere or behind trunk-offset plane (gest-plane)."
  (print hand-pos)
;  (print (setf gest-space (offset trunk)))
  (let* ((dist (distance-between hand-pos gest-space))
         (v1 (vector-scale (direction trunk) gest-plane-offset)) ;offset along trunk normal.
         (v2 (vector-sub v1 (offset trunk))) ;Center of gest-plane vector.
         (v3 (vector-sub v1 hand-pos))) ;pointing from center of gest-plane to hand-pos.
    (print 'v1-)(princ v1)(print 'v2-)(princ v2)(print 'v3-)(princ v3)
    (if (or (> dist gest-space-limit)
 ;are hands behind the gest-plane that's parallel but offset from trunk front plane?
            (> (print (angle-between v3 (direction trunk))) 90))
      nil
      T)
    ))
|#

;;New one, 4/19/95
(defmethod hand-in-gesture-space ((trunk Line)(hand-pos array))
  "Returns nil if hand is outside sphere or behind trunk-offset plane (gest-plane)."
;  (print hand-pos)
;  (print (direction trunk))
;  (print gest-space-offset)
;  (print (offset trunk))
;  (print (vector-scale (direction trunk) gest-space-offset))
  (let* ((gest-space (vector-add 
                      (vector-scale (direction trunk) gest-space-offset)
                      (offset trunk)))
         (dist (distance-between hand-pos gest-space))
         (vec1 (vector-scale (direction trunk) gest-plane-offset)) ;offset along trunk normal.
         (vec2 (vector-add vec1 (offset trunk))) ;Center of gest-plane vector.
         (vec3 (vector-sub hand-pos vec2)))      ;pointing from center of gest-plane to hand-pos.
;    (print 'v1-)(princ vec1)(print 'v2-)(princ vec2)(print 'v3-)(princ vec3)
;    (print 'gest-space-)(princ gest-space)(print 'dist-)(princ dist)
;    (print (rad-to-deg (angle-between vec3 (direction trunk))))
    (if (or (> dist gest-space-limit)
 ;are hands behind the gest-plane that's parallel but offset from trunk front plane?
	    (> (rad-to-deg (angle-between vec3 (direction trunk))) 90.0))
        nil
      T)
    ))

(defmethod looking-hands ((gaze Line)(hand-pos array))
  "Returns nil if line-of-gaze falls outside of hand-sphere."
  (let* ((eye-to-wrist (vector-sub (offset gaze) hand-pos))
         (angle (angle-between eye-to-wrist (direction gaze)))
         (lengt (vector-length eye-to-wrist))
         (dist (* 2 (sin angle) lengt)))
;    (print dist)
    (if (> dist hand-sphere-radius)
        nil
      T)
    ))

;On every cycle of the dialogue knowledge base, the position of gesture
;space, hands and head should be calculated and/or updated.


;;
;; DEICTIC GESTURE SKETCHING/ANALYSIS
;; 5/25/96
;;

(defmethod deictic-sketch ((arm array)(trunk Line))
  (let ((hand-pos (get-body-part wrist-pos-index arm))
        (elbow-angle (get-elbow-angle arm)))
    (if (and (hand-above-trunk-midplane hand-pos trunk)
             (arm-straight elbow-angle 0)) ;0 is foo
        T
      nil)
    ))

(defmethod hand-above-trunk-midplane ((hand-pos array)(trunk Line))
  "Returns T if hand is above a plane cutting through the middle of the trunk."
  (let* ((gest-space-center 
          (vector-add 
	   (vector-scale (direction trunk) gest-space-offset)
	   (offset trunk)))
         (hand-height (aref hand-pos 2))
         (trunk-plane (aref gest-space-center 2)))
    (if (> hand-height trunk-plane)
        T
      nil)
    ))

(setf elbow-straight-threshold 550) ;elbow angle runs from 1300 to 20 (elbow straight)

(defmethod arm-straight (angle foo)
  (if (< angle elbow-straight-threshold)
      T
    nil))


;;
;;
;;  GEOMETRIC OUTPUT FUNCTIONS
;;
;;       begin: 2/4/1996
;;
;;


(defmethod point-plane-at-obj ((plane CPlane)(obj-pos Vector))
  "Returns two angles, one for each dimension that a plane 
   has to be rotated through to be 'facing' a point in 3space."
  (let ((v1 (copy-seq (direction1 plane)))
        (v2 (copy-seq (direction2 plane)))
        (plane-ctr-to-obj-pos (vector-sub (center plane) obj-pos)))
  (list (round (- (rad-to-deg (angle-between v1 plane-ctr-to-obj-pos)) 90)) ;left of screen = +
        (round (- (rad-to-deg (angle-between v2 plane-ctr-to-obj-pos)) 90)) ;below screen = neg
        )
  ))

(defmethod look-at-point ((plane CPlane)(obj-pos Vector))
  "Returns two angles, one for each dimension that plane
   has to be rotated through to ve 'facing' a point in 3space."
  ())