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

;;;  Calls to the Truckworld.

;;;  Implements the following functions 

;;;      (called from truck.lisp):

;;;

;;;     TI-START-SIMULATOR

;;;     TI-STOP-SIMULATOR

;;;     TI-RESTART-SIMULATOR  

;;;         (this is for your debugging purposes only)

;;;     TI-EXECUTE-COMMAND

;;;     TI-EXECUTE-COMMANDS

;;;

;;;  Makes calls to 

;;;     START-SIMULATOR (in the Truckworld code)

;;;     STOP-SIMULATOR  (in the Truckworld code)

;;;     FFCP-EXECUTE    (in the Truckworld code)

;;;

;;;  This code assumes the following about the simulator 

;;;  interface:

;;;     -- the function (MAKE-STANDARD-WORLD) creates the world

;;;     -- the function (MAKE-STANDARD-TRUCK) creates the truck

;;;     -- the truck's initial placement is at the first location

;;;          in the world's map.

;;;  These can all be changed by setting global variables below.

;;;     



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

;;;  Globals





;;;  We need to remember these parameters in case 

;;;  we need to restart the world.

;;;  These will be set in TI-START-SIMULATOR.



(defvar *default-world-form* '(make-standard-world))

(defvar *default-truck-form* '(make-standard-truck))



;;;  Number of seconds to sleep between executing 

;;;  Truckworld primitives when a command sequence

(defvar *sleep-delay* 0.1)



;;;  End globals

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



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

;;;  Starting and stopping the simulator



(defun ti-start-simulator () 

  (start-simulator 

   :world-form *default-world-form*

   :truck-form *default-truck-form*

   :command-processor 'friendly-functional-cp

   :display :NONE))



(defun ti-stop-simulator ()

  (terminate-simulator))



(defun ti-restart-simulator ()

  (ffcp-execute `(restart ,*default-world-form*

                          ,*default-truck-form*  

			  ,(query (first (nodes (world-map *the-world*))) 'id))))



;;;  Execute a single Truckworld command.  The Truckworld 

;;;  returns lots of information in its command return, including

;;;  the current time, a symbol indicating whether or not the 

;;;  execution completed successfully, and the result of any 

;;;  sensor information.  This command ignores everything except 

;;;  the sensor information (if any), and returns it as the 

;;;  result of the function call.



(defun ti-execute-command (command)

  (multiple-value-bind (time res)

      (ffcp-execute command)

   (debug-output :EXECUTION "Executed primitive ~a, result ~a~%" command (third (car res)))

   (third (car res))))



;;;  Execute a sequence of Truckworld commands, with a delay 

;;;  of *sleep-delay* between each.  The delay means things 

;;;  don't flash by too quickly on the display.



(defun ti-execute-commands (commands)

  (let ((cmd-result NIL))

    (dolist (command commands)

      (setf cmd-result (ti-execute-command command))

      (sleep *sleep-delay*))

    cmd-result))