;; -*- Mode: LISP; Syntax: Common-Lisp; Package: (USER); -*-
;-----------------------------------------------------------
;
;  Assemble.lisp
;  -contains initialization
;   routines, as well as main loop.
;
; 'Ymir Agent System, 1994-1995-1996
;
;-----------------------------------------------------------

(in-package 'USER)

(defvar *display-on* nil)
(setf *display-on* nil)

;**********************
;* Set the host first
;**********************

(defvar *window-already-created* nil)
(defvar *make-window?* nil)
(defvar *hostname* nil)
;(setf *hostname* nil)
;(setf *hostname* "spike")

(defun ask-user-host ()
  (princ "Type your hostname (no quotes): " t)
  (setf *hostname* (read-line t)))

(if (and (eq *hostname* nil)
         *display-on*)
    (ask-user-host))

(if (and *display-on* (not *window-already-created*))
    (progn
      (set-display (ask-user-host))
   ;   (load "/mas/ahi/ahi2/users/kris/xvisLW")  ;Display functions
   ;   (open-window)
      (setf *make-window?* t)))
    
;(if *display-on* (set-display *hostname*))
;(if *display-on* (open-window))

(unless *make-window?* 
  (setf *display-on* nil))


;**********************
;* 
;**********************

;(unless   ;To load CLOS ;This is not needed in Lispworks.
; (fboundp 'make-instance)
;	  (load "/mas/lib/ds/lisp/clos.mbin")) 
    
(if (and (not *window-already-created*) *make-window?*)
    (progn
;      (load "/mas/ahi/ahi2/users/kris/xvisLW")  ;Display functions, now done in P*.lisp
      (open-window)
      (setf *window-already-created* t)
      (setf *make-window* nil)))


;******************************************************************************
; LOAD PROTOTYPE FILE
;******************************************************************************

;(load "/ahi/ahi2/users/kris/P2.lisp")          ;Prototype behaviors and integrators.
; Now you only load the prototype file, which will load the Assemble file.

;*********************************************
;* Make sensor and descriptor objects & lists
;*********************************************

(defvar *sensors*        '())
(defvar *descriptors*    '())
(defvar *turn-state-beh* '())
(defvar *ext-rea-beh*    '())
(defvar *state-beh*      '())
(defvar *ext-PCL-beh*    '())
(defvar *int-PCL-beh*    '())

(make-sensors     *turn-sensors*)
(make-descriptors *turn-descriptors*)

(defmethod make-all ()
  (make-input)
  (make-output) ;Make-all will also do the behaviors....
  (KB-init)
  (if *display-on* (update-graph))
  T)    

;(setf *socket-obj1* nil)

(defmethod make-input ()
  (make-sensors *turn-sensors*)
  (make-descriptors *turn-descriptors*)
  (make-sensory-lists)
  (unless (boundp '*socket-obj1*)
    (make-input-sock-obj *socket-obj1*))
  )

(defun make-sensory-lists ()
  (setf *sensors* '())
  (setf *descriptors* '())
  ;Make a list containing only sensor objects.
  (dolist (sub-list *turn-sensors*)   
	  (setf *sensors* 
		(nconc *sensors* (list (eval (first sub-list))))))
  ;Make a list containing only descr objects.
  (dolist (sub-list *turn-descriptors*) 
	  (setf *descriptors* 
		(nconc *descriptors* (list (eval (first sub-list))))))
  )

;*********************************************
;* Make behavior objects & lists
;*********************************************

(defmethod make-output ()
  (make-turn-state-behaviors   *turn-state-behaviors*)
  (make-ext-rea-behaviors      *ext-rea-behaviors*)
  (make-int-rea-behaviors      *int-rea-behaviors*)
  (make-ext-cont-rea-behaviors *ext-cont-rea-behaviors*)
  (make-PCL-state-behaviors    *PCL-state-behaviors*)
  (make-ext-PCL-behaviors      *ext-PCL-behaviors*)
  (make-ext-CL-behaviors       *ext-CL-behaviors*) 
  (make-int-PCL-behaviors      *int-PCL-behaviors*)
  (make-behaviors-lists)
  (unless (boundp '*socket-obj2*)
    (make-output-sock-obj *socket-obj2*))
  )

(defun make-behaviors-lists ()
  (setf *ext-rea-beh*     '())
  (setf *int-rea-beh*     '())
  (setf *PCL-state-beh*   '())
  (setf *ext-PCL-beh*     '())
  (setf *int-PCL-beh*     '())
  (setf *turn-state-beh*  '())  ;Keep this for backward compatibility....
  (setf *turn-state-beh*  '())  ;used to be for turn-state-beh
  (setf *state-beh*       '())
  (setf *CL-beh*          '())
  ;Make a list containing only objects.
  (dolist (sub-list *ext-rea-behaviors*)    
    (setf *ext-rea-beh* 
	  (append *ext-rea-beh* (list (symbol-value (first sub-list))))))
  (dolist (sub-list *ext-cont-rea-behaviors*)
    (setf *ext-rea-beh*
          (append *ext-rea-beh* (list (symbol-value (first sub-list))))))
  (dolist (sub-list *int-rea-behaviors*)    
    (setf *int-rea-beh* 
	  (append *int-rea-beh* (list (symbol-value (first sub-list))))))
  (dolist (sub-list *turn-state-behaviors*)            
    (setf *turn-state-beh*
          (append *turn-state-beh* (list (symbol-value (first sub-list))))))
  (dolist (sub-list *turn-state-behaviors*)            
    (setf *state-beh*                                           ;2/25/96
          (append *state-beh* (list (symbol-value (first sub-list)))))) ;from now on use this list
  (dolist (sub-list *pcl-state-behaviors*)                      ;instead of *turn-state-beh*
    (setf *state-beh*
          (append *state-beh* (list (symbol-value (first sub-list))))))
  (dolist (sub-list *ext-pcl-behaviors*)                  
    (setf *ext-PCL-beh*
          (append *ext-PCL-beh* (list (symbol-value (first sub-list))))))
  (dolist (sub-list *ext-CL-behaviors*)                     
    (setf *CL-beh*
          (append *CL-beh* (list (symbol-value (first sub-list))))))
  (dolist (sub-list *int-PCL-behaviors*)
    (setf *int-PCL-beh*
          (append *int-PCL-beh* (list (symbol-value (first sub-list))))))
  )
   ;Note that *turn-state-behaviors* & *ext...* are 
   ;lists with pointers to the real objects.


;*****************************
;* INITIALIZATION
;*****************************


(defun make-grand-generic-list ()
  (setf *grand-generic-list* 
        (append *sensors* *descriptors* *turn-state-beh* 
                *ext-rea-beh* *CL-beh* *PCL-state-beh* *ext-PCL-beh*)))

(defun initialize-all ()
  (make-all)
  (if (speech-status *socket-obj1*) 
      (progn
        (print 'reading-old-speech...)
        (read-speech-data)
        (print '...done)
        ))
  (initialize-speech) ;clears speech buff, sets *parse-list* to nil.
;  (initialize-kb-msgs)
  (setf *turn-history* nil)
  (initialize *sensors*)
  (initialize *descriptors*)
  (initialize *ext-rea-beh*)
  (initialize *int-rea-beh*)
  (initialize *CL-beh*)
  (initialize-history *sensors*)
  (initialize-history *descriptors*)
  (update-graph)
;  (setf (state take-turn1) nil) (post take-turn1)
;  (setf (state take-turn2) nil)
;  (setf (state give-turn1) T)   (post give-turn1)
;  (if (find-menu "Graphics") (make-grand-generic-list))
;  (make-grand-generic-list)
  ;This has to come after update-graph.
;  (restore-ext-behaviors)   ;This will make them executable.
; 2/4/96 (activate 'give-turn) 
  (initialize-history *socket-obj1*)
;  (set-all-one-shot-ext-rea-beh-to-done *ext-rea-beh*) ;make the right ones get selected as active
  (update *ext-rea-beh*)
  (calibrate-agent-screen) ;calculates the coordinates and center.
  (calibrate-work-screen)
  (make-grand-generic-list)
  (setf *ACTS* nil)
  (setf *pending-behaviors* nil)
  (setf *output-data* nil)  ;contains raw output data, -defined in motorMV
  (setf *output-acts* nil)  ;cognitive acts that have been executed
  (initialize-state)
;  (init-body-data) ;commented out 5/25/96 to add arm data
  (init-kb)
 ; (setf *real-world-act-list* nil) ;contains remaining commands for real-world
  (if *display-on* (redraw))
  (init-planet-knowledge)
  )

(defmethod set-all-one-shot-ext-rea-beh-to-done ((objs list))
  (dolist (obj objs)
    (set-all-one-shot-ext-rea-beh-to-done obj)
    ))

(defmethod set-all-one-shot-ext-rea-beh-to-done ((obj ext-one-shot-rea-beh))
  (setf (done obj) nil))

(defmethod set-all-one-shot-ext-rea-beh-to-done ((obj ext-cont-rea-beh)) )

(defun init-all-sockets (&optional file)
  (if (boundp '*socket-obj1*)
      (progn
        (make-input-sock-obj *socket-obj1*)
        (make-output-sock-obj *socket-obj2*)
        (if (y-or-n-p "Open selected socket connections? [y/n]")
            (open-select-sockets)
          (if (y-or-n-p "Open all sockets? [y/n]")
              (open-all-sockets))
          ))
    )) ; (open-all-sockets *socket-obj1* file)))

(defun open-select-sockets ()
  (if (y-or-n-p "Open Graphics system socket? {on Spleen} [y/n]")
      (progn
        (open-graphics)
        (create-solar-system)
        ))
  (if (y-or-n-p "Open face socket? {on Splotch} [y/n]")
      (open-face-socket))
  (if (y-or-n-p "Open intonation socket? {on Spork} [y/n]")
      (open-inton-socket))
  (if (y-or-n-p (format nil "Open speech socket? {on ~A} [y/n]" speech-host-name))
      (progn
        (open-speech-sock speech-host-name)
        (speech-cmnd 'talk)))
;  (if (y-or-n-p "Open motor-out socket? {on Spot} [y/n]")
;      (open-act-out-socket))
  (if (y-or-n-p "Open body stream socket? {on Sprocket} [y/n]")
      (open-body-socket))
  )

(defun make-and-init-all ()
  (make-all)
  (initialize-all))


;***********************
;*
;*    INIT/HISTORY
;* 
;***********************

(defmethod initialize-history ((obj input-data))
  (setf (inton-data obj) nil
        (body-data obj) nil
        (speech-data obj) nil)
  T)
  
(defmethod initialize-history ((objects list))
  (princ #\newline)
  (princ "Initializing history; posting these objects>")
  (dolist (obj objects)
    (print (msgs obj))
    (post obj))
  T)

(defmethod initialize-state ()
  (de-activate dial-on1))

(defmethod initialize ((objects list))
  (princ #\newline) 
  (princ "Initializing these objects>")
  (unless (null objects)
    (dolist (obj objects)
      (print (msgs obj))
      (initialize obj))
    )
  T)

(defmethod initialize ((obj ext-cont-rea-beh))
  (if (equal 'T (first (pos-cond obj)))
      (setf (active obj) T))
  (setf (stamp obj) 0)
  )

(defmethod initialize ((object turn-descriptor))
  (setf (state object) nil)
  (setf (active object) nil)
  T)

(defmethod initialize ((obj int-one-shot-rea-beh))
  (setf (state obj) nil)
  (setf (active obj) t) ;not sure about this
  (setf (done obj) nil)
  T)

(defmethod initialize ((obj ext-one-shot-rea-beh))
  (setf (state obj) nil)
  (setf (active obj) t)
  T)

(defmethod initialize ((obj PCL-beh))
  (unless (null obj)
    (setf (state obj) nil
          (active obj) t)
    )
  T)

(defmethod initialize ((obj ext-Cl-beh))
  (setf (done obj) T))

(defmethod initialize ((obj symbol))
  (if obj 
      (progn 
        (setf (state (eval obj)) nil)
        T)
    nil)
  )

;------- SENSORS -------

(defmethod initialize ((obj inton-sensor))
  (setf (state obj) nil)
  (setf (active obj) t)
  T)

(defmethod initialize ((obj speech-sensor))
  (setf (state obj) nil))

(defmethod initialize ((obj socket-sensor))
  (setf (state obj) nil))

(defmethod initialize ((obj body-sensor))
  (setf (state obj) nil)
  (setf (data1 obj) (make-line
                     (make-array '(3) :initial-contents '(0 0 0))
                     (make-array '(3) :initial-contents '(0 0 0)))
        (data2 obj) (make-line
                     (make-array '(3) :initial-contents '(0 0 0))
                     (make-array '(3) :initial-contents '(0 0 0))))
  T)

(defmethod initialize ((obj BS-fix-ref))
  (setf (state obj) nil)
  (setf (data1 obj) (make-line
                     (make-array '(3) :initial-contents '(0 0 0))
                     (make-array '(3) :initial-contents '(0 0 0))))
  T)


(defmethod init-body-data ()
  ;"Used to fake data for sensors when body not hooked up."
  (let ((data nil))
    (setf data
          (make-array '(7)))
    (setf (aref data 0)
          (make-array '(1) :initial-contents '(0)))
    (setf (aref data 1)
	  (make-array '(24) 
		      :initial-contents '(0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)))
    (setf (aref data 2)
	  (make-array '(24) 
		      :initial-contents '(0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0)))
    (setf (aref data 3)
	  (make-array '(8) 
		      :initial-contents '(0 0 0 0 0 0 0 0)))
    (setf (aref data 4)
	  (make-array '(12) 
		      :initial-contents '(0 0 0 0 0 0 0 0 0 0 0 0)))
    (setf (aref data 5)
	  (make-array '(12) 
		      :initial-contents '(0 0 0 0 0 0 0 0 0 0 0 0)))
    (setf (body-data *socket-obj1*) data)
    (setf *body* (body-data *socket-obj1*))
    ))

(defun generate-report ()
  (socket-status)
  (print "no report"))

;*****************************
;* MAIN LOOP
;*****************************

;These defined in SockRead.lisp
;(defvar file1 "/ahi/ahi2/users/dbk/kcwc/come_here")
;(defvar file2 "/ahi/ahi2/users/dbk/kcwc/point_at_line")

(setf *step* nil)

(defun main1 ()
  "Read from file."
  (reset-read-file *read-file*)
  (setf *freq* 2
        last-timestamp 0
        new-timestamp 1)
  (if *display-on* (progn
                     (update-graph)
                     (redraw)))
  (initialize-all)
  (print (refresh-body-data)) ;To fill the initial buffer
  (do ((foo 1))((= last-timestamp new-timestamp))
;    (sleep 0.1)
    (setf last-timestamp new-timestamp)
    (if *step* (read-line))
    (read-inton-data) 
    (refresh-body-data)     ;Simply refresh body data
    (setf new-timestamp (aref (aref *body* 0) 0))
;    (print new-timestamp)
    (princ ".")
    (update *sensors*)       
    (update *descriptors*)
    (update *turn-state-beh*)
    (update *ext-rea-beh*)
    (update *int-rea-beh*)
;    (if (listen *standard-input*)
;        (print (setf new-timestamp last-timestamp)))
;    (princ "pending: ")(princ *pending-behaviors*)
    (if *pending-behaviors* (execute-mverse *pending-behaviors*)) ;mv for mverse version
;    (report-numbers)
;   (if *display-on* (redraw))
    ))

(setf *print-timestamp* nil)
(defvar *run* nil)
(setf   *run* t)

(setf synced? nil)

(defun main2 ()
  "Read from real-time connection. Run the face."
  (if (y-or-n-p "Synchronize? [y/n]: ")
      (sync))
  (unless synced? (sync))
  (setf synced? t)
  (if *display-on* (progn
                     (update-graph)
                     (redraw)))
  (initialize-all)
;;commented out 5/25/96 to add arm data:
;  (unless (body-status *socket-obj1*) (init-body-data)) 
  (setf *pending-behaviors* nil)
  (setf *run* t)
  (if (graph-stream *socket-obj2*) 
      (set-up)) ;orient the graphics correctly
  (setf *current-planet* earth)
  (setf *planet-in-view* earth)
  (loop while (and *run* (not (read-char-no-hang))) do
        (main-loop))
  )

(defun main-loop ()
  (if *step* (read-line))
  (if (inton-status *socket-obj1*) (read-inton-data))   ;!this should be controlled by beh.
  (if (body-status  *socket-obj1*) (refresh-body-data)) ;to couple sens. inp. w/ beh directly.
  (update *sensors*)     
  (update *descriptors*) 
  (update *state-beh*)   
  (update *int-rea-beh*) 
  (parse  *parse-list*)   ;what is this good for??????? 4/5/96
  (update *ext-rea-beh*) 
  (update *int-PCL-beh*)    ;added 4/12/96
  (update *ext-pcl-beh*)    ;things such as ask
  (update-KB)               ;Should really run on separate processor
  (update *CL-beh*)         ;DKB and TKB execution
  (monitor-real-world-acts) ;equivalent to "update real-world-acts"
;  (update *turn-state-beh*) ;these now included in *state-beh* 3/14/96
  (if *pending-behaviors* (execute *pending-behaviors*))
  )

(defun main3 ()
  "Run from current state."
  (setf *run* T)
  (loop while (and (not (read-char-no-hang)) *run*) do
        (main-loop))
  )


(defun main4 ()
  "Read from real-time connection."
  (if *display-on* (progn
                     (update-graph)
                     (redraw)))
  (initialize-all)
  (print "Assuming all sockets opened.")
  (print (refresh-body-data))
  (do ((foo 1)) ()
    (read-inton-data)
    (refresh-body-data)
    (read-speech-data)
    (parse *template-list*) ;template-list contains all speech knowledge...
    (update *sensors*)
    (update *descriptors*)
    (update *turn-state-beh*)
    (update *ext-rea-beh*)
    (if *pending-behaviors* (execute *pending-behaviors*))
    ))

;;------ USEFUL ------

(defun update-all ()
  (update *sensors*)
  (update *descriptors*)
  (update *turn-state-beh*)
  (update *ext-rea-beh*))


;*****************************
;* MISC
;*****************************

(defmethod total-conds ((node turn-system-node))
  (+ (length (pos-cond node))
     (length (neg-cond node))))

(defvar *now* nil)

(defun print-time ()
  "Function to test timing."
    (setf *now* (rtb))
    (loop
      (if (< 10 (- (rtb) *now*))
        (and (print "10 ")
             (setf *now* (rtb)))))
    )

(defun report-numbers ()
  (terpri)
  (princ "head: ")
  (describe (get-head-dir))
  (terpri)
  (princ "gaze: ")
  (describe (get-gaze-dir)))


(defun go-to-hist (num)
  (let ((hist (reverse *turn-history*))
        (item nil))
    (setf item (pop hist))
    (loop while (and hist (< (third item) num)) do
          (setf item (pop hist))
          )
    (step-through-history hist)
    ))
           
(defun step-through-history (hist)
  (let ((chara nil))
    (loop while (and hist (not (equal chara #\q))) do
          (print (pop hist))
          (setf chara (read-char))
          )
    ))