;;******************************************
;;                                         *
;; SockRead1.lisp                          *
;;                                         *
;; Hook up to                              *
;;   1. the body model dispatcher, and     *
;;   2. the intonation analysis,           *
;;   and read the data into buffers.       *
;;                                         *
;; K. R. Thorisson, start: Oct.1994        *
;;                                         *
;;******************************************

(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 HAND-num-vals HAND-num-vals 
                          EYE-num-vals HEAD-num-vals 
                          TORSO-num-vals))

;For accessing the *body* list
(defconstant get-STAMP 0)
(defconstant get-RA 1)
(defconstant get-LA 2)
(defconstant get-RH 1) ;the same as arm
(defconstant get-LH 2) ;the same as arm
(defconstant get-EYE 3)
(defconstant get-HEAD 4)
(defconstant get-TORSO 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))

;3/18 body data is just a list of vectors....
(setf *body* (make-array '(6)))

(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-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 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-arm-number-list   '(1 23 24 25))
(defvar right-arm-number-list  '(2 23 24 25))
(defvar left-hand-number-list  '(1 0 1 2))
(defvar right-hand-number-list '(2 0 1 2))
(defvar left-wrist-pitch-list  '(1 3))
(defvar right-wrist-pitch-list '(2 3))
(defvar left-elbow-angle-list  '(1 15))
(defvar right-elbow-angle-list '(2 15))
(defvar eye-number-list        '(3 0 1 2 5 6 7)) 
(defvar head-number-list       '(4 0 1 2 6 7 8))
(defvar torso-number-list      '(5 0 1 2 6 7 8))

(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 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 ()
  (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 ()
  (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* (pop the-list))))
    (setf (aref vect4 0) (aref body-part (pop the-list)))
    ))

(defun get-l-elbow-angle ()
  (let* ((the-list right-elbow-angle-list)
         (body-part (aref *body* (first the-list))))
    (setf (aref vect4 0) (aref body-part (second the-list)))
    ))

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

(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 "hand above! elbow:")
	        (princ (get-r-elbow-angle))
                )
            )
	  )
    ))
  
;;_______________________________
;; 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*))
             ))
       ))
    ))

(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))
        )
  )
   
(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)))

(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))
  )

(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))))