;; -*- Mode: LISP; Syntax: Common-Lisp; Package: (USER); -*-
;***************************************************************
;*
;*  MVERSE MOTOR CONTROL FOR AGENT
;*
;*  HISTORY: April '94; begin
;*           March '95; mverse commands
;*           May   '95; action scheduler connection
;*
;*
;***************************************************************
;*
;* Explanation:
;*    MV stands for mverse and "move":
;*    MotorMV hooks up to M-Verse to control
;*    "TheCube" for cube demo.
;*    It also will take care of shipping out
;*    behaviors to the motor controller (when
;*    ready) on Spot.
;*    

(in-package 'USER)

;----------------------------------------------
;            MOTOR PROCEDURES
;----------------------------------------------

; See below for same-name function that needs to be
; commented out when we go to motor control through
; action scheduler Spot.
; On *pending-behaviors* would be TKB generated
; behaviors and TKB generated "problem" behaviors, as
; well as other dialogical behaviors relating to
; dialogue.

;----------- EXECUTE ---------------


(defmethod execute (act-list) ;What do single acts look like? A list
;  (print act-list)(print *pending-ext-behaviors*)(describe (first *pending-ext-behaviors*))
  (send-act (pop act-list)))  ;of at least 2 features: (actname initiator)

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

(defmethod execute ((act-list list)) 
  ;Receive a list with all acts, find reactive and do all, 
  ; find one dial. beh. and do that one too.
  ;TKB acts should not be done unless no other behaviors pending......??
  (let ((dialogue-acts nil)
        (act nil))
    (dolist (act act-list)
      (if (type-of act 'reactive-beh)
          (progn
            (send-act act)
            (un-post act)))
      )
    (loop until (or (type-of act 'dialogue-beh)(null act-list)) 
          do  (setf act (pop act-list)))
    (if act (send-act act))
    ))

(defmethod execute ((act-list list))
  ;This method simply ships out ALL behaviors in the buffer
  (dolist (act act-list)
    (send-act act)
    (un-post act)))
  
(defmethod un-post ((beh ext-beh))
  (setf *pending-behaviors* (remove beh *pending-behaviors*)))


;-------- SOCKET PREP & SHIPPING -------

(defun send-act (act &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.
  (write-line (write-to-string (list act)) sock)
  (finish-output sock))


;...1995:
;Eventually the motor commands will be sent automatically to a module that integrates 
;them and puts them into animation. This module will check back with the originating
;command unit if it fails to execute...and also to say that it is finished.


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

(defvar *act-out-sock* nil)
(defvar 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*)
  *act-out-sock*
  )

;----------------------------------------------
;      M-VERSE "TheCube" MOTOR COMMANDS
;----------------------------------------------

;----------- 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))
|#