;;*******************************************************************************
;;                                                                              *
;; SockRead2.lisp                                                               *
;;                                                                              *
;; Hook up to                                                                   *
;;   1. the body model dispatcher, and                                          *
;;   2. the intonation analysis,                                                *
;;   and read the data into buffers.                                            *
;;                                                                              *
;; K. R. Thorisson, start: May.1996                                             *
;;                  added: data for deictic gesture analysis, full body record. *
;;                                                                              *
;;*******************************************************************************

(in-package 'USER)
(proclaim '(optimize (speed 3)(safety 0)(debug 0)))

;(load "/ahi2/users/kris/io/my-io.lisp")
                                 

;;_______________________________
;; TIME SERVER SYNCHRONIZATION
;;-------------------------------

(defvar splotch-time-sock# 7766)
;(defvar spud-time-sock# xxxxx)

;Make sure the timeserv process is running on the machine called:
; 29/5/95: The modified timeserver is /ahi/timeserver/timeserv2
; To run: splotch> /ahi/timeserver/timeserv2 > /dev/null &
; To kill: su@splotch> ps -efa, then kill #


(defun synchronize (hosts-name host-port)
  (let ((sock (comm:open-tcp-stream "splotch" splotch-time-sock#
				    :element-type 'signed-byte))
        (time-offset nil)
        (local-time nil)
        (time-in nil)
        (mark nil))  ;A time marker to notify client mach. of timegrab.
    (setf *sock* sock)    
    (if sock
        (progn
          (terpri)(princ "SYNCHRONIZING")
          (terpri)(princ "Asking for time ...")
          (bytes-to-stream "cent" sock)
          (loop while (not (listen sock)))    ;loop until host sends "mark" ... and then
          (setf local-time (local-msec-time))   ;read this as soon as that one returns.
          (setf mark (read-bytes sock 1))
          (setf time-in (read-from-string (read-line sock)))
          (terpri)(princ "Local time:     ")(princ local-time)
          (terpri)(princ "Time from host: ")(princ time-in)
          (setf time-offset (- local-time time-in))
          (if time-offset
	      (progn
	        (terpri)(princ "Time offset:    ")(princ time-offset)
                (print 'CLOSING)
                (bytes-to-stream "bye " sock)
		(close-time-sock sock))
	    (princ "Time offset not read correctly.")))
      (princ "TIME port did not open. Make sure timeserv is running."))
    (setf *time-offset* time-offset))
  )

(defun sync ()
  (synchronize "splotch" splotch-time-sock#))
 
(defun close-time-sock (sock)
  (bytes-to-stream "bye " sock)
  (close sock)
  (terpri)(princ "Time socket closed."))

(defun bytes-to-stream (what stream)
  (write what :stream stream)
  (finish-output stream))

;;_______________________________
;; BODY DATA HANDLING
;;-------------------------------

(defvar *valid-body-indexes* '(x-idx y-idx))
(defvar x-idx 0) 
(defvar y-idx 2)

; New format: 6/16 1995
; ---------------------
; Left-Arm: shoulder XYZ, Elevation, Abduction, Twist, normal XYZ, approach XYZ
;           elbow    XYZ, Flex, Twist, normal XYZ, approach XYZ
;           wrist    XYZ, Pitch, Yaw
; L-hand:   palm     XYZ, normal XYZ, approach XYZ (approach=pointing)
;           fingers  arc mcp ip mcp pip mcp pip mcp pip mcp pip ab1 ab2 ab3 ab4
; Right-Ar: ditto
; R-hand:   ditto
; Eye:   XYZ, elev, azim, approach XYZ  (approach=pointing)
; Head : XYZ, elev, abd, twist, normal XYZ, approach XYZ (normal=pointing)
; Torso: XYZ, roll, pitch, yaw, normal XYZ, approach XYZ (normal=pointing)
;
;  *body-buffer* = ((timestamp = 4 bytes)                  ;Each value 2 bytes.
;                   (X Y Z)(E A T)(X Y Z)(F T)(X Y Z)(P Y) ;Left arm
;                   (X Y Z)(E A T)(X Y Z)(F T)(X Y Z)(P Y) ;Right arm
;                   (X Y Z)(E A)(X Y Z)                    ;Eye
;                   (X Y Z)(E A T)(X Y Z)                  ;Head
;                   (X Y Z)(R P Y)(X Y Z))                 ;Torso                  


 ;Used to collate the values in a list.
(defconstant ARM-num-vals   28) 
(defconstant HAND-num-vals  24)
(defconstant EYE-num-vals    8) 
(defconstant HEAD-num-vals  12)
(defconstant TORSO-num-vals 12) 

; Datarecord sizes  ;all sizes in 8-bit bytes
(defconstant TIME-bytes 4)
(defconstant LA-bytes  (* 2 ARM-num-vals))
(defconstant LH-bytes  (* 2 HAND-num-vals))
(defconstant RA-bytes  (* 2 ARM-num-vals))
(defconstant RH-bytes  (* 2 HAND-num-vals))
(defconstant EYE-bytes (* 2 EYE-num-vals))
(defconstant HED-bytes (* 2 HEAD-num-vals))
(defconstant TOR-bytes (* 2 TORSO-num-vals))


 ;Used to specify what parts we want.
(defconstant ALL-data      127)
(defconstant ALL-but-arms  122)
(defconstant ALL-but-hands 117)
(defconstant BODY-w-o-hands 69)
(defconstant TOR-data       64)
(defconstant HEAD-data      32)
(defconstant EYE-data       16)
(defconstant RA-data         4)
(defconstant LA-data         1)

(setf NUM-vals-list (list ARM-num-vals HAND-num-vals 
                          ARM-num-vals HAND-num-vals 
                          EYE-num-vals HEAD-num-vals 
                          TORSO-num-vals))

;For accessing the *body* list
(defconstant STAMP-index 0)
(defvar RA-index 3) ;changed 6/3/96
(defvar LA-index 1)
(defconstant RH-index 1) ;the same as arm
(defconstant LH-index 2) ;the same as arm
(defconstant EYE-index 3)
(defconstant HEAD-index 4)
(defconstant TORSO-index 5)

(defconstant L-wrist 11)
(defconstant R-wrist 27)

;The two port vars.
(defconstant *body-port-#1* 5194) ;base port (connect to first)
(defvar *body-port-#2* nil)       ;real port received

;Freq of body data updates.
(defvar *freq* 1) ; [0=max speed, 1=polled, 2=<frequency]
(defconstant POLLED 1)
(defconstant as-fast-as-possible 0)

(defvar High-vals nil)
(defvar Low-vals  nil)
;(setf *body* (make-instance 'body-data))

;5/23/96 body data is just a list of vectors....
(setf *body* (make-array '(8)))

(defun REFRESH-BODY-DATA ()
  "This function is called from main loop."
  (if (eq *freq* 1) ;Means we're in polled mode.
      (request-record (body-stream *socket-obj1*)))
  (setf *body* (read-body-record (num-vals *socket-obj1*)))
  )

(defun read-body-record (num-val-list)
  "Read all data records from the body and store in a buffer."
  (setf (body-data *socket-obj1*) nil)     ;Empty the buffer.
  (if (listen (body-stream *socket-obj1*))
      (let ((the-stream (body-stream *socket-obj1*))
            (count 1))
        (setf (aref *body* 0) (read-timestamp the-stream TIME-bytes))
	(dolist (value num-val-list)            
	  (setf (aref *body* count) (read-body-vals the-stream value))
	  (setf count (1+ count)))))
  (setf (body-data *socket-obj1*) *body*))

(defun read-body-no-timestamp (num-val-list)
  "Read all data records from the body and store in a buffer."
  (setf (body-data *socket-obj1*) nil)      ;Empty the buffer.
  (if (listen (body-stream *socket-obj1*))
      (let ((the-stream (body-stream *socket-obj1*))
            (count 1))
        (dolist (value num-val-list)
          (setf (aref *body* count) (read-body-vals the-stream value))
          (setf count (1+ count)))))
  (setf (body-data *socket-obj1*) *body*))

(defun read-body-vals (a-stream num-vals)
  "Read values of each element and return as an array."
  (let ((the-array (eval `(make-array '(,num-vals)))))
    (dotimes (count num-vals nil) 
      (setf (aref the-array count)(read-bytes a-stream 2))) ;all datavals are 2 bytes
    the-array))

(defun read-timestamp (stream num-of-bytes) ;each byte = 8 bits
  (let ((in2 (make-array '(1)))
        (in 0)
        (x 0))
    (setf in (read-byte stream))
    (dotimes (count (- num-of-bytes 1) in)
      (setf x (* 256 in))
      (setf in (boole boole-ior (read-byte stream) x)))
    (setf (aref in2 0) in)
    in2))


(defun open-body-socket (read-min-max?)
  (print "Connecting to Body...")
  (let ((a-stream
	 (comm:open-tcp-stream "sprocket" *body-port-#1* 
                               :element-type 'unsigned-byte)))
    (if (streamp a-stream)
        (progn
          (setf body-stream a-stream) ;safety guard
          (setf (body-stream *socket-obj1*) a-stream)
	  (print "Connected.")
	  (setf (body-status *socket-obj1*) t)
          (report "Sending mask: " ALL-data)
          (ask-for-select-parts (body-stream *socket-obj1*) ALL-data)
          (report "Sending frequency: " *freq*)
          (send-frequency (body-stream *socket-obj1*) *freq*)
          )
      (print "Error: Primary body connection did not open!"))
    )
  (if read-min-max?
      (progn
        (print "Reading MIN and MAX values.")
        (sleep 0.1)
        (read-body-limits) ;This reads in the max and min for each bodypart.
        ))
  'OK
  )

(defun open-old-body-socket (read-min-max?)
  (unless (boundp '*socket-obj1*)
    (make-input-sock-obj *socket-obj1*))
  (let ((temp-stream
	 (comm:open-tcp-stream "sprocket" *body-port-#1* 
                               :element-type 'signed-byte)))
    (if (streamp temp-stream)
        (progn
	  (print "Connecting to Body...")
          (let* ((high (read-byte temp-stream))
                 (low  (read-byte temp-stream))
                 (temp-port-# (dpb high (byte 16 8) low))) ;think this should be 8 8
            (setf *body-port-#2* temp-port-#)
            (close temp-stream)
            (if
                (setf (body-stream *socket-obj1*) 
		      (comm:open-tcp-stream "sprocket" *body-port-#2*
                                            :element-type '(unsigned-byte 8)))
                (progn
		  (print "Connected.")
                  (setf body-stream (body-stream *socket-obj1*))
                  (setf (body-status *socket-obj1*) t))
              (print "Error: connection to body failed!"))
            (report "Sending mask: " ALL-but-arms)
	    (ask-for-select-parts (body-stream *socket-obj1*) ALL-but-arms)
            (report "Sending frequency: " *freq*)
	    (send-frequency (body-stream *socket-obj1*) *freq*)
          ))
      (print "Error: Primary body connection did not open!"))
    )
  (if read-min-max?
      (progn
        (print "Reading MIN and MAX values.")
        (sleep 0.1)
        (read-body-limits) ;This reads in the max and min for each bodypart.
        ))
  'OK
  )

(defun close-body-stream ()
  (if (= *freq* POLLED) 
      (send-item 1 (body-stream *socket-obj1*)))
  (if (close (body-stream *socket-obj1*))
      (setf (body-status *socket-obj1*) nil))
  (not (body-status *socket-obj1*)))

(defun read-body-limits ()
  "Reads off the high and low vals transmitted at beginning of session."
  (describe (setf High-vals (read-body-no-timestamp num-vals-list)))
  (describe (setf Low-vals  (read-body-no-timestamp num-vals-list))))

(defun ask-for-select-parts (stream mask)
  (dotimes (count 3 nil)
    (write-byte 0 stream))
  (write-byte mask stream)
  (finish-output stream))

(defun send-frequency (stream freq)
  (write-byte 0 stream) 
  (write-byte freq stream)
  (finish-output stream))


;*******************************************
;*     GETTING  DATA TO THE SENSORS
;*******************************************

 ;First number is the bodypart.
 ;First bodypart is number 1. (timestamp = 0)

(defvar time-stamp '(0 0))
(defvar left-wrist-number-list   '(1 23 24 25))
(defvar right-wrist-number-list  '(2 23 24 25))
(defvar left-hand-number-list  '(3 0 1 2))
(defvar right-hand-number-list '(4 0 1 2))
(defvar left-wrist-pitch-list  '(1 26))
(defvar right-wrist-pitch-list '(2 26))
(defvar left-elbow-angle-list  '(1 15))
(defvar right-elbow-angle-list '(2 15))
(defvar eye-number-list        '(5 0 1 2 5 6 7)) 
(defvar head-number-list       '(6 0 1 2 6 7 8))
(defvar torso-number-list      '(7 0 1 2 6 7 8))

(defvar elbow-angle-index 15)
(defvar obj-pos-index '(0 1 2))
(defvar wrist-pos-index '(23 24 25))

(defun set-keys ()
  (setf time-stamp '(0 0))
  (setf left-arm-number-list   '(1 23 24 25))
  (setf right-arm-number-list  '(3 23 24 25))
  (setf left-hand-number-list  '(2 0 1 2))
  (setf right-hand-number-list '(4 0 1 2))
  (setf left-wrist-pitch-list  '(1 26))
  (setf right-wrist-pitch-list '(3 26))
  (setf left-elbow-angle-list  '(1 15))
  (setf right-elbow-angle-list '(3 15))
  (setf eye-number-list        '(5 0 1 2 5 6 7)) 
  (setf head-number-list       '(6 0 1 2 6 7 8))
  (setf torso-number-list      '(7 0 1 2 6 7 8))
  (defvar hand-pos-index       '(0 1 2))
  (defvar elbow-angle-index 15)
  )

(set-keys)

(defvar vect1 (make-array '(3))) ;global to increase speed.
(defvar vect2 (make-array '(3)))
(defvar vect3 (make-array '(3)))
(defvar vect4 (make-array '(3)))
(defvar elbow-angle 0)

(defvar normalizer 0.0001) ;To get normals into the right range.

(defun get-head-pos ()
  (let* ((the-list head-number-list)
         (body-part (aref *body* (pop the-list))))
    (dotimes (count 3 nil)
      (setf (aref vect1 count) (aref body-part (pop the-list))))
    vect1
    ))

(defun get-head-dir ()
  (let* ((the-list head-number-list)
         (body-part (aref *body* (pop the-list))))
    (dotimes (count 3 nil)
      (setf (aref vect1 count) (aref body-part (pop the-list))))
    (dotimes (count 3 nil)
      (setf (aref vect2 count) (* (aref body-part (pop the-list)) normalizer)))
    (make-line vect1 vect2)))

(defun get-gaze-dir ()
  (let* ((the-list eye-number-list)
         (body-part (aref *body* (pop the-list))))
    (dotimes (count 3 nil)
      (setf (aref vect1 count) (aref body-part (pop the-list))))
    (dotimes (count 3 nil)
      (setf (aref vect2 count) (* (aref body-part (pop the-list)) normalizer)))
    (make-line vect1 vect2)))

(defun get-trunk-dir ()
  (let* ((the-list torso-number-list)
         (body-part (aref *body* (pop the-list))))
    (dotimes (count 3 nil)
      (setf (aref vect1 count) (aref body-part (pop the-list))))
    (dotimes (count 3 nil)
      (setf (aref vect2 count) (* (aref body-part (pop the-list)) normalizer)))
    (make-line vect1 vect2)))

(defun get-l-wrist-pos () ;this is really palm pos
  (let* ((the-list left-hand-number-list)
         (body-part (aref *body* (pop the-list))))
    (dotimes (count 3 nil)
      (setf (aref vect3 count) (aref body-part (pop the-list))))
    vect3))

(defun get-r-wrist-pos () ;this is really palm pos
  (let* ((the-list right-hand-number-list)
         (body-part (aref *body* (pop the-list))))
    (dotimes (count 3 nil)
      (setf (aref vect4 count) (aref body-part (pop the-list))))
    vect4))

(defun get-r-wrist-pitch ()
  (let* ((the-list right-hand-number-list)
         (body-part (aref *body* (pop the-list))))
    (setf (aref vect4 0) (aref body-part (pop the-list)))
    ))

(defun get-r-elbow-angle ()
  (let* ((the-list right-elbow-angle-list)
         (body-part (aref *body* (first the-list))))
    (aref body-part (second the-list))
  ))

(defun get-l-elbow-angle ()
  (let* ((the-list left-elbow-angle-list)
         (body-part (aref *body* (first the-list))))
    (aref body-part (second the-list))
    ))

(defmethod get-elbow-angle ((arm array))
  (aref arm elbow-angle-index))

(defmethod fetchit ((index list) (buff list))
  ;First index is the body part, second the values.
  (aref (aref buff (first index)) (second index)))

(defmethod fetchit ((index list) (buff vector))
  (aref (aref buff (first index)) (second index)))

(defmethod get-body-part ((index integer) (buff array))
  (aref buff index))

(defmethod get-body-part ((index integer) buff) ;in case you don't want to ship the array
  (aref *body* index))

(defmethod get-body-part ((the-list list)(body-part array)) 
  "Receives a body part array and a list of vals positions, 
   returns a vect with those vals."
  (let ((vect (vector 0 0 0)))
    (dotimes (count 3 nil)
      (setf (aref vect count) (aref body-part (pop the-list))))
    vect
    ))

(defun get-body-buffer ()
  "Read one data record from body and store in *body-buffer*."
  ())

(defun debug-elbow ()
  (let ((last-pos T)
        (last-angle 40))
    (loop while (not (read-char-no-hang)) do
	  (refresh-body-data)
          (if (hand-above-trunk-midplane (get-r-wrist-pos)(get-trunk-dir))
              (progn
                (print "")
	        (princ "R hand above! elbow:")
	        (princ (get-r-elbow-angle))
                )
            )
          (if (hand-above-trunk-midplane (get-l-wrist-pos)(get-trunk-dir))
              (progn
                (print "")
	        (princ "L hand above! elbow:")
	        (princ (get-l-elbow-angle))
                )
            )
	  )
    ))

(defun aaa ()
  (loop 
   (refresh-body-data)
   (print (get-l-wrist-pos))
  ))
  
;;_______________________________
;; INTONATION
;;-------------------------------

(defvar *inton-stream* nil)
(defvar *inton-port#* 2001)
(defvar *valid-inton-indexes* '(speech-on-idx inton-dir-idx))
(defvar speech-on-idx 0) ;returns the first two chars.
(defvar inton-dir-idx 2)
(defvar N nil)

; ===
; Intonation data format
;
; Data contains 3 chars 
; first char: T/N for speech on/off
; second char: U/D/N for intonation going Up or going Down. N means not known.
; ===

(defun open-inton-stream ()
  (open-inton-socket))

(defun open-inton-socket ()
  (if
      (setf (inton-stream *socket-obj1*)
            (comm:open-tcp-stream "spork" *inton-port#*))
      (progn 
        (print "Connected to Spork...")
        (setf inton-stream (inton-stream *socket-obj1*))
        (setf (inton-status *socket-obj1*) t))
    (print " Error (open-inton-socket): Spork connection did not open!"))
  )

(defun read-inton-data ()
  "Read all data available in intonation socket and store in a buffer."
;  (setf (inton-data *socket-obj1*) nil)      ;Empty the buffer.
  (let ((sock (inton-stream *socket-obj1*)))
    (loop
     (unless (listen sock) (return))
     (let ((in (coerce (read-line sock) 'list)) temp)
       (if in 
           (progn
;             (print (time-stamp))  ;3/4/96:estimate net-transmission delay = 200-500 ms 
             (setf temp (clean-inton-data in))     ;depending on the quality of the sync
;             (print temp)
	     (push (clean-inton-data in) (inton-data *socket-obj1*))
             ))
       ))
    ))


;; The first version is the current, two-piece message version
(defun clean-inton-data (data)
  
  (list (read-from-string (coerce (read-before data #\Space) 'string))
        (read-from-string (coerce (read-before (read-after data #\Space) #\Return) 'string))
        )
  )


#|
;;This version is for three-piece messages
(defun clean-inton-data (data)

 (list (read-from-string (coerce (read-before data #\Space) 'string))
	(read-from-string (coerce (read-before (read-after data #\Space) #\Space) 'string))
        (read-from-string (coerce (read-before (read-after (read-after 
                                                            data 
							    #\Space)
                                                           #\Space)
					       #\Return) 'string))
        )
  )
|#
(defun get-inton-data (index1)
  (if (member index1 *valid-inton-indexes*)
      (unless (null (eval index1))
        (nth (eval index1) (first (inton-data *socket-obj1*))))
   ; (print index1)
    ))

(defun close-inton-socket ()
  (close-inton-stream))

(defun close-inton-stream ()
  (close inton-stream)
  (setf (inton-status *socket-obj1*) nil)
  T)

;;-------------------------------
;; SPEECH SOCKET
;;-------------------------------

;(defvar speech-port# 16977)
(defvar speech-port# 8008)            ;Gandalf
(defvar speech-host-name "spud")
(setf speech-host-name "spud")
(defvar speech-stream nil)
(defvar S_IDLE 2)
(defvar S_CONTINUOUS 1)
(defvar S_WAITING_FOR_RECOGNITION 3)

(defun open-speech-socket (host)
  (open-speech-sock host)
  (speech-cmnd 'talk))

(defun open-Speech-sock (host) ;host = machine to run HARK on
  (if (null host) (setf host speech-host-name))
  (if (setf speech-stream (comm:open-tcp-stream host speech-port#))
      (progn 
        (terpri)(princ "Connected to...")(princ host)
	(setf (speech-stream *socket-obj1*) speech-stream)
;        (speech-cmnd 'LISTEN)
        (sleep 1)
        (if (not (equal (read-char speech-stream) #\!))
            (print "Did not receive expected answer from Hark.")
          (progn
            (terpri)(princ "HARK up and running.")
            (setf (speech-status *socket-obj1*) t)))
        )
    (print " Error (open-Speech): Connection did not open!"))
  )

(defun close-speech ()
  (close-speech-sock))

(defun close-speech-sock ()
  (speech-cmnd 'QUIT)
  (close speech-stream)
  (setf (speech-status *socket-obj1*) nil))


;;_______________________________
;; DEFINE SOCKET OBJECT
;; Contains all three sockets 
;; for incoming user data
;;-------------------------------

(defclass input-data ()
  ;Holds all three sockets needed for input.
  ((inton-stream :accessor inton-stream :initarg :inton-stream :initform '())
   (inton-status :accessor inton-status :initarg :inton-status :initform nil)
   (inton-data   :accessor inton-data   :initarg :inton-data   :initform '())
   (body-stream  :accessor body-stream  :initarg :body-stream :initform '())
   (body-status  :accessor body-status  :initarg :body-status :initform nil)
   (body-data    :accessor body-data    :initarg :body-data   :initform '())
   (old-body-data :accessor old-body-data :initarg :old-body-data :initform '())
   (num-vals     :accessor num-vals     :initarg :num-vals    :initform num-vals-list)
   (speech-stream :accessor speech-stream :initarg :speech-stream :initform '())
   (speech-status :accessor speech-status :initarg :speech-status :initform nil)
   (speech-data   :accessor speech-data   :initarg :speech-data   :initform '())
   (old-speech-data :accessor old-speech-data :initarg :old-speech-data :initform '())
   ))

(defclass output-data ()
  ;Holds scheduler socket number, status [t=open] and data.
  ((sched-stream :accessor sched-stream :initarg :sched-stream :initform '())
   (sched-status :accessor sched-status :initarg :sched-status :initform '())
   (sched-data   :accessor sched-data   :initarg :sched-status :initform '())
   (graph-data   :accessor graph-data   :initarg :graph-data   :initform '())
   (graph-status :accessor graph-status :initarg :graph-status :initform '())
   (graph-stream :accessor graph-stream :initarg :graph-stream :initform '())
   ))

(defmacro make-input-sock-obj (name) ;name should be *socket-obj1*
  `(setf ,name (make-instance 'input-data)))

(make-input-sock-obj *socket-obj1*)

(defmacro make-output-sock-obj (name) ;name should be *socket-obj2*
  `(setf ,name (make-instance 'output-data)))

#|
(defmethod open-all-sockets ((sock-obj input-data) &optional read-which-body-file?)
  "Checks if socket is open, and if not, opens it."
  (unless (inton-status sock-obj)
    (open-inton-socket))
  (if (not (null 'read-which-body-file?))
      (open-a-file read-which-body-file?)  ;else...
    (unless (body-status sock-obj)
      (open-body-socket t)))
  (unless (speech-status sock-obj)
    (open-speech-socket))
  (report-socket-status sock-obj)
  )
|#

#|
(defmethod open-all-sockets ((sock-obj input-data) &optional read-which-body-file?)
;  (unless (sched-status sock-obj)
;    (open-sched-socket "leon"))
;  (close-all-sock-obj-sockets sock-obj)
  (open-body-socket t)
  (unless (speech-status sock-obj)
    (open-speech-socket speech-host-name))
  (unless (inton-status sock-obj)
    (open-inton-socket))
  (report-socket-status sock-obj))
|#

   
(defmethod kill-all-sockets ()
  (close graphics-stream)
  (close *act-out-sock*)
  (close (body-stream *socket-obj1*))
  (setf  (body-status *socket-obj1*) nil)
  (close (speech-stream *socket-obj1*))
  (setf  (speech-stream *socket-obj1*) nil)
  (close (inton-stream *socket-obj1*))
  (setf  (inton-stream *socket-obj1*) nil)
;  (close-all-sock-obj-sockets *socket-obj1*)
;  (close 
  (close-all-sock-obj-sockets *socket-obj2*))

(defmethod close-all-sockets ()
  (close-all-sock-obj-sockets *socket-obj1*))

(defmethod close-all-sock-obj-sockets ((sock-obj input-data))
  (unless (inton-status sock-obj)
    (if (close (inton-stream sock-obj))
        (setf (inton-status sock-obj) nil))
    )
  (if (close (body-stream sock-obj))
      (setf (body-status sock-obj) nil))
  (unless (speech-status sock-obj)
    (if (close (speech-stream sock-obj))
        (setf (speech-status sock-obj) nil))
    )
  (report-socket-status sock-obj))

;I commented out this close paran because it didn't seem to be useful (6/19)
;  )

(defmethod close-all-sock-obj-sockets ((sock-obj output-data))
  (if (close (graph-stream sock-obj))
      (setf (graph-status sock-obj) nil))
  (if (close (face-stream sock-obj))
      (setf (face-status sock-obj) nil))
  (report-socket-status sock-obj)
  )

(defmethod close-all-sock-obj-sockets ((sock-obj output-data))
  (close (sched-stream sock-obj))
  (setf (sched-status sock-obj) nil))

;This is for reading from a file....
(defun open-a-file (file)
  (unless (null (body-stream *socket-obj1*))
    (close (body-stream *socket-obj1*)))
  (setf (body-stream *socket-obj1*)
	(open file :Direction :input 
	      :element-type '(unsigned-byte 8)))
;  (print (read-bytes (body-stream *socket-obj1*) 4))
;  (print (read-body-no-timestamp NUM-vals-list2))
;  (print (read-body-no-timestamp NUM-vals-list2))
  (setf (body-status *socket-obj1*) t)
  (terpri)(princ "Opened: ")
  (print file)
  )

(defun open-socket (name num)
  (setf name (comm:open-tcp-stream name num)))

(defun socket-status ()
  (report-socket-status *socket-obj1*))

(defmethod report-socket-status ((sock-obj input-data))
  (if (test-socket (inton-stream *socket-obj1*))
      (print "Intonation socket OK")
    (print "Intonation socket not working!"))
  (if (test-socket (body-stream *socket-obj1*))
      (print "Body socket OK")
    (print "Body socket not working!"))
  (if (test-socket (speech-stream *socket-obj1*))
      (print "Speech socket OK")
    (print "Speech socket not working!"))
  ">END in-socket REPORT"
  )

(defmethod report-socket-status ((sock-obj output-data))
  (if (test-socket (sched-stream sock-obj))
      (print "Scheduler socket OK")
    (print "Scheduler socket not working!"))
  ">END out-socket  REPORT")

;;-------------------------------------------
;;  BIT TWIDDLING
;;-------------------------------------------

(defun read-two-byte-vals (stream num-vals)
  (let ((the-array (eval `(make-array '(,num-vals)))))
    (dotimes (count num-vals nil) 
      (setf (aref the-array count)(read-bytes a-stream 2))) ;all datavals are 2 bytes
    the-array))

(defun read-bytes (stream num-of-bytes) ;each byte = 8 bits
  (let ((in 0)
        (x 0))
    (setf in (read-byte stream))
    (dotimes (count (- num-of-bytes 1) in)
      (setf x (* 256 in))
      (setf in (boole boole-ior (read-byte stream) x)))
    (if (eq 1 (ldb (byte 1 15) in))
        (- in 65536) ;works bec when they go neg, they turn all ones.
      in)))


(defun read-two-byte-vals (stream num-vals)
  "Assumes that all values are two bytes (16 bits) long."
  (let ((result '()))
    (dotimes (count num-vals (reverse result))
      (push (read-bytes stream 2) result))
    ))

(defun send-item (string strm)
  "Send a string with a #\newline at the end."
  (write string :stream strm)
  (terpri strm)
  (finish-output strm))

(defun request-record (stream)
  "For requesting a body record when in polled mode."
  (write-byte #b00000001 stream)
;  (write-byte #b00000000 stream)
  (finish-output stream))

(defun int-to-bin (num)
  (setf *print-base* 2)
  (print num)
  (setf *print-base* 10)
  nil)

;--------------------------------------------------
;DEBUG STUFF
;--------------------------------------------------

(defun report (string value)
  (princ string)
  (princ value)
  (terpri))

(defun r (list num-of-bytes)
  (setf in 0)
  (let ((lista list))
    (dotimes (count num-of-bytes nil)
      (setf in (dpb (pop lista)
                    (byte 8 (* 8 count)) in)))
    in))

(defun read-byte-vals (list num-vals)
  "Assumes that all values are two bytes long."
  (setf result '())
  (dotimes (count num-vals nil)
    (push (pop list) result))
  (reverse result)
  )

;;
;; These methods will return the status of streams.
;;

(defmethod test-socket ((sock symbol))
  ()) ;This dummy method required.

(defmethod test-socket (sock)
  ()) ;This dummy main method required.

(defmethod test-socket ((socket stream))
  (format nil "Stream ~S is open." socket))

(defmethod test-socket :after (socket)
  (format nil "Supposed stream ~S is either not open or not a valid socket name." `socket))


(defun test-body ()
  (loop 
   (read-body-data)
   (print *body*)))

(defun test-body2 ()
  (dotimes (count 4 nil)
   (read-body-data)
   (print *body*)))

(defvar file1 "/ahi/ahi2/users/dbk/kcwc/come_here")
(defvar file2 "/ahi/ahi2/users/dbk/kcwc/point_at_line")

(defvar Num-vals-list2 '(16 24 16 24 6))

(defun reset-read-file (file)
  (unless (null (body-stream *socket-obj1*))
    (close (body-stream *socket-obj1*)))
  (open-a-file file))

(defun resume ()
  "Opens a stream to a JB file and reads from it."
  (close s)
  (setf s (open file1 :Direction :input 
                :element-type '(unsigned-byte 8)))
  (setf (body-stream *socket-obj1*) s)
  (print (read-bytes s 4))
  (print (read-body-no-timestamp NUM-vals-list2))
  (print (read-body-no-timestamp NUM-vals-list2))
  (print 'ready)
  (read-line)
  (loop (read-body-record NUM-vals-list2)
        (print (aref *body* 0))
        (print (aref *body* 1))
        (sleep 2)
;(print (aref *body* 0))
;        (print (aref *body* 1))
;	(print in)(int-to-bin in)
        )
  )

(defun read-body (times)
  (dotimes (count times foo)
    (print (refresh-body-data))))