private/simulation-control.ss
#lang scheme
;;; PLT Scheme Simulation Collection
;;; simulation-control.ss
;;; Copyright (c) 2004-2008 M. Douglas Williams
;;;
;;; This library is free software; you can redistribute it and/or
;;; modify it under the terms of the GNU Lesser General Public
;;; License as published by the Free Software Foundation; either
;;; version 2.1 of the License, or (at your option) any later version.
;;;
;;; This library is distributed in the hope that it will be useful,
;;; but WITHOUT ANY WARRANTY; without even the implied warranty of
;;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
;;; Lesser General Public License for more details.
;;;
;;; You should have received a copy of the GNU Lesser General Public
;;; License along with this library; if not, write to the Free
;;; Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
;;; 02111-1307 USA.
;;;
;;; -------------------------------------------------------------------
;;;
;;; This moduce implements the simulation control routines.
;;;
;;; Note that currently only the future event list is used for
;;; scheduling events.
;;;
;;; Version  Date      Description
;;; 0.1.0    10/16/04  Initial implementation of the simulation control
;;;                    routines.  (Doug Williams)
;;; 0.1.1    03/15/05  Added interrupt and resume routines (Doug
;;;                    Williams)
;;; 0.1.2    07/12/05  Added continuous simulation. (Doug Williams)
;;; 0.1.3    07/23/05  Added hierarchical control.
;;; 1.0.0    02/17/06  Cleaned up the loops and nested ifs.  (Doug
;;;                    Williams)
;;; 1.0.1    02/19/06  Added linked events and priorities.  (Doug
;;;                    Williams)
;;; 1.0.2    03/24/06  Changed with to when in schedule macro.  (Doug
;;;                    Williams)
;;; 1.1.0    04/23/06  Added monitor hook to the main loop.  It is
;;;                    called each time the simulation clock is about
;;;                    to be advanced.  (Doug Williams)
;;; 1.1.1    04/04/07  Added process monitors.  A process monitor is
;;;                    called each time an event for that process is
;;;                    executed.  (Doug Williams)
;;; 1.1.2    04/30/07  Extended process monitors to correctly handle
;;;                    continuous processes.  (Doug Williams)
;;; 3.0.0    06/28/08  Updated for V4.0.  (Doug Williams)
;;; 3.0.1    11/27/08  Converted to a module.  (Doug Williams)

(require "simulation-environment.ss"
         "event.ss"
         "process.ss"
         "set.ss"
         "variable.ss")
(require scheme/mpair)
(require (planet williams/science/ode-initval))

(provide (all-defined-out))

