(in-package 'USER)

;; -*- Mode: LISP; Syntax: Common-Lisp; Package: (USER); -*-
;-----------------------------------------------------------
;
;  Motor2.lisp
;  -contains Action Scheduler functions
;
; 'Ymir Agent System, 1995 
;
;  This file is meant to run on SPOT
;-----------------------------------------------------------

;  This file needs to be loaded before the act-defs.lisp file
;  Only load CLOS and this file for motor scheduler on Spot.
;
;  TO TRY OUT NEWLY ADDED ACTIONS:
;  call (make-all-objects)
;  then call (man-exe action).

;NOTES 5/1 95:
; Debugged and finished class defs and make classes
; Debugged trace mechanism, except haven't finished find-best
; Still have to finish scheduler.
; Idea: make sub-classes for each of the face parts, like
; brows, mouth, gaze, and make find-best use the classes
; to figure out best path through the graph.
;
; 11/15: Finished developing tracing mechanism
; 

(proclaim '(optimize (compilation-speed 0) (speed 3)))

;INCLUDES
;(unless (fboundp 'make-instance) 
;	(load "/mas/lib/ds/lisp/clos.mbin"))
;(load "/var/u/kris/ymir/foreign2.mbin")
;(load "/var/u/kris/ymir/my-net-io.mbin")
;(load "/var/u/kris/ymir/10-10-demo.lisp")

;
; DEF VARS
;

(defvar IN-BOX  '() "Incoming act commands, in the form of lists.") 
(defvar OUT-BOX '() "Outgoing, pending commands contained in Out-Obj.") ;this is a list of out-objects
(defvar RL       1) ;Reactive Layer
(defvar PCL      2) ;Process Control Layer
(defvar CL       3) ;Content Layer
(defvar CONT?    T) ;if CONT? = nil, stop trace-down and execute what we have.

;(setf initiator 1)


;----------------

(defun // (x y)
  (if (= y 0) (setf y 0.000000001))
  (* 1.0 (/ x y)))


(defclass out-act (act)
  ((fully-selected? :accessor fully-sel? :initarg :fully-sel? :initform nil)
   ;^have all options been explored...
   (satisfied?      :accessor satisfied? :initarg :satisfied? :initform nil)
   ;^does the current solution conflict at all with motor state?
   (sth-avail?      :accessor sth-avail? :initarg :sth-avail? :initform nil)
   ;^is there anyting we can currently use?
   (initiator       :accessor initiator  :initarg :initiator  :initform nil)
   ;^which system initiated the behavior asked for.
   (init-time       :accessor init-time  :initarg :init-time  :initform nil)
   ;^time-stamp from the initating system.
   (timeout         :accessor timeout    :initarg :timeout    :initform nil)
   ;^how long can this act wait until it should be executed.
   (start-time      :accessor start-time :initarg :start-time :initform 0)
   ;^when did the scheduler start issuing this out-act (necessary for correct delays)
   (messages        :accessor messages   :initarg :start-time :initform nil)
   ;^special info, like coordinates for spatial actions
   ))

(setf Out-Obj (make-instance 'out-act)) ;global to save on garbage collection and time.


;motor-state is used by select-option to determine which option
;should be selected when requests for actions come in.

(defclass Motor-State ()
  ; Each slot holds a tuple, first number is when started, second is how long it'll run.
  ; nil stands for "not busy"
  ((pupils   :accessor pupils   :initarg :pupils   :initform '(0 0))
   (pupil-m  :accessor pupil-m  :initarg :pupil-m  :initform '(Prh Prv Plh Plv))
   (l-brow   :accessor l-brow   :initarg :l-brow   :initform '(0 0))
   (l-brow-m :accessor l-brow-m :initarg :l-brow-m :initform '(Blm Blc Bll))
   (r-brow   :accessor r-brow   :initarg :r-brow   :initform '(0 0))
   (r-brow-m :accessor r-brow-m :initarg :r-brow-m :initform '(Brm Brc Brl))
   (l-eye    :accessor l-eye    :initarg :l-eye    :initform '(0 0))
   (l-eye-m  :accessor l-eye-m  :initarg :l-eye-m  :initform '(Elu Ell))
   (r-eye    :accessor r-eye    :initarg :r-eye    :initform '(0 0))
   (r-eye-m  :accessor r-eye-m  :initarg :r-eye-m  :initform '(Eru Erl))
   (mouth    :accessor mouth    :initarg :mouth    :initform '(0 0))         ;sides of the mouth
   (mouth-m  :accessor mouth-m  :initarg :mouth-m  :initform '(Mr Ml)) 
   (speak    :accessor speak    :initarg :speak    :initform '(0 0))         ;bottom of mouth+speech
   (speak-m  :accessor speak-m  :initarg :speak-m  :initform '(Mb Sp)) 
   (head-h   :accessor head-h   :initarg :head-h   :initform '(0 0))
   (head-h-m :accessor head-h-m :initarg :head-h-m :initform '(Hh))
   (head-v   :accessor head-v   :initarg :head-v   :initform '(0 0))
   (head-v-m :accessor head-v-m :initarg :head-v-m :initform '(Hv))
   ))


(defvar Motor-state nil)

(defun make-Motor-state ()
  (setf Motor-state (make-instance 'Motor-state)))


;------------------------------------------
; "MAKE" FUNCTIONS
;------------------------------------------

(defun prepare-motors (node-list)
  (loop for (name class ctrlpt exec-t) in node-list
        collect `(defparameter ,name
                     (make-instance ',class
                                    :name ',name
                                    :ctrlpt ',ctrlpt
                                    :exec-time ',exec-t)))
;calculate execution time here!!!
  )

(defmacro make-motors (node-list)   ;send it the list 'motor-list'
  (let ((forms (prepare-motors (symbol-value node-list))))
    `(progn ,@forms)))

(defun prepare-acts (node-list)
  (loop for (name class acts) in node-list
        collect `(defparameter ,name
                     (make-instance ',class
                                    :name ',name
                                    :acts ',acts)))
  )
                                    
(defmacro make-acts (node-list)  ;send it the list 'act-list'
  (let ((forms (prepare-acts (symbol-value node-list))))
    `(progn ,@forms)))

(defun make-one-act (name class)
  (setf name (make-instance class)))

(defun make-action-lists ()
  (setf *motors* '())
  (setf *acts* '())
  ;Make a list containing objects.
  (make-motors *motor-list*)
  (make-acts act-list)
  (dolist (sub-list *motor-list*)    
    (setf *motors* 
         (append *motors* (list (symbol-value (first sub-list))))))
  (dolist (sub-list act-list)
    (setf *acts* 
         (append *acts* (list (symbol-value (first sub-list))))))
  )


;------- MAKE ALL --------


(defun make-all-objects ()
  (make-motors *motor-list*)
  (make-acts act-list)
  (make-Motor-state)
  )


;------------------------------------------
; SCEDULING FUNCTIONS
;------------------------------------------

(defmethod init-object ((obj Out-Act))
  (setf (satisfied? obj) nil
	(fully-sel? obj) nil
	(initiator obj) 1
	(timeout obj) nil
	(start-time obj) 0
	(init-time obj)  0
	(sth-avail? obj) nil
	(messages obj) nil
	(acts obj) nil)
  T)

(defun test-sched ()
  (setf initiator 1)
  (setf now (time-stamp))
  (setf now-plus (+ now 300))
  (setf IN-BOX `((a 1 ,now ,now-plus)))
  (print IN-BOX)
  (schedule))

(defun test-act (act-name)
  (setf initiator 1)
  (setf now (time-stamp))
  (setf now-plus (+ now 400))
  (setf IN-BOX `((,act-name 1 ,now ,now-plus)))
  (print IN-BOX)
  (schedule)
  )

;3/7/96 here is an idea for coordinating head and gaze 
;-for now, just set gaze at 0-0 whenever the head turns.
;this is done in the cognitive system, not here...
(defvar head-state '(0 0)) ;globals for making sure the head is turned
(defvar gaze-state '(0 0)) ;correctly before gaze is...

(defun SCHEDULE ()
  (let ((current-request nil))
    (load "my-net-io") ;necessary to clear socket buffs or something...
    (if (y-or-n-p "Synchronize?") (sync))
    (init-record)
    (print "IN SCEDULEr")
    (print "WAITING FOR COMMANDS.")
    (init-object Out-Obj)
    (setf OUT-BOX nil)
    (setf IN-BOX nil)
    (loop                          ;THE ETERNAL SCHEDULER LOOP 
     (read-act-in-socket)
     (setf CONT? T)  ;this variable may be set to nil during rest of loop.
     ; NB! IN-BOX is a list of lists of the form: (act layer time-stamp time-out)
     (if IN-BOX  ; Do one act request per loop
	 (progn
;	   (print ">>>in-box-now: ")(princ in-box)
	   (init-object Out-Obj)
 	;TEMPLATE: (name initiator *now* now+limit &optional '(x y))
	   (setf current-request     (prioritize-incoming)) 
	   (setf (name Out-Obj)      (first  current-request)
		 (initiator Out-Obj) (second current-request)
		 (init-time Out-Obj) (third  current-request)
		 (timeout Out-Obj)   (fourth current-request)
		 ;coordinates to turn eyes & head
		 ;and string for 'deliver-speech:
		 (messages Out-Obj)  (fifth  current-request))
	   (trace-down (first current-request))
	   )
       )
;     (print 'out-box>>>>)(princ out-box)
     (update-Motor-state)
 ;    (describe out-obj)
     )                       ; <- eternal loop end
    ))

(defun prioritize-incoming ()
  "Receives in-box, returns an act to work on,
   prioritized by the system that initiated it."
  (let ((return nil)
	(type    RL)
	(satisfied nil))
    (loop while (not satisfied) do
	  (dolist (act IN-BOX)
		  (if (eq (second act) type)
		      (setf satisfied t
			    return act))
		  )
	  (setf type (+ type 1)))
    (setf IN-BOX (remove return IN-BOX))
    return
    ))

(defun Update-Motor-State ()
  "Calls execute on ready motor-commands
   and then updates Motor-state to reflect the changes."

  ; If an object in Out-Box has acts = nil, remove it from out-box
  ; Look for initiator = 1; take all motors with start-time = 0 and EXECUTE them
  ;                         remove them from the object's acts slot
  ;                         timestamp the object's delay slot with current time + delay of next motor
  ; Look for initiator = 2; repeat above
  ; Look for initiator = 3; repeat
  
  (if OUT-BOX                  ;if not empty, first check for virgin acts to timestamp...
      (dolist (obj OUT-BOX)
	      (if (acts obj)                                 ;If the act has motors and		  
		  (if (equal 0 (start-time obj))             ;the act is a virgin [=new] act ...
		      (setf (start-time obj) (time-stamp)))  ;timestamp it.
	      (setf OUT-BOX (remove obj OUT-BOX)))         ;Otherwise, remove it.
	      ))
  (if OUT-BOX                   ;still something left in the box
      (dolist (obj OUT-BOX)
	      (let ((out-list nil))
		(dolist (act (acts obj))
			(if (>= (time-stamp) 
				(+ (start-time obj) (/ (second act) 10)))
			    (progn
	;		      (print act)
			      (setf (acts obj) (remove act (acts obj)))
			      (push act out-list)))			
		      )
	;	(print out-list)
		(if out-list (motor-output out-list))
		)
	      )
    )
  )


;------------------------  ------
;
;------  TRACE DOWN -----  ------
;
;------------------------  ------


(defvar Opt-List nil "Stores the options to be selected between.")
(defvar Motor-List nil "Store the final motor selection for each option.")

(defmethod trace-down ((in symbol))
  (let* ((in-act (copy-act (symbol-value in)))
	 (Options-List nil)
	 (num-options (length (acts in-act)))
	 (count 0))
    (setf Opt-List nil
	  Motor-List nil
	  selection nil
	  CONT? t)
;    (setf (initiator Out-Obj) initiator) ;it's already been set!
    (if (not (Motor-Level in-act))
	(dolist (option (acts in-act))
		(let ((this-opt (make-instance 'act 
					       :acts (list option))))
	;	  (describe this-opt)
		  (push (trace-down this-opt) Options-List)
		  (select-and-post Options-List)
		  (setf Opt-List nil)
		  (setf Motor-list nil)
		  (Continue-Trace?)
		  (if CONT? (setf count (1+ count)))
	;	  (Update-Motor-State)
		  ))
      (select-and-post (acts in-act))
      )
    (if (= count num-options) (setf (fully-sel? Out-Obj) T))
  ;  (print options-list)
   ;Check if there is an extra slot with info on coordinates
    (if (messages Out-Obj)           
	(cond ((listp (messages Out-Obj))  ;if it has coordinates             
	       (setf x1 (first (messages Out-Obj))
		     y1 (second (messages Out-Obj)))
	       (setf count 0)
	       (dotimes (count-0 (/ (length (acts Out-Obj)) 2) nil)
			(setf x-act (nth count (acts Out-Obj)))
			(setf y-act (nth (+ 1 count) (acts Out-Obj)))
			(setf (fourth x-act) x1)
			(setf (fourth y-act) y1)
			(setf count (+ count 2)))
	       )
	      ((stringp (messages Out-Obj))   ;if it has speech
	       (setf (fourth (first (acts Out-Obj))) (messages Out-Obj)))
	      )
      ))
    ;Finally, push the selected action onto the OUT-BOX list
    (push (Copy-Act Out-Obj) OUT-BOX) ;ok to push--order don't matter
    ))

(defmethod trace-down ((in Act))
;  (print in)(princ (name in))
  (if CONT?
      (progn
	(if (Has-Options? in) 
	    (dolist (option (acts in))
		    (setf selection nil)
		    (if CONT?
			(dolist (item option)
				(trace-down (Make-Object item))
				))
		    (if CONT?
			(progn
			  (push (make-instance 'act :name (name in)) Opt-List)
		;	  (print 'opt-list->)(princ Opt-List)(print Motor-list)
			  (if Opt-List (setf (acts (first Opt-List)) Motor-List))
			  (setf Motor-List nil)
			  (if selection (setf (acts (first Opt-List)) 
					      (push selection (acts (first Opt-List)))))
		;	  (print 'options>>>>>L>>>>>L>L>L>L>L)(princ Opt-List)
			  (setf selection (Select-Option Opt-List))
		;	  (print 'selection>)(princ selection)
			  (if selection 
			      (progn
				(setf (acts (first Opt-List)) selection)
				(setf Opt-List (list (pop Opt-List)))     ;necessary?
		;		(print 'opt-list-now>)(princ opt-list)
				))
			  )
		      ))
	  (progn          ;else, if no options
	    (dolist (item (first (acts in)))
		    (trace-down (Make-Object item))
		;    (princ opt-list)
		    (if Opt-List (setf (acts (first Opt-List)) Motor-List))
		    )
	    )
	  )))
  (setf Opt-List nil)
  (if selection (progn 
		  (dolist (item (reverse selection)) (push item Motor-list))
		  (setf selection nil)))
;  (Update-Motor-State)
  Motor-List
  )

(defmethod trace-down ((in Mot-Lev)) 
  "Recieves a motor-level act, copies motors into Opt-List."
;  (Copy-down-time in)      ;will set the times for the motors.
  (dolist (motor (acts in))
	  (setf Motor-List (append Motor-List motor))
	  )
  )


;-------------------- ------
;------ Check options ------
;-------------------- ------

(defmethod Has-Options? ((in Act))
  (if (> (length (acts in)) 1)
    T
    nil))

(defmethod More-than-one-option? ((in list))
  (if  (> (length in) 1)
      T
    nil))

(defmethod Motor-Level ((act Act))
  (let ((item (acts act)))
    (loop while (not (atom item)) do
         (setf item (first item)))
    (if (eq (type-of (symbol-value item)) 'Motor)
       T
      nil))
  )

(defmethod Motor-Level ((act-or-options List))
  "It's ugly, but someone has to do it."
  (let* ((item1 (copy-tree act-or-options))
	 (item2 nil))
    (loop until (atom item1) do
	  (setf item1 (first item1)))
    (setf item1 (symbol-value item1))
    (setf item2 (acts item1))
    (loop until (atom item2) do
	  (setf item2 (first item2)))
    (if (eq (type-of (symbol-value item2)) 'Motor)
	T
      nil)
    ))


;---------------------------------
;
;------- SELECT ACT-OPTION -------
;
;---------------------------------


(defmethod Select-and-Post (option)
  "Receives a top-level choice, chooses and sets Out-Obj to the better."
;  (print 'select-and-post---->) (princ option)
  ; if out-act is nil, put option into out-act
  ;  otherwise, compare option to current out-act and choose the better one
  ;  change out-act to chosen act
  (let ((temp nil))
    (if (null (acts Out-Obj))
	(progn
	  (setf (acts Out-Obj) (first option)
		(sth-avail? Out-Obj) T)
	  )
      (progn
	(if (setf temp (Find-Best (first option) (second option)))
	    (setf (acts Out-Obj) temp)
	  (setf (acts Out-Obj) (first option)))
;	(print 'out-obj>)(describe out-obj)
	)
      )
    ))

(defmethod Select-Option ((options List))
;  (print 'opitons>>>>>>>>>>>>)(princ options)
  (let ((select nil) foo)
  ; 1. Compare to Motor-state  
  ; 2. select
  ; 3. call Continue-Trace? or scheduler function to set the CONT? variable
  ;    to determine if we should continue to refine current behavior
  ; Perhaps use a special continue function that both scheduler and select call...
    (cond ((= (length options) 2)
	   (setf select (Find-Best (acts (first options)) 
				   (acts (second options))))
	   )
	  ((= (length options) 1)
	   (setf select (acts (first options)))
	   )
	  ((> (length options) 2)
	   (setf select (Find-Best options foo))
	   )
	  (t nil))
    (if select 
	select  ;then return
      nil)
    ))

(defmethod Find-Best ((options list) foo)
  "When there are more than two options: receives a list 
   of objects, returns a list of motor lists."
  (let ((best-choice (pop options)))
    (dolist (option options)
	    (setf best-choice (Find-Best (acts option) (acts best-option)))
	    )
    best-choice
    ))

(defmethod Find-Best ((option1 list)(option2 list))
  "Receive two options, compares to Motor-state, returns the better."
  ; First compare motor availability, set motor-index
  ; Then, select the higher motor-index,
  ; otherwise, if motor-indexes are equal, compute
  ; exec-time, select shorter exec-time if initator is RL,
  ; else, if initiator is PCL or CL, select at random?
 ; (print 'O1>)(princ option1)(print 'o2>)(princ option2)
  (let ((motor-index1 (compute-motor-index option1))
	(motor-index2 (compute-motor-index option2)))
    (cond ((equal motor-index1 motor-index2)
	   (let ((exec-time1 (compute-exec-time option1))
		 (exec-time2 (compute-exec-time option2)))
	;     (print 'motor-index-is-equal!)
	     (cond ((equal exec-time1 exec-time2)
	;	    (print 'exec-times-are-equal!) 
		    nil)
		   ((> exec-time1 exec-time2)
		    option2)
		   ((>= exec-time2 exec-time1)
		    option1)
		   )
	     ))
	  )
    ))

(defun compute-exec-time (act)
  (let (collect)
    (dolist (item act)
;	    (print item)
;	    (print 'number1)
	    (push (+ (second item) (third item)) collect)
	    )
    (find-highest collect)
    ))

(defun compute-motor-index (act)
  "Receives an act option--a list of acts--and returns an
   index that = 1 if all motors needed for that option are available."
  (let* ((num-motors (length act))
	 (count num-motors)
	 (act-name nil)
	 (now (time-stamp)))
;    (print 'act>>>)(princ act)
;    (print 'num-motors)(princ num-motors)
    (dolist (sub-act act)
	    (setf act-name (first sub-act))
;	    (print act-name)
	    (cond ((and (member act-name (Pupil-M Motor-State))
			(< now (first (Pupils Motor-State))))
		   (setf count (- count 1)))
		  ((and (member act-name (L-brow-M Motor-State))
			(< now (first (L-brow Motor-State))))
		   (setf count (- count 1)))
		  ((and (member act-name (R-brow-M Motor-State))
			(< now (first (R-brow Motor-State))))
		   (setf count (- count 1)))
		  ((and (member act-name (R-Eye-M Motor-State))
			(< now (first (R-Eye Motor-State))))
		   (setf count (- count 1)))
		  ((and (member act-name (R-Eye-M Motor-State))
			(< now (first (R-Eye Motor-State))))
		   (setf count (- count 1)))
		  ((and (member act-name (Mouth-M Motor-State))
			(< now (first (Mouth Motor-State))))
		   (setf count (- count 1)))
		  ((and (member act-name (Speak-M Motor-State))
			(< now (first (Speak Motor-State))))
		   (setf count (- count 1)))
		  ((and (member act-name (Head-h-M Motor-State))
			(< now (first (Head-h Motor-State))))
		   (setf count (- count 1)))		  
		  ((and (member act-name (Head-v-M Motor-State))
			(< now (first (Head-v Motor-State))))
		   (setf count (- count 1)))
		  )
	    )
    (if (equal 0 count)
	count
      (// num-motors count))
    ))

(defun Continue-Trace? ()
  "Decide if continue looking at options for the current request
   by using the timeout of the incoming request as posted in Out-Obj
   and current time."
;  (if (curr-request is satisfied and its (timeout has come setf CONT? to nil
  (if (timeout out-obj) ;just to make sure it's not nil
      (if (and (satisfied? Out-Obj)
	       (>= (timeout Out-Obj) (+ (time-stamp) (timeout Out-Obj))))
	  (progn
	    (setf Cont? nil)
	    (princ 'x))
	))
  )

;----------------------------------------------------------------
;
;                    MISCELLANEOUS FUNCTIONS
;
;----------------------------------------------------------------

(defmethod Make-Object ((a-list list))
  (let* ((return nil)
	 (the-obj (copy-act (symbol-value (first a-list))))
	 (the-acts (acts the-obj))
;	 (start (delay the-obj))
	 (start (second a-list))
	 (name (first a-list))
	 (pos  (second a-list))
	 (exec-t (third a-list)))
    (cond ((and (atom name)
		(not (eq (type-of the-obj) 'Motor)))
	   (progn
	     (setf return (make-instance (type-of the-obj)
					 :acts the-acts
					 :delay start
					 :name name
					 :exec-time exec-t))
	     (Copy-down-time return)
;	     (describe return)
;	     (read-char)           ;This is a good place to stop when debugging.
	     ))
	  (T
	   (setf return  (make-instance (type-of the-obj)
					:name name
					:pos  pos
					:exec-time exec-t))
	   )
	  )  ;end cond
    return
    ))

(defmethod calc-scalar ((option list) e-time)
  "Receives an option, returns a number to scale exec-times and start-times with."
  (let ((item nil)
	(the-list nil))
    (loop while option do                
          (setf item (pop option))       
          (push (+ (second item) (third item)) the-list)
          )       
    (// e-time (find-highest the-list))
    ))

(defun find-highest (a-list)
  (let ((old (first a-list)))
    (dolist (x a-list)
      (if (> x old) (setf old x)))
    old))

(defmethod Copy-down-time ((action Act))
  "Sets the time for every item in the action's options, returns action modified."
  (let ((e-time (exec-time action))
        (scalar nil)
	(start (delay action)))
 ;   (print 'incopydown)
    (dolist (option (acts action))               ;time from above affects all options
	    (setf scalar (calc-scalar option e-time))
	    (dolist (item option)                      ;and all elements of each option
		    (setf (second item) (+ (* 1.0 (second item) scalar) start)) ;Intermediate values are floats....
	;	    (print 'second>>>>>)(princ (second item))
		    (setf (third  item) (* 1.0 (third  item) scalar))
		    ))
    action
    ))

(defmethod Copy-down-time ((action Mot-Lev))
  "Receives motor-level act, copies down the times."
  (let ((e-time (exec-time action))
	(start  (delay action))
        (scalar nil))
    ; copy down times...
 ;   (print 'incopydownmotortimes)(describe action)
    (dolist (option (acts action))                   ;time from above affects all options
	    (setf scalar (calc-scalar option e-time))
	 ;   (print 'scalar-now>)(princ scalar)
	    (dolist (item option)                    ;and all elements of each option		
	;	    (print 'item>)(princ item)
	;	    (print (second item))
		    (setf (second item) (round (* (second item) scalar)))
		    (setf (second item) (round (+ (second item) start)))
	;	    (print (second item))
	;	    (print (third item))
		    (setf (third  item) (round (* (third item) scalar)))
	;	    (print (third item))
		    )
	    )
    (setf (exec-time action) e-time)
;    (describe action)
    ))

(defmethod Copy-Act ((object Act))
  "Receive a global object, return a local copy of it."
  (make-instance (type-of object) 
                :name (name object)
                :exec-time (exec-time object)
                :delay (delay object)
                :acts (copy-tree (acts object))
                )
  )

(defmethod Copy-Act ((object out-act))
  "Receive an out-act, return a new object w/same values."
  (make-instance 'out-act
		 :name  (name  object)
		 :acts  (acts  object)
		 :delay (delay object)
		 :exec-time  (exec-time  object)
		 :fully-sel? (fully-sel? object)
		 :satisfied? (satisfied? object)
		 :sth-avail? (sth-avail? object)
		 :initiator  (initiator  object)
		 :init-time  (init-time  object)
		 :timeout    (timeout    object)
		 :start-time (start-time object)
		 )
  )

(defmethod Motors? ((item list))
  (loop while (not (atom item)) do
       (setf item (first item)))
  (if (eq (type-of (symbol-value item)) 'Motor)
      T
    nil)
  )


;---------------------
;------ Execute ------
;---------------------

(defmethod EXECUTE ((action list))
  "Formats the motor act for socket transmission."
;  (print (list (ctrlpt (symbol-value (first action))) (third action) (fourth action)))
  (motor-output (ctrlpt (symbol-value (first action))) (third action) (fourth action))
  )

(defmethod man-exe ((action act))
  (load "my-net-io")
  (manual-execute action))

(defmethod me ((action act))
  (manual-execute action))

(defmethod manual-execute ((action act))
  "This function describes the idea behind the scheduling in a nutshell."
  (init-object Out-Obj)
  (setf OUT-BOX nil)
  (trace-down (name action))
  (loop while OUT-BOX do
	(Update-Motor-State)
	)
  )


;------------------------------------------
;              MOTOR OUTPUT
;------------------------------------------


; Dimension
(defconstant horiz 0)
(defconstant verti 1)
; Start and stop codes
(defvar start 9998)
(defvar stop  9999)
(defvar QUIT  9997)


(defun out (ctrlpt exec-time abs-pos)
  (motor-output ctrlpt exec-time abs-pos))

;(defun say (utter)
;  (motor-output 0 nil utter))

(defun say (utter)
  (motor-output (list (list 'Sp nil nil utter))))

(defun MOTOR-OUTPUT2 (ctrlpt time pos)
  (if face-stream             ;
      (if (eq ctrlpt 0)       ;if speech
	  (let ((words pos)
		(string nil)
		(f-stream face-stream))
	    (setf string (format nil "~A ~A ~A"
				 ctrlpt words #\newline))
	    (transmit-buf f-stream string))
	(let ((dir verti)     ;else
	      (string nil)
	      (f-stream face-stream))
	  (if (> ctrlpt 50)
	      (progn
		(if (and (> ctrlpt 11)
			 (oddp ctrlpt))
		    (setf dir horiz))
		(setf ctrlpt (round (* ctrlpt 0.1)))))
	  (progn
	    (setf string (format nil "~A ~A ~A ~A ~A ~A"
				 ctrlpt pos dir time #\newline))
	    (transmit-buf f-stream string)
	    (princ "."))
	  ))
    (progn                       ;Debug block
      (terpri)  (terpri)
      (princ "Face-stream closed. Data: ")
      (princ (list ctrlpt time pos))(princ " stamp: ")
      (princ (time-stamp))
      ))
  )

(defmacro make-resize-string (len)
  "Create an unitialized string of length `len' which is adjustable
   and has a fill-pointer. The fill-pointer is set to zero so the string
   initially appears empty."
  `(make-array  ,len  :element-type 'string-char :adjustable t :fill-pointer 0))

;(defvar speech-begin '#\$)
;(defvar motor-begin '#\#)
(defvar horizont '#\0)
(defvar vertical '#\1)

(defun Motor-Output (motors)   ;motors is a list of motor lists
  "It's ugly but it works.
   FORMAT: (list (list ctrlpt delay time pos))"
  (let ((record (make-resize-string 0))
	(ctrlpt nil)
	(data nil)
	(pos  nil)
	(controlpoint nil)
	(foo nil))
;    (print motors)
    (dolist (motor motors)
	    (setf ctrlpt (ctrlpt (symbol-value (first motor))))
	    (if (equal 0 ctrlpt) ;speech
		(let ((words (fourth motor)))
		  (setf record (write-it record '#\$))       ;Speech record
		  (setf record (write-it record #\Space))
		  (dotimes (count2 (length words) foo)
			   (setf record (write-it record (aref words count2))))
		  (setf record (write-it record #\Newline))
		  )  ;else
	      (let ((dir  vertical)
		    (pos  (write-to-string (fourth motor)))    
		    (time (write-to-string (third  motor))))
		(if (> ctrlpt 50)
		    (progn
		      (if (and (> ctrlpt 11) (oddp ctrlpt))
			  (setf dir horizont))
		      (setf ctrlpt (round (* ctrlpt 0.1)))
		      ))
		(setf controlpoint (write-to-string ctrlpt))
		(setf record (write-it record #\#))          ;Motor record
		(setf record (write-it record #\Space))
		(dotimes (count2 (length controlpoint) foo)
			 (setf record (write-it record (aref controlpoint count2))))		
		(setf record (write-it record #\Space))
		(setf record (write-it record dir))
		(setf record (write-it record #\Space))
		(dotimes (count2 (length pos) foo)
			 (setf record (write-it record (aref pos count2))))
		(setf record (write-it record #\Space))
		(dotimes (count2 (length time) foo)
			 (setf record (write-it record (aref time count2))))
		(setf record (write-it record #\Newline))
		)
	      )
	    )
    (if face-stream
	(progn
	  (transmit-buf face-stream record)
;	  (print record)(princ (time-stamp))  
	  (if *record-flag* (record-it record))
	  (princ ".")
	  )
      (progn                       ;Debug block
	(terpri)
	(princ "Face-stream closed. Data: ")
	(print record)
	(princ (time-stamp))
	))
    ))

(defun write-it (record item)
  (vector-push-extend item record 1)
  record)

(defun debug-write (ctrlpt pos time)
  (write start :stream *standard-output*)
  (write ctrlpt :stream *standard-output*)
  (write pos :stream *standard-output*)
  (write dir :stream *standard-output*)
  (write time :stream *standard-output*)
  (write stop :stream *standard-output*)
  )


;------------------------------------------
;
;              BOTH SOCKETS
;
;------------------------------------------

(defun os ()
  (open-sockets))

(defun obs ()
  "Open Both Sockets"
  (os))

(defun open-sockets ()
  (if (y-or-n-p "Open Face Stream? {Splotch}: ")
      (open-face-socket (progn (print "Port number: ")(read-line))))
  (if (y-or-n-p "Open Act-In socket? {Sparta}: ")
      (open-act-in-socket))
  )

(defun cs ()
  (close-sockets))

(defun close-sockets ()
  (close face-stream)
  (close-in-act-socket)
  )

(defvar face-sock# 4050)

(defun ofs () 
  "Open Face Socket/Stream"
  (manual-socket face-sock#))

(defun oos (numb)
  (manual-socket numb))

(defun ms (numb)
  (manual-socket numb))

(defun ois ()
  "Open Acts Socket/Stream"
  (open-act-in-socket))

(defun ors ()
  "Open Read Socket."
  (open-act-in-socket))

(defun ows ()
  "Open Write Socket"
  (open-socket face-sock#))

(defun cfs ()
  "Close Face Sock/Stream"
  (setf face-stream nil)
  (close-face-stream))

(defun cis ()
  "Close In Sock/Stream"
  (close-act-in-socket))

(defun manual-in-sock (num)
  (setf act-in-socket-# num)
  (open-act-in-socket))

;------------------------------------------
;              INPUT SOKCET
;------------------------------------------

(defvar act-in-socket-# 1998)
(setf   act-in-socket-# 1999)
(defvar act-in-socket-status nil)
(defvar *in-acts-socket* nil)

(defun open-act-in-socket ()
  "Open the IN socket for receiving acts from the Alpha."
  (setf IN-BOX nil) ;empty buffer...
  (terpri)(princ "Setting up connection to Dialogue System.")
  (terpri)(princ "Waiting for connection on socket #")
  (princ act-in-socket-#)(princ " ......")
  (if
      (and (setf *in-acts-socket*
                 (wait-for-socket act-in-socket-#))
           (numberp *in-acts-socket*))
      (progn 
        (terpri)(princ "Connected to Dialogue System.")
        (setf act-in-sock-status t))
    (progn
      (print "Error (open-act-socket): Dialogue System connection did not open!")
      (print *in-acts-socket*))) ;In which case this is an error message.
  *in-acts-socket*
  )

(defun close-act-in-socket ()
  (close-socket *in-acts-socket*)
  (setf act-in-sock-status nil)
  (print "Act-In-Socket now closed."))

(defun sched-socket-status ()
  (terpri)
  (princ "act-in-socket-#: ")(princ act-in-socket-#)
  (terpri)
  (princ "*in-act-socket*: ")
  (if act-sock-status (princ " (apparently open).")
    (princ " (apparently closed)."))
  (terpri)
  (princ "animation-socket-#: ")(princ animation-socket-#)
  (terpri)
  )

;------------------------
;------ Read Input ------
;------------------------
   
(defun read-act-in-socket ()
  "Read all data available in act socket and store in a buffer."
  (let ((in (receive-line-no-hang *in-acts-socket*)))
    (if in 
	(progn
	  (setf IN-BOX (append IN-BOX  (list (read-from-string in)))) ;make a FIFO for act requests
	  ))
    ))

;	  (print "in read act ")(princ in)(print (length in))


;-------------------------------------------------
;
;          FACE OUTPUT SOCKET CONNECTION
;
;-------------------------------------------------


(defvar face-socket# 4000)
(setf   face-socket# 4000)
(defvar face-cmnd-sock# 4050)
(setf   face-cmnd-sock# 4050)
(defvar face-stream nil)
(defvar face-host-name "splotch")

(defun summon-gandalf ()
  (startup-face)
  (sleep 3)
  (open-com-sock)
  (close-socket face-startup-stream)
  (init-voice))

(defun startup-face ()
  "Open connection to face animation subsystem."
  (terpri) (princ "Calling FACE program. Socket: ")  (princ face-socket#)
  (if (numberp (setf face-startup-stream (open-socket face-host-name face-socket#)))
      (progn
        (terpri)
        (princ "Connected to ")(princ face-host-name)(princ " on above port.")
	(terpri)(princ "Stream #: ") (princ face-startup-stream)
        ) 
    (progn
      (print "Error in face-anim-sock, connection to FACE not made.")
      face-startup-stream)
    )
  )

(defun close-face ()  (close-face-stream))

(defun open-com-sock ()  (open-face-cmnd-sock))

(defun open-face-cmnd-sock ()
  "Open connection to face animation subsystem."
  (terpri) (princ "Calling command socket for ToonFace. Socket: ")  (princ face-cmnd-sock#)
  (If (numberp (setf face-stream (open-socket face-host-name face-cmnd-sock#)))
      (progn
        (terpri)
        (princ "Connected to ")(princ face-host-name)(princ " on above port.")
	(terpri)(princ "Stream #: ") (princ face-stream)
	)
    (progn
      (print "Error in face-anim-sock, connection to FACE not made.")
      face-stream)
    )
  )

(defun close-face-cmnd-sock ()
  (close-socket face-stream))

(defun close-face-sock ()
  (close-socket face-stream)
  (setf face-stream nil)
  (close-socket face-startup-stream))

(defun close-face-stream ()
  (close-face-sock))

(defun manual-socket (socket-num)
  (setf face-stream (open-socket "splotch" socket-num))
  )

(defun init-voice ()  (say "[:nr :dv ap 130 :ra 200]"))

;;____________________________________
;;
;;   TIME SERVER SYNCHRONIZATION
;;
;; For this routine to work,
;; timeserv2 has to be running
;; somewhere.
;;
;; modified 2/2 '96 for compatibility
;; with timeserv2
;;------------------------------------

(defvar splotch-sync-sock# 7766)

;(defvar spud-sync-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 (host-name host-port)
  (load "my-net-io")
  (let ((sock (open-socket host-name host-port))
        (time-offset nil)
        (local-time nil)
        (time-in nil)
        (temp-time nil))     ;A time marker to notify client mach. of timegrab.
    (if (not (atom sock))
	(progn
	  (print "TIME port did not open!")(princ sock)
	  (print "Make sure '/mas/disks/ahi/timserver/timeserv2 > /dev/null &' is running."))
      (progn
	(setf *sock* sock)    
	(terpri)(princ "SYNCHRONIZING")
	(terpri)(princ "Asking for time ...")
	(transmit-buf sock "cent")
	(loop while (not (receive-char-no-hang sock)))    ;loop until host sends "mark" ... and then
	(setf local-time (get-time-stamp))        ;read this as soon as that one returns. 
	(loop while (not (setf temp-time (receive-line-no-hang sock))))
	(setf time-in (read-from-string temp-time))
	(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)
	      (transmit-buf sock "bye ")	      
	      (close-sync-sock sock))
	  (princ "Time offset not read correctly."))
	)
      )
    (setf *time-offset* time-offset)
    ))

(defun close-sync-sock (sock)
  (print 'CLOSING)
  (let ((return nil))
    (terpri)
    (if (eq (setf return (close-socket  sock)) -1)
	(format nil "Time socket did not close / is already closed. Error: ~A" return)
      (princ "Time socket closed."))
    ))

(defun sync ()
  (synchronize "splotch" splotch-sync-sock#))
 
(defvar *time-offset* 0 "Maintains diff. betw. speech and local clock.")
(setf   *time-offset* 0)

(defun time-stamp ()
  "Returns time in hundreds of seconds (centiceconds)."
  (-  (round (/ (get-internal-real-time) 
		(/ internal-time-units-per-second 100)))
      *time-offset*)) ;This is subtracted to synchronize to speech & body.

(defun get-time-stamp ()
  "Returns time in hundreds of seconds (centiceconds), without subtracting offset."
  (round (/ (get-internal-real-time) 
	    (/ internal-time-units-per-second 100))))

(defun stamp ()
  (time-stamp))

(defun test-time-sync ()
  (let ((sock (open-socket host-name host-port))
        (time-offset nil)
        (local-time nil)
        (time-in nil)
        (temp-time nil))     ;A time marker to notify client mach. of timegrab.
    (loop
     (transmit-buf sock "cent")
     (loop while (not (receive-char-no-hang sock)))    ;loop until host sends "mark" ... and then
     (setf local-time (get-internal-real-time))        ;read this as soon as that one returns. 
     (loop while (not (setf temp-time (receive-line-no-hang sock))))
     (setf time-in (read-from-string temp-time))
     (terpri)(princ "Local time:     ")(princ local-time)
     (terpri)(princ "Time from host: ")(princ time-in)
     (setf time-offset (- local-time time-in))
     (sleep 0.5)
     )
    ))


;----------------------------------------
;               RECORDER
;----------------------------------------

(setf *record-list nil)
(setf *record-flag nil)

(defun init-record ()
  (if (y-or-n-p "Record next session?")
      (progn
	(setf *record-flag* T)
	(if (y-or-n-p "Reset *record-list*?")
	    (setf *record-list* nil))
	)
    (setf *record-flag* nil))
  )

(defun Record-It (out-action)
  (push (list out-action (time-stamp)) *record-list*)
  )

(defun Playback ()
  (if (y-or-n-p "Synchronize? [recommended]") (sync))
  (let* ((record-list-copy (copy-list (reverse *record-list*)))
	 (offset (- (time-stamp) (second (first record-list-copy))))
	 (xtra-delay -20)) ;to make it start promptly
    (terpri)(princ "In PLAYBACK ....")
    (loop while (and record-list-copy (not (read-char-no-hang))) do
	  (if (< (+ xtra-delay (second (first record-list-copy))) (- (time-stamp) offset))
	      (progn 
		(princ ".")
		(transmit-buf face-stream (first (pop record-list-copy))))
	    )
	  )
    (terpri)(princ "Finished PLAYBACK.")
    T
    ))
	    
;----------------------------------------
;(make-all-objects) now done in act-defs 3/4/96