;; -*- Mode: LISP; Syntax: Common-Lisp; Package: (USER); -*-
;***************************************************************
;*
;*  MOTOR CONTROL FOR AGENT
;*
;*  HISTORY: April '94; begin
;*           March '95; mverse commands
;*           May   '95; action scheduler connection
;*           Feb   '96; change to face control (instead of cube)
;*
;***************************************************************
;*
;* Explanation:
;*    MV "move", i.e. move the "intentions" out to the motor system.
;*    MotorMV used to hook up to M-Verse to control
;*    "TheCube" for cube demo.
;*    It now takes care of shipping out
;*    behaviors to the motor controller on Spot.
;*    

(in-package 'USER)

(defvar *output-data* "Stores data sent to motor scheduler.")
(setf   *output-data* nil)
(defvar *output-acts* nil "Stores the cognitive acts executed.")
(setf   *output-acts* nil)

;----------- EXECUTE ------------
;Execute all pending reactive behaviors & one dialogue beh.
;Then, if no other reactive or dial behaviors are pending, execute the to-do list.


(defmethod man-exe ((act basic-behavior)) ;for debugging work.
  (send-act (prepare-act act)))

(defmethod man-exe ((act action-output))
  (prepare-act act)
  (loop while *real-world-act-list* do
        (monitor-real-world-acts))
  )

(defmethod execute ((acts list))
  ;Receive a list with all acts, find reactive and do all, 
  ; find one PCL beh. or CL and do that one too.
  ;TKB acts should not be done unless no other behaviors pending......??
  (let ((dialogue-acts nil)
        (act-list (copy-list acts))
        (act nil)
        (temp nil))
    (if act-list
        (dolist (act act-list)        ;Do all reactive behaviors NOW.
          (if (belongs-to-layer act 1)
              (progn
            ;    (print 'belongs-to-1)
                (send-act (prepare-act act))
                (setf act-list (remove act act-list))
                (un-post act)))
          ))
    (setf act-list *pending-behaviors*) ;presumably only type 2 & 3 acts left
    (setf act nil)
    (if act-list
        (progn
          (setf act (pop act-list))
     ;     (describe act)
          (loop until (or (belongs-to-layer act 2)(null act-list)) do ;find a type 2 (= PCL)
                (setf act (pop act-list)))
          (if (belongs-to-layer act 2)
	      (progn
                (send-act (prepare-act act))
                (un-post act))
            )
          ))
;    (setf act (pop act-list))
;    (if act (execute-CL-action *pending-behaviors*))
    (if (and act (belongs-to-layer act 3))  ;whatever is left must be type 3 (=CL)
	(execute-CL-action *pending-behaviors*))
    ))

(defmethod post-DKB-act ()
  "Pops 1 DKB act off *ACTS* and put on *pending-behaviors*.
   This method called from a dia-beh node. Equiv. to other 'posts'."
  (let ((act (pop-DKB-act *ACTS*)))
    (if act
        (progn
          (setf *ACTS* (remove act *ACTS*))
          (push act *pending-behaviors*)
          ))
    (unless (pop-DKB-act *ACTS*)
      (post-KB-msgs 'DKB-act-avail 'nil))
    ))

(defmethod pop-DKB-act ((acts list))
  "Return next ready DKB act."
  (let ((return nil)
        (temp 0)
        (act (copy-list *acts*)))
    (if acts
        (loop while acts do
              (setf act (pop acts))
              (if (and (> (goodness act) temp) 
                       (equal (type-of act) 'DKB-Speech-Output))
                  (progn
                    (setf return act)
                    (setf temp (goodness act)))
                ))
      )
    return
    ))

(defmethod post-TKB-act ()
  "Pops 1 DKB act off *ACTS* and put on *pending-behaviors*
   - essentially makes it available for execution."
  (let ((act (pop-TKB-act *ACTS*)))
    (if act
        (progn
          (setf *ACTS* (remove act *ACTS*))
          (push act *pending-behaviors*)
          ))
    (unless (pop-TKB-act *ACTS*)
      (post-KB-msgs 'TKB-act-avail 'nil))
    ))

(defmethod pop-TKB-act ((acts list))
  "Receive a list of acts, find a TKB-speech-action, if not
   found, look for a TKB-action-action, if not found, return nil."
  (let ((return nil)
        (acts2 acts))
    (if acts
        (loop while (and acts (not (equal (type-of return) 'TKB-Speech-Output))) do
              (setf return (pop acts)))
      (unless acts
        (loop while (and acts (not (equal (type-of return) 'Action-Output))) do
              (setf return (pop acts))))
      )
    (if (or (equal (type-of return) 'TKB-Speech-Output)
            (equal (type-of return) 'Action-Output))
        return
      nil)
    ))


;----- EXECUTE KB -----

(defmethod execute-CL-action ((actions list))
  "Execute ONE KB action. Do a DKB first, if 
   none available, pop one TKB and execute."
  (let ((act (pop-DKB-act actions)))
    (if act
        (progn
          (print "DKB-Posting: ")(princ (words act))
          (execute-CL-action act)
          (post-kb-msgs 'KB-exe-act  'T)
	  (post-kb-msgs 'DKB-exe-act 'T)
          (setf *ACTS* (remove act *ACTS*))
          (setf *pending-behaviors* (remove act *pending-behaviors*))
          (push act *output-acts*)
;          (print *acts*)
          )
      (progn       ;maybe there is a tkb act available
        (setf act (pop-TKB-act actions))
   ;     (describe act)
        (if act    ;there is a tkb act available
            (progn
	      (post-kb-msgs 'KB-exe-act  'T)
              (cond ((equal (type-of act) 'TKB-speech-output)
	             (execute-CL-action act)
                     (post-kb-msgs 'TKB-exe-speech-act 'T)
		     (print "TKB-speech-Posting: ")(princ (words act))
                     )
		    (T ;if not speech act, it must be world act
	             (execute-CL-action act)
                     (post-kb-msgs 'TKB-exe-world-act 'T) 
		     (print "TKB-action-Posting: ")(princ (func act)))
                    )
	      (setf *ACTS* (remove act *ACTS*))
	      (setf *pending-behaviors* (remove act *pending-behaviors*))
              (push act *output-acts*)
;              (print *acts*)
              ))
        )
      )
    ))

(defmethod execute-CL-action ((action Speech-Output))
;  (print 'speech-output)
  (describe action)
  (if (acts action)
      (send-act (prepare-act (acts action))))
  (send-act (prepare-act action)))

(defmethod execute-CL-action ((action Action-Output))
;  (print 'action-output)
;  (describe action)
  (if (words action)
      (send-act (prepare-act (words action))))
  (prepare-act action) ;prepare-act for action-output does all...
  )
;;extra
;;)

(defmethod belongs-to-layer ((beh reactive-beh) (num integer)) (= 1 num))
(defmethod belongs-to-layer ((beh PCL-beh)      (num integer)) (= 2 num))
(defmethod belongs-to-layer ((beh Speech-Output)(num integer)) (= 3 num))
(defmethod belongs-to-layer ((beh CL-beh)       (num integer)) (= 3 num))
(defmethod belongs-to-layer ((beh Action-Output)(num integer)) (= 3 num))
(defmethod belongs-to-layer (foo1 foo2)
  (print "BELONGS-TO-LAYER: Received an invalid act: ")
  (princ (type-of foo1))(princ foo1)
  nil)

(defmethod un-post ((beh ext-beh))  ;name "un-post" not used anywhere else
  (setf *pending-behaviors* (remove beh *pending-behaviors*)))

(defmethod un-post ((beh DKB-speech-output))
 ; (describe beh)
  (setf *pending-behaviors* (remove beh *pending-behaviors*)))

(defmethod un-post ((beh TKB-speech-output))
  (setf *pending-behaviors* (remove beh *pending-behaviors*)))

(defmethod un-post ((beh Output-Act))
  (setf *pending-behaviors* (remove beh *pending-behaviors*)))

(defmethod prepare-act ((act reactive-beh))
  ;TEMPLATE: (name type stamp timeout)
  ; type in this case is reactive = 1
  (list (first (msgs act))   ;name of motor act to do
        1                    ;system initiating this request
        (stamp act)          ;when was the request initated
        (extent act)         ;how long is it allowed to live
        nil)                 ;this last info used for spatial and speech data.
  )

(defmethod prepare-act ((act ext-pcl-beh))
  ;TEMPLATE: (name type stamp timeout)
  ; type in this case is reactive = 1
  (list (msgs act)           ;name of motor act to do
        2                    ;system initiating this request
        (stamp act)          ;when was the request initated
        (extent act)         ;how long is it allowed to live in sys
        nil)                 ;this last info used for spatial data.
  )

(defmethod prepare-act ((act spatial-rea-beh)) 
  ;the idea here is that the appropriate coordinates are shipped...
  ;Pseudo-code
  ; 1. find-object whose name is the second in the msgs list
  ; 2. get it's coordinates at this point, in real-space
  ; 3. compute what angles are needed to point any vector directly at it
  ; 4. send the angles out with the act
  ; NOTE; this is not quite the way it SHOULD be done; one should allow
  ;       the motor system ("cerebellum") to access the spatial knowledge base
  ;       and establish some sort of link between the current motor act
  ;       and the object of focus in the spatial knowledge base. This is
  ;       how I propose it in my thesis.
  (let ((obj-in-question (second (msgs act)))
	(obj-pos nil)
	(coords '(0 0)))
    (if (null (stamp act)) ;needed when manually sending things over...
        (setf (stamp act) (time-stamp))) 
    (cond ((equal obj-in-question 'user)
           (setf obj-pos (get-head-pos)))   
         ;  (setf coords '(0 0)))
          ((equal obj-in-question 'big-screen)
           (setf obj-pos (center work-screen))
	   (setf coords (point-plane-at-obj agent-screen obj-pos)))
	  )
    (list (first (msgs act))
	  1  ;code for reactive behaviors
	  (stamp act)
	  (extent act)
	  coords)
    ))

(defmethod prepare-act ((act ext-spatial-PCL-beh))
  (let ((obj-in-question (second (msgs act)))
	(obj-pos nil)
	(coords '(0 0)))
 ;   (print '***FIRST_HELLO***)
    (cond ((equal obj-in-question 'user)
       ;    (setf obj-pos (get-head-pos)) ;changed 2/7/96
           (setf coords '(0 0)))
          ((equal obj-in-question 'big-screen)
           (setf obj-pos (center work-screen)) ;also avail through the global *big-screen-center*
	   (setf coords (point-plane-at-obj agent-screen obj-pos)))
	  )
    (list (first (msgs act))
	  2  ;code for PCL behaviors
	  (stamp act)
	  (extent act)
	  coords)
    ))

(defmethod prepare-act ((utter string))
  (list 'deliver-speech
        3
        (time-stamp)
        9000
        utter)
  )

(defmethod prepare-act ((act symbol))
  "act must be a TKB-speech-act accompaniment of manual gesture."
  (list act
        3
        (time-stamp)
        9000
        nil)
  )

(defmethod prepare-act ((act Speech-Output))
  ;TEMPLATE: (name type stamp timeout)
  (list 'deliver-speech
        3
        (stamp act)
        9000
        (words act))
  )

(defmethod prepare-act ((act Action-Output))
  (let ((real-world-object nil))
    (cond ((equal (func act) 'zoom)
           (funcall (func act) (domain-act act)))
          (T ;otherwise it's a go-to
           (if (subtypep 
                (type-of (setf real-world-object (get-KB-obj (words (domain-act act)))))
                'object)
               (funcall (func act) real-world-object))
	   )
          )
    ))


;-------- REAL WORLD ACTS MONITOR -------

(defmethod monitor-real-world-acts ()
  "Receives a list of lists -> [action1 parameter1 t1 dur1], checks the
   time in the domain world and if it is > t1, execute action1.
   If the time is > t1+dur1 then the list is popped."
  (if *real-world-act-list*   ;if there is anything left to do in the domain world
      (let ((now (get-graphics-time))
            (xtra-pause 40)) ;because clock is a little behind
        (post-KB-msgs 'TKB-exe-world-act 'T)
        (dolist (act *real-world-act-list*) ;in case there are multiple acts to perform.
          (if (> now (third act))
	      (if (> now (+ xtra-pause (third act) (fourth act))) ;means the act is done
                  (setf *real-world-act-list* (remove act *real-world-act-list*))
                (progn
                  (if (first (last act)) ;means the act hasn't been executed yet
                      (progn 
                        (eval act)
                        (setf *real-world-act-list* (remove act *real-world-act-list*))
		        (setf act (append act (list 'nil))) ;mark the act
                        (push act *real-world-act-list*) ;put new marked act on list
                   ;     (print act)
		        ))
		  ))
	    ))
        )
    (if (kb-time-since 'TKB-exe-world-act  200)
        (post-KB-msgs  'TKB-exe-world-act 'nil))
    )
  )
  
;-------- SOCKET PREP & SHIPPING -------

(defun send-act (info-list &optional (sock nil))
  "Receives a single act (as a list) and sends it through the socket."
  (if (not sock) (setf sock *act-out-sock*))
               ;we 'read-from-string' on the other side.
  (if sock
      (progn
        (push (append info-list (list (time-stamp))) *output-data*)
        (write-line (write-to-string info-list) sock)
        (finish-output sock))
    (progn
      (print "No conn. to face.")
      (princ info-list)
      )
    )
  )


;----------------------------------------------
;      SCHEDULER SOCKET SETUP PROCEDURES
;----------------------------------------------

(defvar *act-out-sock* nil)
(defvar act-out-sock-# 1999)
(setf   act-out-sock-# 1999)
(defvar sched-mach-name "spot") ;Machine running action scheduler

(defun open-act-out-socket ()
  (if (streamp *act-out-sock*)
      (close *act-out-sock*))
  (setf *act-out-sock* nil)
  (if 
      (y-or-n-p "Is Action Scheduler listening on socket # ~D? " act-out-sock-#)
      (if (setf *act-out-sock* 
                (comm:open-tcp-stream sched-mach-name act-out-sock-#))
          (progn
            (terpri)(princ "Connected to Action Scheduler on ")
            (princ sched-mach-name)
            (terpri)(princ "Socket #: ")(princ act-out-sock-#))
        (print "Error (open-act-out-socket): No connection made to Action Scheduler.")
        )
    (progn
      (print "No attempt made to connect.")
      (print "Modify act-out-sock-# to change sock number."))
    )
  (print '*act-out-sock*=)(princ *act-out-sock*)
  (print "Connected to Motor Scheduler on Spot.")
  (setf (sched-stream *socket-obj2*) *act-out-sock*)
  (setf (sched-status *socket-obj2*) T)
  *act-out-sock*
  )

(defun open-face-socket ()
  (open-act-out-socket))

(defun close-act-out-socket ()
  (close *act-out-sock*)
  (print "Act-out-socket closed.")
  (setf (sched-stream *socket-obj2*) *act-out-sock*)
  (setf (sched-status *socket-obj2*) nil))


;----------------------------------------------
;      M-VERSE "TheCube" MOTOR COMMANDS
;          2/16 1996: out of date
;----------------------------------------------

;----------- PROTOTYPE MOTOR CONTROL ----------

(defvar mverse-stream nil)

(defmethod execute-mverse ((behavs list))
  (unless (null behavs)
    (dolist (first-beh behavs)
;      (print first-beh)(princ (msgs first-beh))
      (execute-action-mv first-beh))))

(defmethod execute-action-mv ((behav ext-beh)) 
  ;behavior for now..call it s-th else later.
  (eval (msgs behav))
;  (if *display-on* (redraw-obj behav))
  (un-post behav))

;----------- CUBE-RELATED COMMANDS ----------

(defun cube-actv ()
  (cube-active)
  (print 'cubeactive-payattention))

(defun displ-speaking ()
  (MV-cmnd "SET_COLOR 0 1 0")
  (mv-cmnd "REDRAW")
  (print 'displ-speaking-green))

(defun displ-silent ()
  (MV-cmnd "SET_COLOR 1 1 0")
  (mv-cmnd "REDRAW")
  (print 'displ-silent-rg))

(defun show-take-turn ()
  (MV-cmnd "SET_COLOR 1 0 0")
  (mv-cmnd "REDRAW")
 (print 'show-take-turn-red))

(defun show-give-turn ()
  (MV-cmnd "SET_COLOR 0 1 0")
  (print 'show-give-turn-blue))

(defun slow-cube1 ()
  (slow-down)
  (print 'slowdown-gettinganxious))

(defun speed-cube ()
  (speed-up)
  (print 'speedup))

(defun slow-cube2 ()
  (slow-down)
  (print 'slowdown))

(defun set-cube-white ()
  (MV-cmnd "SET_COLOR 1 1 1")
  (print 'white-showsurprise))

(defun slow-down ()
  (remove-wl)
  (cube-active)
  (print 'cubeactive))

(defun MV-cmnd (command)
  (if (streamp mverse-stream)
      (progn
        (write-line command mverse-stream)
        (if (listen mverse-stream)
            (read mverse-stream)))
    (print command))
  )

(defun mv (in)
  (mv-cmnd in))

(defun make-cube ()
  (mv-cmnd "LOAD_CLASSES primitives")
  (mv-cmnd "SET_CLASS cube")
  (mv-cmnd "SET_COLOR 1 0 0.6")
  (mv-cmnd "SET_SCALE 4 4 4")
  (mv-cmnd "START_CLOCK"))

(defun cube-active ()
  (mv-cmnd "START_CLOCK")
  (remove-wl)
  (mv-cmnd "ANIMATE  1 0 1 1 !0 0 0 1 0.2 0.2 1 0")
  (mv-cmnd "ANIMATE  1 0 1 1 !0 0 0 2 0 0 1 0")
  (mv-cmnd "ANIMATE  1 0 1 1 !0 0 0 0.8 0 0.4 1 0"))

(defun speed-up ()
  (mv-cmnd "START_CLOCK")
  (mv-cmnd "ANIMATE  1 0 1 2 !0 0 0 1 0.5 0 1 0")
;  (mv-cmnd "ANIMATE  1 0 1 2 !0 0 0 1 0 0.6 1 0")
  (Mv-cmnd "ANIMATE  1 0 1 3 !0 0 0 0.6 0.4 0.7 1 0"))

(defun stop-clock ()
  (mv-cmnd "STOP_CLOCK"))

(defun mv-reset ()
  (mv-cmnd "RESET"))

(defun cube-freeze ()
  (mv-cmnd "RM_WL 0"))

(defun remove-wl ()
  (mv "RM_WL 0"))

(defun quit-mverse ()
  (MV-cmnd "QUIT"))

(defun close-mverse ()
  (quit-mverse))

(defun remove-objects ()
  (mv-cmnd "DELETE_ALL"))


;;_______________________________
;; MVERSE CUBE SOCKET CONNECTION
;;-------------------------------

(defvar Mverse-leon# 15004) ;Josh's mverse_exp version
(defvar Mverse-spleen# 15006) ;This is the port for displaying on spike....
;Open a socket to Mverse and create The Cube.

(setf Mverse-port# 15006)

(defun open-Mverse (host) ;host = machine to run mverse on
  (if (equal host "leon")
      (setf port Mverse-leon#)
    (setf port Mverse-spleen#))
  (terpri)(princ "Trying ")(princ host)(princ " at ")(princ port)
  (if
      (setf mverse-stream
            (comm:open-tcp-stream host port :direction :io))
      (progn 
        (terpri)(princ "Connected to ")(princ host)
        (make-cube)
        )
    (print " Error (open-Mverse): Connection did not open!"))
  mverse-stream
  )

; For TheCube
(defun open-sched-socket (host)
  (open-Mverse host))