;;; schedule macro
;;; Schedule a process or event for execution in the current
;;; simulation environment.
(define-syntax schedule
  (syntax-rules (now at in when)
    ((schedule now (function . arguments) #:priority priority)
     (schedule '#:now (function . arguments) priority))
    ((schedule now (function . arguments))
     (schedule '#:now (function . arguments)))
    ((schedule (at time) (function . arguments) #:priority priority)
     (schedule time (function . arguments) priority))
    ((schedule (at time) (function . arguments))
     (schedule time (function . arguments)))
    ((schedule (in delay) (function . arguments) #:priority priority)
     (schedule (+ delay (current-simulation-time))
               (function . arguments) priority))
    ((schedule (in delay) (function . arguments))
     (schedule (+ delay (current-simulation-time))
               (function . arguments)))
    ((schedule (when event) (function . arguments) #:priority priority)
     (schedule event (function . arguments) priority))
    ((schedule (when event) (function . arguments))
     (schedule event (function . arguments)))
    ((schedule time (function . arguments))
     (schedule time (function . arguments) 0))
    ((schedule time (function . arguments) priority)
     (if (process-def? function)
         (let ((process (make-process function (list . arguments))))
           (set-event-priority! (process-event process) priority)
           (schedule-event (process-event process) time (current-simulation-environment))
           process)
         (let ((event (make-event time priority #f function (list . arguments))))
           (schedule-event event time (current-simulation-environment))
           event)))))

;;; schedule-event: event x time -> void
;;; schedule-event: event -> void
;;; Schedule an event for execution in the current simulation
;;; environment.
(define schedule-event
  (case-lambda
    ((event time environment)
     (cond ((real? time)
            (set-event-time! event time)
            (event-list-add! (simulation-environment-future-event-list
                              environment)
                             event))
           ((eq? time '#:now)
            (set-event-time! event (simulation-environment-time
                                    environment))
            (event-list-add! (simulation-environment-now-event-list
                              environment)
                             event))
           ((eq? time '#:continuous)
            (set-event-time! event (simulation-environment-time
                                    environment))
            (event-list-add! (simulation-environment-continuous-event-list
                              environment)
                             event))
           ((event? time)
            (when (not (event-linked-event-list time))
              (set-event-linked-event-list! time (make-event-list)))
            (set-event-time! event (event-time time))
            (event-list-add! (event-linked-event-list time) event))
           ((process? time)
            (schedule-event event (process-event time) environment))
           (else
            (error 'schedule-event
                   "Illegal time specification ~a" time))))
    ((event time)
     (schedule-event event time (current-simulation-environment)))
    ((event)
     (event-list-add! (current-simulation-future-event-list) event))))

;;; make-and-schedule-event:
;;;   real x process x procedure x list -> void
;;; Create and schedule an event.  This is just a shortcut for the two
;;; operations.
(define (make-and-schedule-event time priority process function arguments)
  (let ((event (make-event +inf.0 priority process function arguments)))
    (schedule-event event time)
    event))

;;; start-simulation
;;; Start the simulation main loop.  Exits when there are no more
;;; events to execute or when some routine explicitly calls the
;;; loop-exit continuation.
(define (start-simulation)
  (let ((current-environment (current-simulation-environment)))
    (let/ec exit
      ;; Save the main-loop exit continuation
      ;;(current-simulation-loop-exit exit)
      (set-simulation-environment-loop-exit!
       current-environment exit)
      ;; Simulation main loop
      (let main-loop ()
        ;; Update the simulation state
        (let/cc next
          ;; Save the main-loop next continuation
          ;;(current-simulation-loop-next next)
          (set-simulation-environment-loop-next!
           current-environment next)
          ;; Determine the next event and how to advance the time
          ;; (discretely or continuously), if necessary.
          ;; The simulation monitor, if any, is executed before
          ;; advancing the clock for both discrete and continuous
          ;; events.
          (cond ((not (event-list-empty?
                       (current-simulation-now-event-list)))
                 ;; Execute the next now event
                 (let ((event (event-list-pop!
                               (simulation-environment-now-event-list
                                current-environment))))
                   (execute-discrete-event event current-environment)))
                ;; Now event list is empty
                ((not (event-list-empty?
                       (simulation-environment-future-event-list
                        current-environment)))
                 ;; The future event list is not empty
                 (let ((event (mcar (event-list-events
                                     (simulation-environment-future-event-list
                                      current-environment)))))
                   (if (and (not (event-list-empty?
                                  (simulation-environment-continuous-event-list
                                   current-environment)))
                            (< (simulation-environment-time
                                current-environment)
                               (event-time event)))
                       ;; There are continuous events and we are not
                       ;; at the new time, so advance time continuously
                       (execute-continuous-events
                        (event-time event) current-environment)
                       ;; Otherwise, advance the time and execute the event
                       (begin
                         (set! event
                               (event-list-pop!
                                (simulation-environment-future-event-list
                                 current-environment)))
                         ;; Exit if time advances to infinity
                         (when (= (event-time event) +inf.0)
                           (exit))
                         ;; Execute the simulation monitor here
                         (when (simulation-environment-monitor
                                current-environment)
                           ((simulation-environment-monitor
                             current-environment)))
                         ;;(current-simulation-time (event-time event))
                         (set-simulation-environment-time!
                          current-environment (event-time event))
                         (execute-discrete-event event current-environment)))))
                ;; The future event list is empty
                ((not (event-list-empty?
                       (simulation-environment-continuous-event-list
                        current-environment)))
                 ;; There are continuous events so advance time
                 ;; to infinity.  (Hopefully, some of the cont-
                 ;; inuous events have termination conditions.
                 ;; Otherwise, the simulation will never end.)
                 (execute-continuous-events +inf.0 current-environment))
                ;; Otherwise, there is nothing left to do.
                (else
                 (exit))))
        ;; Execute the process monitor, if there is one
        (let ((process (simulation-environment-process
                        current-environment)))
          (when process
            (let ((monitor (process-monitor process)))
              (when monitor
                (monitor process)))))
        (main-loop)))
    ;; Reset simulation environment
    ;;(current-simulation-event #f)
    (set-simulation-environment-event!
     current-environment #f)
    ;;(current-simulation-process #f)
    (set-simulation-environment-process!
     current-environment #f)
    ;;(current-simulation-loop-exit #f)
    (set-simulation-environment-loop-exit!
     current-environment #f)
    ;;(current-simulation-loop-next #f)
    (set-simulation-environment-loop-next!
     current-environment #f)))

;;; execute-discrete-event: simulation-environment x event -> void
;;; Execute the given event in the current simulation environment.
(define (execute-discrete-event event current-environment)
  (let ((process (event-process event)))
    ;; If there are linked event, schedule them to execute now.
    (when (event-linked-event-list event)
      (for-each
       (lambda (linked-event)
         (schedule-event linked-event #:now current-environment))
       (event-list-events (event-linked-event-list event)))
      (set-event-list-events! (event-linked-event-list event) '()))
    ;;(current-simulation-event event)
    (set-simulation-environment-event!
     current-environment event)
    ;;(current-simulation-process process)
    (set-simulation-environment-process!
     current-environment process)
    (when process
      (set-process-state! process PROCESS-ACTIVE))
    (apply (event-function event)
           (event-arguments event))
    ;; If we returned here, then the process/event terminated
    ;; Change the process state to terminated
    (when process
      (set-process-state! process PROCESS-TERMINATED))))

;;; execute-continuous-events: simulation-environment x real -> void
;;; Execute the continuous events in the current environment advancing
;;; time to the given time.
(define (execute-continuous-events time-end current-environment)
  ;; (Re-)Initialize the state vector if there has been a change.
  (when (current-simulation-state-changed?)
    (continuous-initialize current-environment))
  (let* ((step (simulation-environment-step
                current-environment))
         (control (simulation-environment-control
                   current-environment))
         (evolve (simulation-environment-evolve
                  current-environment))
         (system (simulation-environment-system
                  current-environment))
         (t (box (simulation-environment-time
                  current-environment)))
         (h (box (simulation-environment-step-size
                  current-environment)))
         (y (make-vector
             (simulation-environment-dimension
              current-environment)
             0.0))
         (save-step-size (simulation-environment-step-size
                          current-environment)))
    (set-state-vector-values! y current-environment)
    (ode-evolve-reset evolve)
    (let loop ()
      (when (< (unbox t) time-end)
        (let ((terminating-events '())
              (save-y (simulation-environment-y
                       current-environment)))
          ;; Check terminating conditions
          ;;(current-simulation-y y)
          (set-simulation-environment-y!
           current-environment y)
          (mfor-each
           (lambda (event)
             (let ((process (event-process event)))
               (when (and (process-terminating-condition process)
                          ((process-terminating-condition process)))
                 (set! terminating-events
                       (cons event terminating-events)))))
           (event-list-events
            (simulation-environment-continuous-event-list
             current-environment)))
          ;;(current-simulation-y save-y)
          (set-simulation-environment-y!
           current-environment save-y)
          ;; Are there any terminating events?
          (if (not (null? terminating-events))
              ;; Yes, remove them from the continuous event list
              ;; and exit.
              (for-each
               (lambda (event)
                 (let ((process (event-process event)))
                   (event-list-remove!
                    (simulation-environment-continuous-event-list
                     current-environment)
                    event)
                   (for-each
                    (lambda (variable)
                      (set-variable-state-index! variable -1))
                    (process-continuous-variables process))
                   (schedule-event event '#:now current-environment)
                   ;;(current-simulation-state-changed? #t)
                   (set-simulation-environment-state-changed?!
                    current-environment #t)))
               terminating-events)
              ;; No, evolve the system through another time step.
              (begin
                ;; Execute the simulation monitor
                (when (simulation-environment-monitor
                       current-environment)
                  ((simulation-environment-monitor
                    current-environment)))
                (ode-evolve-apply
                 evolve control step system
                 t time-end h y)
                (set-box!
                 h (min (unbox h)
                        (simulation-environment-max-step-size
                         current-environment)))
                ;;(current-simulation-time (unbox t))
                (set-simulation-environment-time!
                 current-environment (unbox t))
                (when (simulation-environment-control
                       current-environment)
                  ;;(current-simulation-step-size (unbox h))
                  (set-simulation-environment-step-size!
                   current-environment (unbox h)))
                (set-variable-values! y current-environment)
                ;; Add process monitor execution here
                ;; Note that continuous events are always associated
                ;; with a process.
                (mfor-each
                 (lambda (event)
                   (let* ((process (event-process event))
                          (monitor (process-monitor process)))
                     (when monitor
                       (monitor process))))
                 (event-list-events
                  (simulation-environment-continuous-event-list
                   current-environment)))
                (loop))))))
    ;;(current-simulation-step-size save-step-size)
    (set-simulation-environment-step-size!
     current-environment save-step-size)))

(define (continuous-eval t y dydt params)
  (let* ((current-environment (car params))
         (save-time (simulation-environment-time
                     current-environment))
         (save-y (simulation-environment-y
                  current-environment))
         (save-dydt (simulation-environment-dydt
                     current-environment)))
    ;; Set the state variables in the simulation environment
    ;;(current-simulation-time t)
    (set-simulation-environment-time!
     current-environment t)
    ;;(current-simulation-y y)
    (set-simulation-environment-y!
     current-environment y)
    ;;(current-simulation-dydt dydt)
    (set-simulation-environment-dydt!
     current-environment dydt)
    ;; Execute the process differentiation functions
    (mfor-each
     (lambda (event)
       (let ((process (event-process event)))
         ((process-differentiation-function process))))
     (event-list-events (simulation-environment-continuous-event-list
                         current-environment)))
    ;; Resore the saved fields
    ;;(current-simulation-time save-time)
    (set-simulation-environment-time!
     current-environment save-time)
    ;;(current-simulation-y save-y)
    (set-simulation-environment-y!
     current-environment save-y)
    ;;(current-simulation-dydt save-dydt)
    (set-simulation-environment-dydt!
     current-environment save-dydt)))

;;; continuous-initialize
(define (continuous-initialize current-environment)
  (let ((dimension 0))
    ;; Determine the dimension of the system of equations and allocate
    ;; the indices.
    (mfor-each
     (lambda (event)
       (for-each
        (lambda (variable)
          (let ((index dimension))
            (set-variable-state-index! variable index)
            (set! dimension (+ 1 dimension))))
        (process-continuous-variables (event-process event))))
     (event-list-events
      (simulation-environment-continuous-event-list
       current-environment)))
    ;;(current-simulation-dimension dimension)
    (set-simulation-environment-dimension!
     current-environment dimension)
    ;; Create ode-system object
    ;;(current-simulation-system
    ;; (make-ode-system continuous-eval #f dimension '()))
    (set-simulation-environment-system!
     current-environment
     (make-ode-system continuous-eval #f dimension (list current-environment)))
    ;; Create ode-step object
    ;;(current-simulation-step
    ;; (make-ode-step (current-simulation-step-type) dimension))
    (set-simulation-environment-step!
     current-environment
     (make-ode-step (simulation-environment-step-type
                     current-environment)
                    dimension))
    ;; The ode-control object should already exist
    ;; Create ode-evolve object
    ;;(current-simulation-evolve (make-ode-evolve dimension))
    (set-simulation-environment-evolve!
     current-environment
     (make-ode-evolve dimension))
    ;; Reset the state-changed? flag
    ;;(current-simulation-state-changed? #f)
    (set-simulation-environment-state-changed?!
     current-environment #f)))
     
;;; set-variable-values!: simulation-environment x (vectorof real) -> void
(define (set-variable-values! state-vector current-environment)
  (mfor-each
   (lambda (event)
     (for-each
      (lambda (variable)
        (set-variable-value!
         variable (vector-ref state-vector
                              (variable-state-index variable))))
      (process-continuous-variables (event-process event))))
   (event-list-events
    (simulation-environment-continuous-event-list
     current-environment))))

;;; set-state-vector-values!: simulation-environment x (vectorof real) -> void
(define (set-state-vector-values! state-vector current-environment)
  (mfor-each
   (lambda (event)
     (for-each
      (lambda (variable)
        (vector-set! state-vector
                     (variable-state-index variable)
                     (variable-field-ref variable 1)))
      (process-continuous-variables (event-process event))))
   (event-list-events
    (simulation-environment-continuous-event-list
     current-environment))))

;;; stop-simulation:
(define stop-simulation
  (case-lambda
    ((return-value)
     ((current-simulation-loop-exit) return-value))
    (()
     ((current-simulation-loop-exit)))))

;;; wait/work: real -> void
;;; Delays the execution of the process or event for the specified
;;; length of time.
(define (wait/work delay)
  (let/cc continue
    ;; Reuse the current event
    (let ((event (current-simulation-event)))
      (set-event-time! event (+ (current-simulation-time) delay))
      (set-event-function! event continue)
      (set-event-arguments! event '())
      ;; Change the process state to waiting/working
      (when (event-process event)
        (set-process-state! (event-process event) PROCESS-WAITING/WORKING))
      (schedule-event event))
    ;; Done with this event
    (current-simulation-event #f)
    ;; Return to main loop
    ((current-simulation-loop-next))))

(define wait wait/work)
(define work wait/work)

;;; work/continuously
(define-syntax work/continuously
  (syntax-rules (until)
    ((work/continuously
      until condition
      body ...)
     (let/cc continue
       (let ((process (current-simulation-process))
             (event (current-simulation-event)))
         ;; Set up process for working continuously
         (set-process-terminating-condition!
          process
          (lambda () condition))
         (set-process-differentiation-function!
          process
          (lambda () body ...))
         (set-process-state! process PROCESS-WORKING-CONTINUOUSLY)
         ;; Set up event
         (set-event-function! event continue)
         (set-event-arguments! event '())
         (schedule-event event '#:continuous))
       ;; Done with this event
       (current-simulation-event #f)
       ;; Mark state as being changed
       (current-simulation-state-changed? #t)
       ;; Return to the main loop
       ((current-simulation-loop-next))))
    ((work/continuously
      body ...)
     (work/continuously
      until #f
      body ...))))

;;; suspend-process: -> void
;;; Suspend the execution of the current process.
(define (suspend-process)
  (let/cc continue
    ;; Reuse the current event
    (let ((process (current-simulation-process))
          (event (current-simulation-event)))
      (set-event-time! event 0.0)
      (set-event-function! event continue)
      (set-event-arguments! event '())
      (set-process-state! process PROCESS-SUSPENDED))
    ;; Done with the event
    (current-simulation-event #f)
    ((current-simulation-loop-next))))

;;; interrupt-process: process -> void
;;; Interrupt the execution of a waiting process.
(define (interrupt-process process)
  (let ((event (process-event process)))
    (event-list-remove! event)
    (set-event-time! event (- (event-time event) (current-simulation-time)))
    (set-process-state! process PROCESS-INTERRUPTED)))

;;; resume-process: process -> void
;;; Resume the execution of a suspended or interrupted process.
(define (resume-process process)
  (let ((event (process-event process)))
    (schedule-event event (+ (current-simulation-time)
                             (event-time event)))))